diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/lib/airspeed_validator/AirspeedValidator.cpp index 0d2e1d4140..79e4cf5832 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.cpp +++ b/src/lib/airspeed_validator/AirspeedValidator.cpp @@ -46,12 +46,6 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat // get indicated airspeed from input data (raw airspeed) _IAS = input_data.airspeed_indicated_raw; - // to be able to detect missing data, save timestamp (used in data_missing check) - if (input_data.airspeed_timestamp != _previous_airspeed_timestamp && input_data.airspeed_timestamp > 0) { - _time_last_airspeed = input_data.timestamp; - _previous_airspeed_timestamp = input_data.airspeed_timestamp; - } - update_CAS_scale(); update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius); update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx, @@ -63,6 +57,13 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat update_airspeed_valid_status(input_data.timestamp); } +void +AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp) +{ + _airspeed_valid = false; + _time_checks_failed = timestamp; +} + void AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid, float lpos_vx, float lpos_vy, @@ -148,16 +149,14 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _innovations_check_failed = false; _time_last_tas_pass = time_now; _time_last_tas_fail = 0; - _airspeed_valid = true; - _time_last_aspd_innov_check = time_now; } else { const float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz if (dt_s < 1.0f) { // compute the ratio of innovation to gate size - 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()); + 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()); if (tas_test_ratio <= _tas_innov_threshold) { // record pass and reset integrator used to trigger @@ -185,8 +184,9 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } } - _time_last_aspd_innov_check = time_now; } + + _time_last_aspd_innov_check = time_now; } @@ -212,9 +212,6 @@ AirspeedValidator::check_load_factor(float accel_z) void AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) { - // Declare data stopped if not received for longer than 1 second - _data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s; - if (_innovations_check_failed || _load_factor_check_failed) { // either innovation or load factor check failed, so record timestamp _time_checks_failed = timestamp; @@ -233,12 +230,12 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) // a timeout period is applied. const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s; - if (_data_stopped_failed || both_checks_failed || single_check_fail_timeout) { + if (both_checks_failed || single_check_fail_timeout) { _airspeed_valid = false; } - } else if (!_data_stopped_failed && (timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) { + } else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) { _airspeed_valid = true; } } diff --git a/src/lib/airspeed_validator/AirspeedValidator.hpp b/src/lib/airspeed_validator/AirspeedValidator.hpp index a013f70aa9..aa661cac7d 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.hpp +++ b/src/lib/airspeed_validator/AirspeedValidator.hpp @@ -78,6 +78,8 @@ public: void update_airspeed_validator(const airspeed_validator_update_data &input_data); + void reset_airspeed_to_invalid(const uint64_t timestamp); + float get_IAS() { return _IAS; } float get_CAS() { return _CAS; } float get_TAS() { return _TAS; } @@ -125,12 +127,6 @@ private: float _TAS{0.0f}; ///< true airspeed in m/s float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS - uint64_t _time_last_airspeed{0}; ///< time last airspeed measurement was received (uSec) - - // states of data stopped check - bool _data_stopped_failed{false}; ///< data_stopp check has detected failure - hrt_abstime _previous_airspeed_timestamp{0}; ///< timestamp from previous measurement input, to check validity of measurement - // states of innovation check float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio) bool _innovations_check_failed{false}; ///< true when airspeed innovations have failed consistency checks @@ -149,7 +145,7 @@ private: float _load_factor_ratio{0.5f}; ///< ratio of maximum load factor predicted by stall speed to measured load factor // states of airspeed valid declaration - bool _airspeed_valid{false}; ///< airspeed valid (pitot or groundspeed-windspeed) + bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed) int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec) int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec) uint64_t _time_checks_passed{0}; ///< time the checks have last passed (uSec) @@ -167,5 +163,6 @@ private: float estimator_status_mag_test_ratio); void check_load_factor(float accel_z); void update_airspeed_valid_status(const uint64_t timestamp); + void reset(); }; diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index cfb573f60c..9628becbd5 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -146,6 +146,8 @@ private: bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */ + hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {}; + perf_counter_t _perf_elapsed{}; DEFINE_PARAMETERS( @@ -352,6 +354,13 @@ AirspeedModule::Run() // push input data into airspeed validator _airspeed_validator[i].update_airspeed_validator(input_data); + + _time_last_airspeed_update[i] = _time_now_usec; + + } else if (_time_last_airspeed_update[i] - _time_now_usec > 1_s) { + // declare airspeed invalid if more then 1s since last raw airspeed update + _airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec); + } } }