diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 275c9be931..dd72dd75e9 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -57,7 +57,6 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); - check_airspeed_data_variation(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); @@ -213,36 +212,6 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) _data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT && _in_fixed_wing_flight; } -void -AirspeedValidator::check_airspeed_data_variation(uint64_t time_now) -{ - // Data variation after boot test: trigger when IAS is not changing for within VARIATION_CHECK_TIMEOUT - // after the first data is received. - - if (!_data_variation_check_enabled) { - _data_variation_test_failed = false; - return; - } - - if (_time_first_data == 0) { - // init - _time_first_data = time_now; - _data_variation_check_ias_prev = _IAS; - } - - if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) { - if (fabsf(_IAS - _data_variation_check_ias_prev) > FLT_EPSILON) { - _variation_detected = true; - } - - _data_variation_check_ias_prev = _IAS; - - } else { - // only update the test_failed flag once the timeout since first data is over - _data_variation_test_failed = !_variation_detected; - } -} - 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) @@ -321,13 +290,13 @@ AirspeedValidator::check_load_factor(float accel_z) void AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) { - if (_data_variation_test_failed || _data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) { - // at least one check (variation, data stuck, innovation or load factor) failed, so record timestamp + if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) { + // at least one check (data stuck, innovation or load factor) failed, so record timestamp _time_checks_failed = timestamp; - } else if (!_data_variation_test_failed && ! _data_stuck_test_failed && !_innovations_check_failed + } else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) { - // all checks(variation, data stuck, innovation and load factor) must pass to declare airspeed good + // all checks(data stuck, innovation and load factor) must pass to declare airspeed good _time_checks_passed = timestamp; } @@ -340,7 +309,7 @@ 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 (both_checks_failed || single_check_fail_timeout || _data_variation_test_failed || _data_stuck_test_failed) { + if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) { _airspeed_valid = false; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index f15f0ac27f..970b2980d8 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -118,7 +118,6 @@ public: 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; } - void set_enable_data_variation_check(bool enable) { _data_variation_check_enabled = enable; } private: @@ -147,13 +146,6 @@ private: float _IAS_prev{0.f}; static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat - // variation check - 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 bool _innovations_check_failed{false}; ///< true when airspeed innovations have failed consistency checks float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure @@ -193,7 +185,6 @@ private: void update_CAS_scale_applied(); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void check_airspeed_data_stuck(uint64_t timestamp); - void check_airspeed_data_variation(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); void check_load_factor(float accel_z); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 80c1535fe9..9cccb1247e 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -162,8 +162,7 @@ private: 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), - CHECK_TYPE_DATA_VARIATION_BIT = (1 << 4) + CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3) }; DEFINE_PARAMETERS( @@ -480,8 +479,6 @@ void AirspeedModule::update_params() CheckTypeBits::CHECK_TYPE_INNOVATION_BIT); _airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() & CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT); - _airspeed_validator[i].set_enable_data_variation_check(_param_airspeed_checks_on.get() & - CheckTypeBits::CHECK_TYPE_DATA_VARIATION_BIT); } } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index d40bef557d..b216506e8c 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -151,15 +151,14 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * Note that the data missing check is enabled if any of the options is set. * * @min 0 - * @max 31 + * @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 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, 23); +PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7); /** * Enable fallback to sensor-less airspeed estimation