diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 71db6b9c4f..3358340b7e 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -59,7 +59,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat check_airspeed_data_stuck(input_data.timestamp); check_load_factor(input_data.accel_z); check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, - input_data.ground_velocity); + input_data.ground_velocity, input_data.lpos_valid); update_airspeed_valid_status(input_data.timestamp); } @@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI) + float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid) { // Check normalised innovation levels with requirement for continuous data and use of hysteresis // to prevent false triggering. @@ -228,7 +228,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _time_last_tas_pass = time_now; _apsd_innov_integ_state = 0.f; - } else if (estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.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; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 69985fc448..c0b6db0377 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -185,7 +185,7 @@ private: void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI); + float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid); void check_load_factor(float accel_z); void update_airspeed_valid_status(const uint64_t timestamp); void reset();