Airspeed selector follow-up

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