zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Fix timeout parameter documentation and name 

The parameter used to control the maximum dead reckoning time had 'gps' in the parameter name which was confusing because it was used for all measurement types capable of constraining horizontal velocity error growth. The parameter variable has been renamed and the documentation for it improved.
The parameter used to control the maximum time since fusing a measurement before the measurement is considered to be not contributing to aiding had misleading documentation which has been updated.
master
Paul Riseborough 7 years ago committed by Paul Riseborough
parent
2354c30d81
commit
342c3ab202
2 changed files with 11 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      EKF/common.h
  2. 18
      EKF/control.cpp

6
EKF/common.h
Unescape View File

@ -324,10 +324,8 @@ struct parameters { @@ -324,10 +324,8 @@ struct parameters {
float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
unsigned no_gps_timeout_max{7000000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement (uSec)
unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of measurements that constrain horizontal velocity drift before
///< the EKF will report that it has been inertial dead-reckoning for too long and needs to revert to a
/// mode that doesn't privide horizontal vbelocity and position estimates (uSec)
unsigned reset_timeout_max{7000000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)

18
EKF/control.cpp
Unescape View File

@ -310,7 +310,7 @@ void Ekf::controlExternalVisionFusion() @@ -310,7 +310,7 @@ void Ekf::controlExternalVisionFusion()
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// check if we have been deadreckoning too long
if (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) {
if (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it
if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) {
resetVelocity();
@ -343,7 +343,7 @@ void Ekf::controlExternalVisionFusion() @@ -343,7 +343,7 @@ void Ekf::controlExternalVisionFusion()
} else if (_control_status.flags.ev_pos
&& (_time_last_imu >= _time_last_ext_vision)
&& (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) {
&& (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false;
@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion() @@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion()
_control_status.flags.opt_flow = false;
_time_last_of_fuse = 0;
} else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.no_gps_timeout_max) {
} else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.reset_timeout_max) {
_control_status.flags.opt_flow = false;
}
@ -465,7 +465,7 @@ void Ekf::controlOpticalFlowFusion() @@ -465,7 +465,7 @@ void Ekf::controlOpticalFlowFusion()
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos) {
bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max;
bool do_reset = _time_last_imu - _time_last_of_fuse > _params.reset_timeout_max;
if (do_reset) {
resetVelocity();
@ -572,13 +572,13 @@ void Ekf::controlGpsFusion() @@ -572,13 +572,13 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max)
&& (_time_last_imu - _time_last_delpos_fuse > _params.no_gps_timeout_max)
&& (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max)
&& (_time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max);
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_delpos_fuse > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_vel_fuse > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_of_fuse > _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max);
do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.reset_timeout_max);
if (do_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle

Write Preview
Loading…
Cancel
Save