diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index b435bdc0fd..9953155f1d 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -198,6 +198,11 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) { // data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) + if (!_data_stuck_check_enabled) { + _data_stuck_test_failed = false; + return; + } + if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON || _time_last_unequal_data == 0) { _time_last_unequal_data = time_now; _IAS_prev = _IAS; @@ -217,8 +222,8 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _time_wind_estimator_initialized = time_now; } - // 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) < 5_s + // 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) { _innovations_check_failed = false; _time_last_tas_pass = time_now; @@ -261,6 +266,12 @@ AirspeedValidator::check_load_factor(float accel_z) { // Check if the airspeed reading is lower than physically possible given the load factor + if (!_load_factor_check_enabled) { + _load_factor_ratio = 0.5f; + _load_factor_check_failed = false; + return; + } + if (_in_fixed_wing_flight) { float max_lift_ratio = fmaxf(_CAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f); diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index a07561817e..0389262100 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -116,10 +116,19 @@ public: void set_CAS_scale_validated(float scale) { _CAS_scale_validated = scale; } void set_scale_init(float scale) { _wind_estimator.set_scale_init(scale); } + void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; } + void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; } + void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; } + private: WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator + // Check enabling + bool _data_stuck_check_enabled{false}; + bool _innovation_check_enabled{false}; + bool _load_factor_check_enabled{false}; + // airspeed scale validity check static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f9eecc8505..09716115a0 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -157,6 +157,13 @@ private: float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */ + enum CheckTypeBits { + CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0), + CHECK_TYPE_DATA_STUCK_BIT = (1 << 1), + CHECK_TYPE_INNOVATION_BIT = (1 << 2), + CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3) + }; + DEFINE_PARAMETERS( (ParamFloat) _param_west_w_p_noise, (ParamFloat) _param_west_sc_p_noise, @@ -460,6 +467,13 @@ void AirspeedModule::update_params() _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get()); _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get()); _airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get()); + + _airspeed_validator[i].set_enable_data_stuck_check(_param_airspeed_checks_on.get() & + CheckTypeBits::CHECK_TYPE_DATA_STUCK_BIT); + _airspeed_validator[i].set_enable_innovation_check(_param_airspeed_checks_on.get() & + CheckTypeBits::CHECK_TYPE_INNOVATION_BIT); + _airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() & + CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT); } } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 14fadf07b3..f88d721dcf 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -145,12 +145,18 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); /** * Enable checks on airspeed sensors * - * If set to true then the data comming from the airspeed sensors is checked for validity. Only applied if ASPD_PRIMARY > 0. + * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0. + * Note that the data missing check is enabled if any of the options is set. * - * @boolean + * @min 0 + * @max 15 + * @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 2 Innovation check (see ASPD_FS_INNOV) + * @bit 3 Load factor check (triggers if measurement is below stall speed) * @group Airspeed Validator */ -PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1); +PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7); /** * Enable fallback to sensor-less airspeed estimation