diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a9c329c8a7..6f484f3195 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -408,11 +408,11 @@ 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) - const 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) - const 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) + const unsigned reset_timeout_max{7'000'000}; ///< 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) + const unsigned no_aid_timeout_max{1'000'000}; ///< 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) const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion 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) + int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) // static barometer pressure position error coefficient along body axes float static_pressure_coef_xp{0.0f}; // (-) @@ -447,7 +447,6 @@ struct parameters { float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) - const unsigned EKFGSF_reset_count_limit{3}; ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value }; union fault_status_u { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index f2674aea52..b4dbd3b6be 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -612,10 +612,3 @@ void Ekf::controlAuxVelFusion() } } } - -bool Ekf::hasHorizontalAidingTimedOut() const -{ - return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); -} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 081e1e5ba7..6e921fe235 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -875,7 +875,6 @@ private: // control fusion of GPS observations void controlGpsFusion(); bool shouldResetGpsFusion() const; - bool hasHorizontalAidingTimedOut() const; bool isYawFailure() const; void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); @@ -1060,9 +1059,6 @@ private: HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; - int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) - uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate - void runYawEKFGSF(); // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index db39044439..a320d13c3a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1434,9 +1434,6 @@ bool Ekf::resetYawToEKFGSF() _inhibit_ev_yaw_use = true; } - _ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us; - _ekfgsf_yaw_reset_count++; - return true; } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 982b61cb05..66db64579b 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -112,25 +112,24 @@ void Ekf::controlGpsFusion() fuseVelocity(_aid_src_gnss_vel); fuseHorizontalPosition(_aid_src_gnss_pos); - if (shouldResetGpsFusion()) { - const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000); + bool do_vel_pos_reset = shouldResetGpsFusion(); - /* A reset is not performed when getting GPS back after a significant period of no data - * because the timeout could have been caused by bad GPS. - * The total number of resets allowed per boot cycle is limited. + if (isYawFailure() + && _control_status.flags.in_air + && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { + /* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously + * fails while the difference between the yaw emergency estimator and the yas estimate is large. + * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was + * present before flight to prevent triggering due to GPS glitches or other sensor errors. */ - if (isYawFailure() - && _control_status.flags.in_air - && !was_gps_signal_lost - && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit - && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) { - // The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time - // to improve its estimate if the previous reset was not successful. - if (resetYawToEKFGSF()) { - ECL_WARN("GPS emergency yaw reset"); - } + if (resetYawToEKFGSF()) { + ECL_WARN("GPS emergency yaw reset"); + do_vel_pos_reset = true; } + } + if (do_vel_pos_reset) { ECL_WARN("GPS fusion timeout, resetting velocity and position"); _information_events.flags.reset_vel_to_gps = true; _information_events.flags.reset_pos_to_gps = true; @@ -221,30 +220,20 @@ bool Ekf::shouldResetGpsFusion() const /* We are relying on aiding to constrain drift so after a specified time * with no aiding we need to do something */ - const bool is_reset_required = hasHorizontalAidingTimedOut() + const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); + + const bool is_reset_required = has_horizontal_aiding_timed_out || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); - /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of - * navigation casued by a bad yaw estimate. - - * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity - * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff, - * different test criteria are used that take longer to trigger and reduce false positives. A reset is - * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches - * or other sensor errors. - */ - const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air - && isRecent(_time_last_on_ground_us, 30000000) - && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) - && (_time_last_hor_vel_fuse > _time_last_on_ground_us); - const bool is_inflight_nav_failure = _control_status.flags.in_air && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) && (_time_last_hor_vel_fuse > _time_last_on_ground_us) && (_time_last_hor_pos_fuse > _time_last_on_ground_us); - return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure); + return (is_reset_required || is_inflight_nav_failure); } bool Ekf::isYawFailure() const diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b12ce5db08..f20efc7a66 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -209,7 +209,6 @@ void Ekf::runInAirYawReset() resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); _information_events.flags.yaw_aligned_to_imu_gps = true; - _ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us; // if world magnetic model (inclination, declination, strength) available then use it to reset mag states if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {