diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 15de4ed41b..4c64f615be 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -197,7 +197,9 @@ AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_c void AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) { - // data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) + // Data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) when in fixed-wing flight. + // Only consider fixed-wing flight as some airspeed sensors have a very low resolution around 0m/s and + // can output the exact same value for several seconds then. if (!_data_stuck_check_enabled) { _data_stuck_test_failed = false; @@ -209,7 +211,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) _IAS_prev = _IAS; } - _data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT; + _data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT && _in_fixed_wing_flight; } void @@ -226,15 +228,15 @@ AirspeedValidator::check_airspeed_data_variation(uint64_t time_now) if (_time_first_data == 0) { // init _time_first_data = time_now; - _IAS_prev = _IAS; + _data_variation_check_ias_prev = _IAS; } if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) { - if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON) { + if (fabsf(_IAS - _data_variation_check_ias_prev) > FLT_EPSILON) { _variation_detected = true; } - _IAS_prev = _IAS; + _data_variation_check_ias_prev = _IAS; } else { // only update the test_failed flag once the timeout since first data is over diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 4a782508eb..f15f0ac27f 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -151,6 +151,7 @@ private: bool _data_variation_test_failed{false}; static constexpr uint64_t VARIATION_CHECK_TIMEOUT{10_s}; ///< timeout to check for data variation after first data is received hrt_abstime _time_first_data{0}; + float _data_variation_check_ias_prev{0.f}; bool _variation_detected{false}; // states of innovation check diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index ebc1b59f3d..d40bef557d 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -153,13 +153,13 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * @min 0 * @max 31 * @bit 0 Only data missing check (triggers if more than 1s no data) - * @bit 1 Data stuck (triggers if data is exactly constant for 2s) + * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode) * @bit 2 Innovation check (see ASPD_FS_INNOV) * @bit 3 Load factor check (triggers if measurement is below stall speed) * @bit 4 Check for data variation in first 10s after sensor connection * @group Airspeed Validator */ -PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 21); +PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 23); /** * Enable fallback to sensor-less airspeed estimation