diff --git a/EKF/common.h b/EKF/common.h index eef4102415..54badc3f10 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -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) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9b37f151d6..700e118a28 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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