Airspeed Selector: enable airspeed_validated in control modules (#12887)

* FW attitude controller, FW position controller and VTOL attitude controller subscribe to airspeed_validated topic
* add possibility to switch off the airspeed valid checks
* remove airspeed valid checks from commander
* clean up of VTOL transition logic
* Airspeed Selector: remove dynamic allocation of airspeed validators (depending on number of connected sensors) but do it statically for the maximum number allowed. Check for number of connected sensors not only during start up, but always when vehicle is disarmed.
* Airspeed Selector: change work queue from lp to att_pos_ctrl as this module is safety-critical
* add airspeed selector to px4_fmu-v2 defaults

boards/px4/fmu-v2/default.cmake

@@ -85,7 +85,7 @@ px4_add_board(
 		sensors
 		vmount
 		vtol_att_control
-		#airspeed_selector
+		airspeed_selector
 
 	SYSTEMCMDS
 		bl_update

msg/airspeed_validated.msg

@@ -4,7 +4,9 @@ float32 indicated_airspeed_m_s		# indicated airspeed in m/s (IAS), set to NAN if
 float32 equivalent_airspeed_m_s     # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid
 float32 true_airspeed_m_s			# true filtered airspeed in m/s (TAS), set to NAN if invalid
 
+float32 equivalent_ground_minus_wind_m_s # EAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
 float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
-float32 indicated_ground_minus_wind_m_s # IAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
 
-int8 selected_airspeed_index # 0-2: airspeed sensor index, -1: groundspeed-windspeed, -2: airspeed invalid
+bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid.
+
+int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid

msg/vehicle_status.msg

@@ -94,11 +94,3 @@ uint8 failure_detector_status			# Bitmask containing FailureDetector status [0,
 uint32 onboard_control_sensors_present
 uint32 onboard_control_sensors_enabled
 uint32 onboard_control_sensors_health
-
-# airspeed fault and airspeed use status
-float32 arspd_check_level	# integrated airspeed inconsistency as checked against the COM_TAS_FS_INTEG parameter
-bool aspd_check_failing		# true when airspeed consistency checks are failing
-bool aspd_fault_declared	# true when an airspeed fault has been declared
-bool aspd_use_inhibit		# true if switching to a non-airspeed control mode has been requested
-bool aspd_fail_rtl		# true if airspeed failure invoked RTL has been requested
-float32 load_factor_ratio	# ratio of measured to aerodynamic load factor limit. Greater than 1 indicates airspeed low error condition.

src/lib/airspeed_validator/AirspeedValidator.cpp

@@ -109,10 +109,23 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 	return wind_est;
 }
 
+void
+AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
+{
+	_airspeed_scale_manual = airspeed_scale_manual;
+	_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
+}
+
 void
 AirspeedValidator::update_EAS_scale()
 {
-	_EAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.75f, 1.25f);
+	if (_wind_estimator.is_estimate_valid()) {
+		_EAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);
+
+	} else {
+		_EAS_scale = _airspeed_scale_manual;
+	}
+
 }
 
 void
@@ -186,7 +199,7 @@ AirspeedValidator::check_load_factor(float accel_z)
 {
 	// Check if the airpeed reading is lower than physically possible given the load factor
 
-	bool bad_number_fail = false; // disable this for now
+	const bool bad_number_fail = false; // disable this for now
 
 	if (_in_fixed_wing_flight) {
 
@@ -214,7 +227,7 @@ void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
 
-	bool bad_number_fail = false; // disable this for now
+	const bool bad_number_fail = false; // disable this for now
 
 	// Check if sensor data is missing - assume a minimum 5Hz data rate.
 	const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms;

src/lib/airspeed_validator/AirspeedValidator.hpp

@@ -98,8 +98,9 @@ class AirspeedValidator
 	}
 
 	void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
-	void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) {_wind_estimator_scale_estimation_on = scale_estimation_on;}
-	void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual);} // scale is inverted inside the wind estimator
+	void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) { _wind_estimator_scale_estimation_on = scale_estimation_on;}
+
+	void set_airspeed_scale_manual(float airspeed_scale_manual);
 
 	// setters for failure detection tuning parameters
 	void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }
@@ -115,6 +116,7 @@ class AirspeedValidator
 
 	// wind estimator parameter
 	bool _wind_estimator_scale_estimation_on{false};	///< online scale estimation (IAS-->CAS/EAS) is on
+	float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
 
 	// general states
 	bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks