AirspeedSelector: add bitmask to enable checks seperately in ASPD_DO_CHECKS

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

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);

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°)
 

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<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
 		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _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);
 	}
 }
 

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