AirspeedValidator: add new check (variance after boot) and disable data stuck check by default

This new check is inteded to trigger if there is no data variation published by
the airspeed driver for the first 10s after the first data is published.
This is to capture malfunctioning sensors/drivers that do publish a value, this
value though does not come from real sensor measurements.
Previously this was captured by the data stuck check, but it has shown that
some drivers can publish a stuck value without being actually malfunctioning
(e.g. when the airspeed is outside of their measurement range). Checking for
any data variation is the more conservative check that still catches the above
described failure case.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/airspeed_selector/AirspeedValidator.cpp

@@ -57,6 +57,7 @@ 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);
@@ -211,6 +212,36 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
 	_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT;
 }
 
+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;
+		_IAS_prev = _IAS;
+	}
+
+	if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) {
+		if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON) {
+			_variation_detected = true;
+		}
+
+		_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)
@@ -289,12 +320,13 @@ AirspeedValidator::check_load_factor(float accel_z)
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t 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
+	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
 		_time_checks_failed = timestamp;
 
-	} else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) {
-		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
+	} else if (!_data_variation_test_failed && ! _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
 		_time_checks_passed = timestamp;
 	}
 
@@ -307,13 +339,13 @@ 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_stuck_test_failed) {
+		if (both_checks_failed || single_check_fail_timeout || _data_variation_test_failed || _data_stuck_test_failed) {
 
 			_airspeed_valid = false;
 		}
 
 	} else if (_checks_clear_delay > 0.f && (timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
-		// disable the re-enabling if the clear delay is negative
+		// re-enabling is only possible if the clear delay is positive
 		_airspeed_valid = true;
 	}
 }

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -118,6 +118,7 @@ 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:
 
@@ -127,6 +128,7 @@ private:
 	bool _data_stuck_check_enabled{false};
 	bool _innovation_check_enabled{false};
 	bool _load_factor_check_enabled{false};
+	bool _data_variation_check_enabled{false};
 
 	// airspeed scale validity check
 	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
@@ -145,6 +147,12 @@ 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};
+	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
@@ -184,6 +192,7 @@ 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);

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -162,7 +162,8 @@ 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_LOAD_FACTOR_BIT = (1 << 3),
+		CHECK_TYPE_DATA_VARIATION_BIT = (1 << 4)
 	};
 
 	DEFINE_PARAMETERS(
@@ -479,6 +480,8 @@ 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);
 	}
 }
 

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -151,14 +151,15 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
  * Note that the data missing check is enabled if any of the options is set.
  *
  * @min 0
- * @max 15
+ * @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 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, 7);
+PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 21);
 
 /**
  * Enable fallback to sensor-less airspeed estimation