diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 4707774eb0c3..63343b402322 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -222,16 +222,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } // reset states if check is disabled, we are not flying or wind estimator was just initialized/reset - if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s - || _tas_innov_integ_threshold <= 0.f) { + if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s) { _innovations_check_failed = false; - _time_last_tas_pass = time_now; _aspd_innov_integ_state = 0.f; } else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) { //nav velocity data is likely not good //don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good - _time_last_tas_pass = time_now; _aspd_innov_integ_state = 0.f; } else { @@ -249,11 +246,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _aspd_innov_integ_state = 0.f; } - if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) { - _time_last_tas_pass = time_now; - } - - _innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY; + _innovations_check_failed = _aspd_innov_integ_state > _tas_innov_integ_threshold; } _time_last_aspd_innov_check = time_now; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 4ce2949623a5..ed7008a5c621 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -150,9 +150,7 @@ class AirspeedValidator float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure uint64_t _time_last_aspd_innov_check{0}; ///< time airspeed innovation was last checked (uSec) - uint64_t _time_last_tas_pass{0}; ///< last time innovation checks passed float _aspd_innov_integ_state{0.0f}; ///< integral of excess normalised airspeed innovation (sec) - static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s}; ///< time required for innovation levels to pass or fail (usec) uint64_t _time_wind_estimator_initialized{0}; ///< time last time wind estimator was initialized (uSec) // states of load factor check