fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 02:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix timeout parameter documentation and name

Browse Source 
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.
This commit is contained in:
Paul Riseborough 2018-08-28 07:00:53 +10:00 committed by Paul Riseborough
parent 2354c30d81
commit 342c3ab202
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/common.h
View File

@ -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
View File

@ -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()
} 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()
_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()
&& !_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()
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