From 5d186ee83cfebc8242a0b1a70f95dfc3817e07bb Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Jun 2024 09:20:59 +0200 Subject: [PATCH 1/4] AirspeedValidator: remove additional one second of hysteresis for triggering innovation checks - this check already uses an integrator and so adding more delay just makes log analysis more difficult and does not really add any value Signed-off-by: RomanBapst --- src/modules/airspeed_selector/AirspeedValidator.cpp | 8 +------- src/modules/airspeed_selector/AirspeedValidator.hpp | 2 -- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 4707774eb0c3..8bec2bc544a1 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -225,13 +225,11 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ 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; _aspd_innov_integ_state = 0.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; _aspd_innov_integ_state = 0.f; } else { @@ -249,11 +247,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _aspd_innov_integ_state = 0.f; } - if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) { - _time_last_tas_pass = time_now; - } - - _innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY; + _innovations_check_failed = _tas_innov_integ_threshold > 0.0f && _aspd_innov_integ_state > _tas_innov_integ_threshold; } _time_last_aspd_innov_check = time_now; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index ba8f5c1a305a..8476f522d1e2 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -150,9 +150,7 @@ class AirspeedValidator float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure uint64_t _time_last_aspd_innov_check{0}; ///< time airspeed innovation was last checked (uSec) - uint64_t _time_last_tas_pass{0}; ///< last time innovation checks passed float _aspd_innov_integ_state{0.0f}; ///< integral of excess normalised airspeed innovation (sec) - static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s}; ///< time required for innovation levels to pass or fail (usec) uint64_t _time_wind_estimator_initialized{0}; ///< time last time wind estimator was initialized (uSec) // states of load factor check From a886ee9269a34fcacdb0710ce48c6aa9bbb8463b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Jun 2024 12:46:35 +0200 Subject: [PATCH 2/4] removed unnecessary conditions Signed-off-by: RomanBapst --- src/modules/airspeed_selector/AirspeedValidator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 8bec2bc544a1..7809b3964024 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -247,7 +247,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _aspd_innov_integ_state = 0.f; } - _innovations_check_failed = _tas_innov_integ_threshold > 0.0f && _aspd_innov_integ_state > _tas_innov_integ_threshold; + _innovations_check_failed = _aspd_innov_integ_state > _tas_innov_integ_threshold; } _time_last_aspd_innov_check = time_now; From b6aa0b1060f0f30ee265754099484aa36970efd3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 11 Jun 2024 08:09:26 +0200 Subject: [PATCH 3/4] AirspeedValidator: only disable innov checks if ASPD_FS_INTEG is negative Signed-off-by: Silvan Fuhrer --- src/modules/airspeed_selector/AirspeedValidator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 7809b3964024..713461a8427b 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -223,7 +223,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ // 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) { + || _tas_innov_integ_threshold < -FLT_EPSILON) { _innovations_check_failed = false; _aspd_innov_integ_state = 0.f; From c399e01da1029bc8f7bc3fdeffc725f20a605cbb Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 11 Jun 2024 13:20:39 +0200 Subject: [PATCH 4/4] get rid of unnecessary check on innovation threshold parameter Signed-off-by: RomanBapst --- src/modules/airspeed_selector/AirspeedValidator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 713461a8427b..63343b402322 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -222,8 +222,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } // 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 < -FLT_EPSILON) { + if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s) { _innovations_check_failed = false; _aspd_innov_integ_state = 0.f;