diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 75a67b6017..eae10b8778 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -396,6 +396,7 @@ struct parameters { 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 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) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fd099a641e..8c61068b65 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -86,9 +86,6 @@ void Ekf::controlFusionModes() } if (_baro_buffer) { - // check for intermittent data - _baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); - const uint64_t baro_time_prev = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); @@ -771,18 +768,21 @@ void Ekf::controlBaroHeightFusion() _baro_b_est.predict(_dt_ekf_avg); + // check for intermittent data + const bool baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); + if (_baro_data_ready) { _baro_lpf.update(_baro_sample_delayed.hgt); updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt); - const bool continuing_conditions_passing = !_baro_hgt_faulty && !_baro_hgt_intermittent; + const bool continuing_conditions_passing = !_baro_hgt_faulty && !baro_hgt_intermittent; const bool starting_conditions_passing = continuing_conditions_passing; if (_control_status.flags.baro_hgt) { if (continuing_conditions_passing) { fuseBaroHgt(_aid_src_baro_hgt); - const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, (uint64_t)5e6); + const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, _params.hgt_fusion_timeout_max); if (isHeightResetRequired()) { // All height sources are failing @@ -804,7 +804,7 @@ void Ekf::controlBaroHeightFusion() } } - } else if (_control_status.flags.baro_hgt && _baro_hgt_intermittent) { + } else if (_control_status.flags.baro_hgt && baro_hgt_intermittent) { // No baro data anymore. Stop until it comes back. stopBaroHgtFusion(); } @@ -827,7 +827,7 @@ void Ekf::controlGnssHeightFusion() if (continuing_conditions_passing) { /* fuseGpsHgt(); */ // Done in fuseGpsPos - const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.reset_timeout_max); + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.hgt_fusion_timeout_max); if (isHeightResetRequired()) { // All height sources are failing @@ -862,6 +862,8 @@ void Ekf::controlRangeHeightFusion() _rng_hgt_b_est.predict(_dt_ekf_avg); + const bool rng_intermittent = !isRecent(_time_last_range, 2 * RNG_MAX_INTERVAL); + // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air @@ -878,7 +880,8 @@ void Ekf::controlRangeHeightFusion() const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable(); - const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid); + const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && !rng_intermittent + && ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid); const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData(); @@ -886,7 +889,7 @@ void Ekf::controlRangeHeightFusion() if (continuing_conditions_passing) { fuseRngHgt(_aid_src_rng_hgt); - const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6); + const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max); if (isHeightResetRequired()) { // All height sources are failing @@ -910,7 +913,7 @@ void Ekf::controlRangeHeightFusion() } } - } else if (_control_status.flags.rng_hgt && isTimedOut(_time_last_range, (uint64_t)5e6)) { + } else if (_control_status.flags.rng_hgt && rng_intermittent) { stopRngHgtFusion(); } } @@ -924,10 +927,11 @@ void Ekf::controlEvHeightFusion() _ev_hgt_b_est.predict(_dt_ekf_avg); + const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); + if (_ev_data_ready) { - const bool continuing_conditions_passing = true; - const bool starting_conditions_passing = continuing_conditions_passing - && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); + const bool continuing_conditions_passing = !ev_intermittent; + const bool starting_conditions_passing = continuing_conditions_passing; if (_control_status.flags.ev_hgt) { if (continuing_conditions_passing) { @@ -949,7 +953,7 @@ void Ekf::controlEvHeightFusion() } } - } else if (_control_status.flags.ev_hgt && isTimedOut(_time_last_ext_vision, (uint64_t)5e6)) { + } else if (_control_status.flags.ev_hgt && ev_intermittent) { stopEvHgtFusion(); } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 60aed247b1..b61efae04c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -622,7 +622,6 @@ private: // height sensor status bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags - bool _baro_hgt_intermittent{true}; ///< true if data into the buffer is intermittent bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent // imu fault status diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9155a0772d..34c0b7fb9a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -219,7 +219,7 @@ bool Ekf::isHeightResetRequired() const const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long - const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); + const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max); return (continuous_bad_accel_hgt || hgt_fusion_timeout); }