diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/lib/airspeed_validator/AirspeedValidator.cpp index 79e4cf5832..8c2c16be27 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.cpp +++ b/src/lib/airspeed_validator/AirspeedValidator.cpp @@ -144,46 +144,38 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _time_wind_estimator_initialized = time_now; } - // reset states if we are not flying - if (!_in_fixed_wing_flight) { + // reset states if we are not flying or wind estimator was just initialized/reset + if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s) { _innovations_check_failed = false; _time_last_tas_pass = time_now; - _time_last_tas_fail = 0; } else { - const float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz + const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz - if (dt_s < 1.0f) { + if ((estimator_status_vel_test_ratio < 1.f) && (estimator_status_mag_test_ratio < 1.f)) { + // nav velocity data is likely good so airspeed innovations are able to be used // compute the ratio of innovation to gate size const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov() - / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.0f) * _wind_estimator.get_tas_innov_var()); + / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var()); - if (tas_test_ratio <= _tas_innov_threshold) { - // record pass and reset integrator used to trigger - _time_last_tas_pass = time_now; - _apsd_innov_integ_state = 0.0f; + if (tas_test_ratio > _tas_innov_threshold) { + _apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance } else { - // integrate exceedance - _apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); - } + // reset integrator used to trigger and record pass if integrator check is disabled + _apsd_innov_integ_state = 0.f; - if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) { - // nav velocity data is likely good so airspeed innovations are able to be used - if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) { - _time_last_tas_fail = time_now; + if (_tas_innov_integ_threshold <= 0.f) { + _time_last_tas_pass = time_now; } } - if (!_innovations_check_failed) { - _innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY; - - } else { - _innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100) - || (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100; + if (_tas_innov_integ_threshold > 0.f && _apsd_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; } _time_last_aspd_innov_check = time_now; diff --git a/src/lib/airspeed_validator/AirspeedValidator.hpp b/src/lib/airspeed_validator/AirspeedValidator.hpp index aa661cac7d..5a02d3bd1a 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.hpp +++ b/src/lib/airspeed_validator/AirspeedValidator.hpp @@ -134,8 +134,7 @@ private: 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 - uint64_t _time_last_tas_fail{0}; ///< last time innovation checks failed - float _apsd_innov_integ_state{0.0f}; ///< inegral of excess normalised airspeed innovation (sec) + float _apsd_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)