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 @@ public:
 	}
 
 	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 @@ private:
 
 	// 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

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -94,6 +94,13 @@ public:
 
 private:
 	static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
+	enum airspeed_index {
+		DISABLED_INDEX = -1,
+		GROUND_MINUS_WIND_INDEX,
+		FIRST_SENSOR_INDEX,
+		SECOND_SENSOR_INDEX,
+		THIRD_SENSOR_INDEX
+	};
 
 	uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)};			/**< airspeed validated topic*/
 	uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
@@ -110,7 +117,6 @@ private:
 	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 	estimator_status_s _estimator_status {};
-	parameter_update_s _parameter_update {};
 	vehicle_acceleration_s _accel {};
 	vehicle_air_data_s _vehicle_air_data {};
 	vehicle_attitude_s _vehicle_attitude {};
@@ -123,52 +129,56 @@ private:
 	wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
 
 	int _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {}; /**< raw airspeed topics subscriptions. Max 3 airspeeds sensors. */
-	int _number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in use (detected during initialization)*/
-	AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */
+	int32_t _number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in use (detected during initialization)*/
+	int32_t _prev_number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in previous loop (to detect a new added sensor)*/
+	AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
 
-	int _valid_airspeed_index{-1}; /**< index of currently chosen (valid) airspeed sensor */
-	int _prev_airspeed_index{-1}; /**< previously chosen airspeed sensor index */
+	hrt_abstime _time_now_usec{0};
+	int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
+	int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
 	bool _initialized{false}; /**< module initialized*/
 	bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
 	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
 	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
-	float _ground_minus_wind_EAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_EAS{0.0f}; /**< equivalent airspeed from groundspeed minus windspeed */
 
 	bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
 
 	perf_counter_t _perf_elapsed{};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::ARSP_W_P_NOISE>) _param_west_w_p_noise,
-		(ParamFloat<px4::params::ARSP_SC_P_NOISE>) _param_west_sc_p_noise,
-		(ParamFloat<px4::params::ARSP_TAS_NOISE>) _param_west_tas_noise,
-		(ParamFloat<px4::params::ARSP_BETA_NOISE>) _param_west_beta_noise,
-		(ParamInt<px4::params::ARSP_TAS_GATE>) _param_west_tas_gate,
-		(ParamInt<px4::params::ARSP_BETA_GATE>) _param_west_beta_gate,
-		(ParamInt<px4::params::ARSP_SCALE_EST>) _param_west_scale_estimation_on,
-		(ParamFloat<px4::params::ARSP_ARSP_SCALE>) _param_west_airspeed_scale,
-
-
-		(ParamFloat<px4::params::COM_TAS_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
-		(ParamFloat<px4::params::COM_TAS_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
-		(ParamInt<px4::params::COM_TAS_FS_T1>) _checks_fail_delay, /**< delay to declare airspeed invalid */
-		(ParamInt<px4::params::COM_TAS_FS_T2>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
-		(ParamFloat<px4::params::COM_ASPD_STALL>) _airspeed_stall /**<  stall speed*/
+		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
+		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
+		(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
+		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
+		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
+		(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
+		(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
+		(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
+		(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
+		(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
+		(ParamInt<px4::params::ASPD_FALLBACK>) _param_airspeed_fallback,
+
+		(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
+		(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
+		(ParamInt<px4::params::ASPD_FS_T1>) _checks_fail_delay, /**< delay to declare airspeed invalid */
+		(ParamInt<px4::params::ASPD_FS_T2>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
+		(ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall /**<  stall speed*/
 	)
 
-	int 		start();
+	void 		init(); 	/**< initialization of the airspeed validator instances */
+	void 		check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */
 	void		update_params(); /**< update parameters */
 	void 		poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
 	void 		update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
 	void		update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
 	void 		select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
-	void 		publish_wind_estimates(); /**< publish wind estimator states (from all wind estimators running) */
 
 };
 
 AirspeedModule::AirspeedModule():
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
 {
 	// initialise parameters
 	update_params();
@@ -182,7 +192,6 @@ AirspeedModule::~AirspeedModule()
 
 	perf_free(_perf_elapsed);
 
-	delete[] _airspeed_validator;
 }
 
 int
@@ -198,42 +207,67 @@ AirspeedModule::task_spawn(int argc, char *argv[])
 
 	_object.store(dev);
 
-	if (dev) {
-		dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
-		_task_id = task_id_is_work_queue;
-		return PX4_OK;
+	dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
+	_task_id = task_id_is_work_queue;
+	return PX4_OK;
+
+}
+void
+AirspeedModule::init()
+{
+	check_for_connected_airspeed_sensors();
+
+	/* Set the default sensor */
+	if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
+		/* constrain the index to the number of sensors connected*/
+		_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
+
+		if (_number_of_airspeed_sensors == 0) {
+			mavlink_and_console_log_info(&_mavlink_log_pub,
+						     "No airspeed sensor detected. Switch to non-airspeed mode.");
+
+		} else {
+			mavlink_and_console_log_info(&_mavlink_log_pub,
+						     "Primary airspeed index bigger than number connected sensors. Take last sensor.");
+		}
+
+	} else {
+		_valid_airspeed_index =
+			_param_airspeed_primary_index.get(); // set index to the one provided in the parameter ASPD_PRIMARY
 	}
 
-	return PX4_ERROR;
+	_prev_airspeed_index = _valid_airspeed_index; // needed to detect a switching
 }
 
 void
-AirspeedModule::Run()
+AirspeedModule::check_for_connected_airspeed_sensors()
 {
-	perf_begin(_perf_elapsed);
-
-	/* the first time we run through here, initialize N airspeedValidator
-	 * instances (N = number of airspeed sensors detected)
-	 */
-	if (!_initialized) {
+	if (_param_airspeed_primary_index.get() > 0) {
 		for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
-			if (orb_exists(ORB_ID(airspeed), i) != 0) {
-				continue;
+			if (orb_exists(ORB_ID(airspeed), i) == PX4_OK) {
+				_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
+
+			} else {
+				break;
 			}
 
 			_number_of_airspeed_sensors = i + 1;
 		}
 
-		_airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors];
+	} else {
+		_number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
+	}
+
+}
 
-		if (_number_of_airspeed_sensors > 0) {
-			for (int i = 0; i < _number_of_airspeed_sensors; i++) {
-				_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
-				_valid_airspeed_index = 0; // set index to first sensor
-				_prev_airspeed_index = 0; // set index to first sensor
-			}
-		}
 
+void
+AirspeedModule::Run()
+{
+	perf_begin(_perf_elapsed);
+
+	if (!_initialized) {
+		init(); // initialize airspeed validator instances
 		_initialized = true;
 	}
 
@@ -243,19 +277,27 @@ AirspeedModule::Run()
 		update_params();
 	}
 
+	_time_now_usec = hrt_absolute_time(); //hrt time of the current cycle
+
+	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+	/* Check for new connected airspeed sensors as long as we're disarmed */
+	if (!armed) {
+		check_for_connected_airspeed_sensors();
+	}
+
 	poll_topics();
 	update_wind_estimator_sideslip();
 	update_ground_minus_wind_airspeed();
 
-	if (_airspeed_validator != nullptr) {
+	if (_number_of_airspeed_sensors > 0) {
 
-		bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 		bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode;
 		bool in_air = !_vehicle_land_detected.landed;
 
 		/* Prepare data for airspeed_validator */
 		struct airspeed_validator_update_data input_data = {};
-		input_data.timestamp = hrt_absolute_time();
+		input_data.timestamp = _time_now_usec;
 		input_data.lpos_vx = _vehicle_local_position.vx;
 		input_data.lpos_vy = _vehicle_local_position.vy;
 		input_data.lpos_vz = _vehicle_local_position.vz;
@@ -310,7 +352,6 @@ AirspeedModule::Run()
 	}
 }
 
-
 void AirspeedModule::update_params()
 {
 	updateParams();
@@ -334,7 +375,8 @@ void AirspeedModule::update_params()
 		_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
 
 		/* Only apply manual entered airspeed scale to first airspeed measurement */
-		_airspeed_validator[0].set_airspeed_scale(_param_west_airspeed_scale.get());
+		// TODO: enable multiple airspeed sensors
+		_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
 
 		_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
 		_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
@@ -345,8 +387,8 @@ void AirspeedModule::update_params()
 
 	/* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */
 	if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index >= 0) {
-			_airspeed_validator[0].set_airspeed_scale(
+		if (_valid_airspeed_index > 0) {
+			_airspeed_validator[0].set_airspeed_scale_manual(
 				-1.0f);  // set it to a negative value to start estimation inside wind estimator
 
 		} else {
@@ -355,17 +397,17 @@ void AirspeedModule::update_params()
 			_param_west_scale_estimation_on.commit_no_notification();
 		}
 
-		/* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ARSP_ARSP_SCALE */
+		/* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ASPD_ASPD_SCALE */
 
 	} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index >= 0) {
+		if (_valid_airspeed_index > 0) {
 
-			_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index].get_EAS_scale());
+			_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
 			_param_west_airspeed_scale.commit_no_notification();
-			_airspeed_validator[_valid_airspeed_index].set_airspeed_scale(_param_west_airspeed_scale.get());
+			_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
 
-			mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ARSP_ARSP_SCALE): %0.2f",
-						     (double)_airspeed_validator[_valid_airspeed_index].get_EAS_scale());
+			mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
+						     (double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
 
 		} else {
 			mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
@@ -387,31 +429,27 @@ void AirspeedModule::poll_topics()
 	_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 
-	_vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s)
+	_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
 					&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
-
-
-
 }
 
 void AirspeedModule::update_wind_estimator_sideslip()
 {
 	bool att_valid = true; // TODO: check if attitude is valid
-	const hrt_abstime time_now_usec = hrt_absolute_time();
 
 	/* update wind and airspeed estimator */
-	_wind_estimator_sideslip.update(time_now_usec);
+	_wind_estimator_sideslip.update(_time_now_usec);
 
 	if (_vehicle_local_position_valid && att_valid) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.q);
 
 		/* sideslip fusion */
-		_wind_estimator_sideslip.fuse_beta(time_now_usec, vI, q);
+		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
 	}
 
 	/* fill message for publishing later */
-	_wind_estimate_sideslip.timestamp = time_now_usec;
+	_wind_estimate_sideslip.timestamp = _time_now_usec;
 	float wind[2];
 	_wind_estimator_sideslip.get_wind(wind);
 	_wind_estimate_sideslip.windspeed_north = wind[0];
@@ -441,71 +479,85 @@ void AirspeedModule::update_ground_minus_wind_airspeed()
 
 void AirspeedModule::select_airspeed_and_publish()
 {
-	/* airspeed index:
-	/  0: first airspeed sensor valid
-	/  1: second airspeed sensor valid
-	/ -1: airspeed sensor(s) invalid, but groundspeed-windspeed estimate valid
-	/ -2: airspeed invalid (sensors and groundspeed-windspeed estimate invalid)
-	*/
-	bool find_new_valid_index = false;
-
-	/* find new valid index if airspeed currently invalid (but we have sensors) */
-	if ((_number_of_airspeed_sensors > 0 && _prev_airspeed_index < 0) ||
-	    (_prev_airspeed_index >= 0 && !_airspeed_validator[_prev_airspeed_index].get_airspeed_valid()) ||
-	    _prev_airspeed_index == -2) {
-
-		find_new_valid_index = true;
-	}
+	/* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled or new sensor was added. */
+	bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
+			!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
+	bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
+			_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
+	bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
+
+	if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
 
-	if (find_new_valid_index) {
-		_valid_airspeed_index = -1;
+		_valid_airspeed_index = airspeed_index::DISABLED_INDEX; // set to disabled
 
+		/* Loop through all sensors and take the first valid one */
 		for (int i = 0; i < _number_of_airspeed_sensors; i++) {
 			if (_airspeed_validator[i].get_airspeed_valid()) {
-				_valid_airspeed_index = i;
+				_valid_airspeed_index = i + 1;
 				break;
 			}
 		}
 	}
 
-	if (_valid_airspeed_index < 0 && !_vehicle_local_position_valid) {
-		_valid_airspeed_index = -2;
+	/* Airspeed enabled by user (Primary set to > -1), and no valid airspeed sensor available or primary set to 0. Thus set index to ground-wind one (if position is valid), otherwise to disabled*/
+	if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
+	    (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
+	     || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+
+		/* _vehicle_local_position_valid determines if ground-wind estimate is valid */
+		/* To use ground-windspeed as airspeed source, either the primary has to be set this way or fallback be enabled */
+		if (_vehicle_local_position_valid && (_param_airspeed_fallback.get()
+						      || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+			_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
+
+		} else {
+			_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
+		}
 	}
 
 	/* publish critical message (and log) in index has changed */
-	if (_valid_airspeed_index != _prev_airspeed_index) {
+	/* Suppress log message if still on the ground and no airspeed sensor connected */
+	if (_valid_airspeed_index != _prev_airspeed_index && (_number_of_airspeed_sensors > 0
+			|| !_vehicle_land_detected.landed)) {
 		mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index,
 				     _valid_airspeed_index);
 	}
 
 	_prev_airspeed_index = _valid_airspeed_index;
+	_prev_number_of_airspeed_sensors = _number_of_airspeed_sensors;
 
 	/* fill out airspeed_validated message for publishing it */
 	airspeed_validated_s airspeed_validated = {};
-	airspeed_validated.timestamp = hrt_absolute_time();
+	airspeed_validated.timestamp = _time_now_usec;
 	airspeed_validated.true_ground_minus_wind_m_s = NAN;
-	airspeed_validated.indicated_ground_minus_wind_m_s = NAN;
+	airspeed_validated.equivalent_ground_minus_wind_m_s = NAN;
 	airspeed_validated.indicated_airspeed_m_s = NAN;
 	airspeed_validated.equivalent_airspeed_m_s = NAN;
 	airspeed_validated.true_airspeed_m_s = NAN;
-
+	airspeed_validated.airspeed_sensor_measurement_valid = false;
 	airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
 
 	switch (_valid_airspeed_index) {
-	case -2:
+	case airspeed_index::DISABLED_INDEX:
 		break;
 
-	case -1:
+	case airspeed_index::GROUND_MINUS_WIND_INDEX:
+		/* Take IAS, EAS, TAS from groundspeed-windspeed */
+		airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_EAS;
+		airspeed_validated.equivalent_airspeed_m_s = _ground_minus_wind_EAS;
+		airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
+		airspeed_validated.equivalent_ground_minus_wind_m_s = _ground_minus_wind_EAS;
 		airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
-		airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS;
+
 		break;
 
 	default:
-		airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_IAS();
-		airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_EAS();
-		airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_TAS();
+		airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
+		airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_EAS();
+		airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
+		airspeed_validated.equivalent_ground_minus_wind_m_s = _ground_minus_wind_EAS;
 		airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
-		airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS;
+		airspeed_validated.airspeed_sensor_measurement_valid = true;
 		break;
 	}
 
@@ -517,7 +569,7 @@ void AirspeedModule::select_airspeed_and_publish()
 
 	/* publish the wind estimator states from all airspeed validators */
 	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
-		wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(hrt_absolute_time());
+		wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
 		_wind_est_pub[i + 1].publish(wind_est);
 	}
 
@@ -559,7 +611,7 @@ int AirspeedModule::print_usage(const char *reason)
 		R"DESCR_STR(
 ### Description
 This module provides a single airspeed_validated topic, containing an indicated (IAS),
-equivalend (EAS), true airspeed (TAS) and the information if the estimation currently
+equivalent (EAS), true airspeed (TAS) and the information if the estimation currently
 is invalid and if based sensor readings or on groundspeed minus windspeed.
 Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
 to a valid sensor in case of failure detection. For failure detection as well as for

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -9,7 +9,7 @@
  * @unit m/s/s
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ARSP_W_P_NOISE, 0.1f);
+PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed scale process noise
@@ -21,7 +21,7 @@ PARAM_DEFINE_FLOAT(ARSP_W_P_NOISE, 0.1f);
  * @unit 1/s
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ARSP_SC_P_NOISE, 0.0001);
+PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed measurement noise
@@ -33,7 +33,7 @@ PARAM_DEFINE_FLOAT(ARSP_SC_P_NOISE, 0.0001);
  * @unit m/s
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ARSP_TAS_NOISE, 1.4);
+PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
 
 /**
  * Airspeed Selector: Wind estimator sideslip measurement noise
@@ -45,7 +45,7 @@ PARAM_DEFINE_FLOAT(ARSP_TAS_NOISE, 1.4);
  * @unit rad
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ARSP_BETA_NOISE, 0.3);
+PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
 
 /**
  * Airspeed Selector: Gate size for true airspeed fusion
@@ -57,10 +57,10 @@ PARAM_DEFINE_FLOAT(ARSP_BETA_NOISE, 0.3);
  * @unit SD
  * @group Airspeed Validator
  */
-PARAM_DEFINE_INT32(ARSP_TAS_GATE, 3);
+PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
 
 /**
- * Airspeed Selector: Gate size for true sideslip fusion
+ * Airspeed Selector: Gate size for sideslip angle fusion
  *
  * Sets the number of standard deviations used by the innovation consistency test.
  *
@@ -69,25 +69,128 @@ PARAM_DEFINE_INT32(ARSP_TAS_GATE, 3);
  * @unit SD
  * @group Airspeed Validator
  */
-PARAM_DEFINE_INT32(ARSP_BETA_GATE, 1);
+PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
 
 /**
  * Automatic airspeed scale estimation on
  *
- * Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended level (keeping altitude) while performing the estimation. Set to 1 to start estimation (best when already flying). Set to 0 to end scale estimation. The estimated scale is then saved in the ARSP_ARSP_SCALE parameter.
+ * Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended to fly level
+ * altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
+ * Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
  *
  * @boolean
  * @group Airspeed Validator
  */
-PARAM_DEFINE_INT32(ARSP_SCALE_EST, 0);
+PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
 
 /**
  * Airspeed scale (scale from IAS to CAS/EAS)
  *
- * Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1.
+ * Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
  *
  * @min 0.5
  * @max 1.5
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ARSP_ARSP_SCALE, 1.0f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
+
+/**
+ * Index or primary airspeed measurement source
+ *
+ * @value -1 Disabled
+ * @value 0 Groundspeed minus windspeed
+ * @value 1 First airspeed sensor
+ * @value 2 Second airspeed sensor
+ * @value 3 Third airspeed sensor
+ *
+ * @reboot_required true
+ * @group Airspeed Validator
+ */
+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.
+ *
+ * @reboot_required true
+ * @boolean
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 0);
+
+/**
+ * Enable fallback to secondary airspeed measurement.
+ *
+ * If ASPD_DO_CHECKS is set to true, then airspeed estimation can fallback from what specified in ASPD_PRIMARY to secondary source (other airspeed sensors, groundspeed minus windspeed).
+
+ * @value 0 To other airspeed sensor (if one valid), else disable airspeed
+ * @value 1 To other airspeed sensor (if one valid), else to ground-windspeed
+ * @boolean
+ * @reboot_required true
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_FALLBACK, 0);
+
+/**
+ * Airspeed failsafe consistency threshold (Experimental)
+ *
+ * This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
+ * smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
+ * inconsistency between predicted and measured airspeed is large enough to cause the navigation EKF to reject airspeed measurements.
+ * The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
+*
+ * @min 0.5
+ * @max 3.0
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
+
+/**
+ * Airspeed failsafe consistency delay (Experimental)
+ *
+ * This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
+ * For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
+ * rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
+ * make it more sensitive.
+ *
+ * @unit s
+ * @max 30.0
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, -1.0f);
+
+/**
+ * Airspeed failsafe stop delay (Experimental)
+ *
+ * Delay before stopping use of airspeed sensor if checks indicate sensor is bad.
+ *
+ * @unit s
+ * @group Airspeed Validator
+ * @min 1
+ * @max 10
+ */
+PARAM_DEFINE_INT32(ASPD_FS_T1, 3);
+
+/**
+ * Airspeed failsafe start delay (Experimental)
+ *
+ * Delay before switching back to using airspeed sensor if checks indicate sensor is good.
+ *
+ * @unit s
+ * @group Airspeed Validator
+ * @min 10
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(ASPD_FS_T2, 100);
+
+/**
+ * Airspeed fault detection stall airspeed. (Experimental)
+ *
+ * This is the minimum indicated airspeed at which the wing can produce 1g of lift. It is used by the airspeed sensor fault detection and failsafe calculation to detect a significant airspeed low measurement error condition and should be set based on flight test for reliable operation.
+ *
+ * @group Airspeed Validator
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(ASPD_STALL, 10.0f);

src/modules/commander/Commander.cpp

@@ -577,11 +577,6 @@ Commander::Commander() :
 	status_flags.avoidance_system_valid = false;
 }
 
-Commander::~Commander()
-{
-	delete[] _airspeed_fault_type;
-}
-
 bool
 Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
 			  uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
@@ -1710,7 +1705,6 @@ Commander::run()
 		}
 
 		estimator_check(&status_changed);
-		airspeed_use_check();
 
 		/* Update land detector */
 		if (land_detector_sub.updated()) {
@@ -4018,275 +4012,6 @@ void Commander::battery_status_check()
 	}
 }
 
-void Commander::airspeed_use_check()
-{
-	if (_airspeed_fail_action.get() < 1 || _airspeed_fail_action.get() > 4) {
-		// disabled
-		return;
-	}
-
-	_airspeed_sub.update();
-	const airspeed_s &airspeed = _airspeed_sub.get();
-
-	vehicle_acceleration_s accel{};
-	_vehicle_acceleration_sub.copy(&accel);
-
-	bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
-
-	// assume airspeed sensor is good before starting FW flight
-	bool valid_flight_condition = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) &&
-				      is_fixed_wing && !land_detector.landed;
-	bool fault_declared = false;
-	bool fault_cleared = false;
-	bool bad_number_fail = !PX4_ISFINITE(airspeed.indicated_airspeed_m_s) || !PX4_ISFINITE(airspeed.true_airspeed_m_s);
-
-	if (!valid_flight_condition) {
-
-		_tas_check_fail = false;
-		_time_last_tas_pass = hrt_absolute_time();
-		_time_last_tas_fail = 0;
-
-		_tas_use_inhibit = false;
-		_time_tas_good_declared = hrt_absolute_time();
-		_time_tas_bad_declared = 0;
-
-		status.aspd_check_failing = false;
-		status.aspd_fault_declared = false;
-		status.aspd_use_inhibit = false;
-		status.aspd_fail_rtl = false;
-		status.arspd_check_level = 0.0f;
-
-		_time_last_airspeed = hrt_absolute_time();
-		_time_last_aspd_innov_check = hrt_absolute_time();
-		_load_factor_ratio = 0.5f;
-
-	} else {
-
-		// Check normalised innovation levels with requirement for continuous data and use of hysteresis
-		// to prevent false triggering.
-		float dt_s = float(1e-6 * (double)hrt_elapsed_time(&_time_last_aspd_innov_check));
-
-		if (dt_s < 1.0f) {
-			if (_estimator_status_sub.get().tas_test_ratio <= _tas_innov_threshold.get()) {
-				// record pass and reset integrator used to trigger
-				_time_last_tas_pass = hrt_absolute_time();
-				_apsd_innov_integ_state = 0.0f;
-
-			} else {
-				// integrate exceedance
-				_apsd_innov_integ_state += dt_s * (_estimator_status_sub.get().tas_test_ratio - _tas_innov_threshold.get());
-			}
-
-			status.arspd_check_level = _apsd_innov_integ_state;
-
-			if ((_estimator_status_sub.get().vel_test_ratio < 1.0f) && (_estimator_status_sub.get().mag_test_ratio < 1.0f)) {
-				// nav velocity data is likely good so airspeed innovations are able to be used
-				if ((_tas_innov_integ_threshold.get() > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold.get())) {
-					_time_last_tas_fail = hrt_absolute_time();
-				}
-			}
-
-			if (!_tas_check_fail) {
-				_tas_check_fail = (hrt_elapsed_time(&_time_last_tas_pass) > TAS_INNOV_FAIL_DELAY);
-
-			} else {
-				_tas_check_fail = (hrt_elapsed_time(&_time_last_tas_fail) < TAS_INNOV_FAIL_DELAY);
-			}
-		}
-
-		_time_last_aspd_innov_check = hrt_absolute_time();
-
-
-		// The vehicle is flying so use the status of the airspeed innovation check '_tas_check_fail' in
-		// addition to a sanity check using airspeed and load factor and a missing sensor data check.
-
-		// Check if sensor data is missing - assume a minimum 5Hz data rate.
-		const bool data_missing = (hrt_elapsed_time(&_time_last_airspeed) > 200_ms);
-
-		// Declare data stopped if not received for longer than 1 second
-		const bool data_stopped = (hrt_elapsed_time(&_time_last_airspeed) > 1_s);
-
-		_time_last_airspeed = hrt_absolute_time();
-
-		// Check if the airpeed reading is lower than physically possible given the load factor
-		bool load_factor_ratio_fail = true;
-
-		if (!bad_number_fail) {
-			float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f);
-			max_lift_ratio *= max_lift_ratio;
-
-			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel.xyz[2]) / CONSTANTS_ONE_G) / max_lift_ratio;
-			_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
-			load_factor_ratio_fail = (_load_factor_ratio > 1.1f);
-			status.load_factor_ratio = _load_factor_ratio;
-
-			// sanity check independent of stall speed and load factor calculation
-			if (airspeed.indicated_airspeed_m_s <= 0.0f) {
-				bad_number_fail = true;
-			}
-		}
-
-		//  Decide if the control loops should be using the airspeed data based on the length of time the
-		// airspeed data has been declared bad
-		if (_tas_check_fail || load_factor_ratio_fail || data_missing || bad_number_fail) {
-			// either load factor or EKF innovation or missing data test failure can declare the airspeed bad
-			_time_tas_bad_declared = hrt_absolute_time();
-			status.aspd_check_failing = true;
-
-		} else if (!_tas_check_fail && !load_factor_ratio_fail && !data_missing && !bad_number_fail) {
-			// All checks must pass to declare airspeed good
-			_time_tas_good_declared = hrt_absolute_time();
-			status.aspd_check_failing = false;
-		}
-
-		if (!_tas_use_inhibit) {
-			// A simultaneous load factor and innovaton check fail makes it more likely that a large
-			// airspeed measurement fault has developed, so a fault should be declared immediately
-			const bool both_checks_failed = (_tas_check_fail && load_factor_ratio_fail);
-
-			// Because the innovation, load factor and data missing checks are subject to short duration false positives
-			// a timeout period is applied.
-			const bool single_check_fail_timeout = (hrt_elapsed_time(&_time_tas_good_declared) > (_tas_use_stop_delay.get() * 1_s));
-
-			if (data_stopped || both_checks_failed || single_check_fail_timeout || bad_number_fail) {
-
-				_tas_use_inhibit = true;
-				fault_declared = true;
-
-				if (data_stopped || data_missing) {
-					strcpy(_airspeed_fault_type, "MISSING");
-
-				} else  {
-					strcpy(_airspeed_fault_type, "FAULTY ");
-				}
-			}
-
-		} else if (hrt_elapsed_time(&_time_tas_bad_declared) > (_tas_use_start_delay.get() * 1_s)) {
-			_tas_use_inhibit = false;
-			fault_cleared = true;
-		}
-	}
-
-	// Do actions based on value of COM_ASPD_FS_ACT parameter
-	status.aspd_fault_declared = false;
-	status.aspd_use_inhibit = false;
-	status.aspd_fail_rtl = false;
-
-	switch (_airspeed_fail_action.get()) {
-	case 4: { // log a message, warn the user, switch to non-airspeed TECS mode, switch to Return mode if not in a pilot controlled mode.
-			if (fault_declared) {
-				status.aspd_fault_declared = true;
-				status.aspd_use_inhibit = true;
-
-				if ((internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
-				    || (internal_state.main_state == commander_state_s::MAIN_STATE_ACRO)
-				    || (internal_state.main_state == commander_state_s::MAIN_STATE_STAB)
-				    || (internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL)
-				    || (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL)
-				    || (internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE)) {
-
-					// don't RTL if pilot is in control
-					mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use", _airspeed_fault_type);
-
-				} else if (hrt_elapsed_time(&_time_tas_good_declared) < (_airspeed_rtl_delay.get() * 1_s)) {
-					// Wait for timeout and issue message
-					mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use, RTL in %i sec", _airspeed_fault_type,
-							     _airspeed_rtl_delay.get());
-
-				} else if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
-						&internal_state)) {
-
-					// Issue critical message even if already in RTL
-					status.aspd_fail_rtl = true;
-
-					if (_airspeed_rtl_delay.get() == 0) {
-						mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use and returning", _airspeed_fault_type);
-
-					} else {
-						mavlink_log_critical(&mavlink_log_pub, "ASPD DATA STILL %s - returning", _airspeed_fault_type);
-					}
-
-				} else {
-					status.aspd_fail_rtl = true;
-
-					if (_airspeed_rtl_delay.get() == 0) {
-						mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use, return failed", _airspeed_fault_type);
-
-					} else {
-						mavlink_log_critical(&mavlink_log_pub, "ASPD DATA STILL %s - return failed", _airspeed_fault_type);
-					}
-				}
-
-			} else if (fault_cleared) {
-				mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD - restarting use");
-			}
-
-			// Inhibit airspeed use immediately if a bad number
-			if (bad_number_fail && !status.aspd_use_inhibit) {
-				status.aspd_use_inhibit = true;
-			}
-
-			return;
-		}
-
-	case 3: { // log a message, warn the user, switch to non-airspeed TECS mode
-			if (fault_declared) {
-				mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s  - stopping use", _airspeed_fault_type);
-				status.aspd_fault_declared = true;
-				status.aspd_use_inhibit = true;
-
-			} else if (fault_cleared) {
-				mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD - restarting use");
-			}
-
-			// Inhibit airspeed use immediately if a bad number
-			if (bad_number_fail && !status.aspd_use_inhibit) {
-				status.aspd_use_inhibit = true;
-			}
-
-			return;
-		}
-
-	case 2: { // log a message, warn the user
-			if (fault_declared) {
-				mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s", _airspeed_fault_type);
-				status.aspd_fault_declared = true;
-
-			} else if (fault_cleared) {
-				mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD");
-			}
-
-			// Inhibit airspeed use immediately if a bad number
-			if (bad_number_fail && !status.aspd_use_inhibit) {
-				status.aspd_use_inhibit = true;
-			}
-
-			return;
-		}
-
-	case 1: { // log a message
-			if (fault_declared) {
-				mavlink_log_info(&mavlink_log_pub, "ASPD DATA %s", _airspeed_fault_type);
-				status.aspd_fault_declared = true;
-
-			} else if (fault_cleared) {
-				mavlink_log_info(&mavlink_log_pub, "ASPD DATA GOOD");
-			}
-
-			// Inhibit airspeed use immediately if a bad number
-			if (bad_number_fail && !status.aspd_use_inhibit) {
-				status.aspd_use_inhibit = true;
-			}
-
-			return;
-		}
-
-	default:
-		// Do nothing
-		return;
-	}
-}
-
 void Commander::estimator_check(bool *status_changed)
 {
 	// Check if quality checking of position accuracy and consistency is to be performed

src/modules/commander/Commander.hpp

@@ -77,7 +77,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 {
 public:
 	Commander();
-	~Commander();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -133,15 +132,9 @@ private:
 		(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
 		(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
 
-		(ParamFloat<px4::params::COM_TAS_FS_INNOV>) _tas_innov_threshold,
-		(ParamFloat<px4::params::COM_TAS_FS_INTEG>) _tas_innov_integ_threshold,
-		(ParamInt<px4::params::COM_TAS_FS_T1>) _tas_use_stop_delay,
-		(ParamInt<px4::params::COM_TAS_FS_T2>) _tas_use_start_delay,
-		(ParamInt<px4::params::COM_ASPD_FS_ACT>) _airspeed_fail_action,
-		(ParamFloat<px4::params::COM_ASPD_STALL>) _airspeed_stall,
-		(ParamInt<px4::params::COM_ASPD_FS_DLY>) _airspeed_rtl_delay,
 		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
 
+
 		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
 		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
 		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
@@ -184,20 +177,6 @@ private:
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
 
-	/* class variables used to check for airspeed sensor failure */
-	bool		_tas_check_fail{false};	/**< true when airspeed innovations have failed consistency checks */
-	hrt_abstime	_time_last_tas_pass{0};		/**< last time innovation checks passed */
-	hrt_abstime	_time_last_tas_fail{0};		/**< last time innovation checks failed */
-	static constexpr hrt_abstime TAS_INNOV_FAIL_DELAY{1_s};	/**< time required for innovation levels to pass or fail (usec) */
-	bool		_tas_use_inhibit{false};	/**< true when the commander has instructed the control loops to not use airspeed data */
-	hrt_abstime	_time_tas_good_declared{0};	/**< time TAS use was started (uSec) */
-	hrt_abstime	_time_tas_bad_declared{0};	/**< time TAS use was stopped (uSec) */
-	hrt_abstime	_time_last_airspeed{0};		/**< time last airspeed measurement was received (uSec) */
-	hrt_abstime	_time_last_aspd_innov_check{0};	/**< time airspeed innovation was last checked (uSec) */
-	char		*_airspeed_fault_type = new char[7];
-	float		_load_factor_ratio{0.5f};	/**< ratio of maximum load factor predicted by stall speed to measured load factor */
-	float		_apsd_innov_integ_state{0.0f};	/**< inegral of excess normalised airspeed innovation (sec) */
-
 	bool _geofence_loiter_on{false};
 	bool _geofence_rtl_on{false};
 	bool _geofence_warning_action_on{false};
@@ -240,8 +219,6 @@ private:
 
 	void offboard_control_update(bool &status_changed);
 
-	void airspeed_use_check();
-
 	void battery_status_check();
 
 	void esc_status_check(const esc_status_s &esc_status);

src/modules/commander/commander_params.c

@@ -848,95 +848,6 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
  */
 PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100);
 
-/**
- * Airspeed failsafe consistency threshold (Experimental)
- *
- * This specifies the minimum airspeed test ratio as logged in estimator_status.tas_test_ratio required to trigger a failsafe. Larger values make the check less sensitive, smaller values make it more sensitive. Start with a value of 1.0 when tuning. When estimator_status.tas_test_ratio is > 1.0 it indicates the inconsistency between predicted and measured airspeed is large enough to cause the navigation EKF to reject airspeed measurements. The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the COM_TAS_FS_INTEG parameter. The subsequent failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
-*
- * @min 0.5
- * @max 3.0
- * @group Commander
- * @category Developer
- */
-PARAM_DEFINE_FLOAT(COM_TAS_FS_INNOV, 1.0f);
-
-/**
- * Airspeed failsafe consistency delay (Experimental)
- *
- * This sets the time integral of airspeed test ratio exceedance above COM_TAS_FS_INNOV required to trigger a failsafe. For example if COM_TAS_FS_INNOV is 100 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values make it more sensitive. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
- *
- * @unit s
- * @max 30.0
- * @group Commander
- * @category Developer
- */
-PARAM_DEFINE_FLOAT(COM_TAS_FS_INTEG, -1.0f);
-
-/**
- * Airspeed failsafe stop delay (Experimental)
- *
- * Delay before stopping use of airspeed sensor if checks indicate sensor is bad. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
- *
- * @unit s
- * @group Commander
- * @category Developer
- * @min 1
- * @max 10
- */
-PARAM_DEFINE_INT32(COM_TAS_FS_T1, 3);
-
-/**
- * Airspeed failsafe start delay (Experimental)
- *
- * Delay before switching back to using airspeed sensor if checks indicate sensor is good. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
- *
- * @unit s
- * @group Commander
- * @category Developer
- * @min 10
- * @max 1000
- */
-PARAM_DEFINE_INT32(COM_TAS_FS_T2, 100);
-
-/**
- * Airspeed fault detection stall airspeed. (Experimental)
- *
- * This is the minimum indicated airspeed at which the wing can produce 1g of lift. It is used by the airspeed sensor fault detection and failsafe calculation to detect a significant airspeed low measurement error condition and should be set based on flight test for reliable operation. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
- *
- * @group Commander
- * @category Developer
- * @unit m/s
- */
-PARAM_DEFINE_FLOAT(COM_ASPD_STALL, 10.0f);
-
-/**
- * Airspeed fault detection (Experimental)
- *
- * Failsafe action when bad airspeed measurements are detected. Ensure the COM_ASPD_STALL parameter is set correctly before use.
- *
- * @value 0 disabled
- * @value 1 log a message
- * @value 2 log a message, warn the user
- * @value 3 log a message, warn the user, switch to non-airspeed TECS mode
- * @value 4 log a message, warn the user, switch to non-airspeed TECS mode, switch to Return mode after COM_ASPD_FS_DLY seconds
- * @group Commander
- * @category Developer
- */
-PARAM_DEFINE_INT32(COM_ASPD_FS_ACT, 0);
-
-/**
- * Airspeed fault detection delay before RTL (Experimental)
- *
- * RTL delay after bad airspeed measurements are detected if COM_ASPD_FS_ACT is set to 4. Ensure the COM_ASPD_STALL parameter is set correctly before use. The failsafe start and stop delays are controlled by the COM_TAS_FS_T1 and COM_TAS_FS_T2 parameters. Additional protection against persistent airspeed sensor errors can be enabled using the COM_TAS_FS_INNOV parameter, but these addtional checks are more prone to false positives in windy conditions.
- *
- * @min 0
- * @max 300
- * @unit s
- * @group Commander
- * @category Developer
- */
-PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0);
-
 /**
  * User Flight Profile
  *

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -258,17 +258,16 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
 
 float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
 {
-	_airspeed_sub.update();
-	const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
-				    && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s)
-				    && !_vehicle_status.aspd_use_inhibit;
+	_airspeed_validated_sub.update();
+	const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().indicated_airspeed_m_s)
+				    && (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);
 
 	// if no airspeed measurement is available out best guess is to use the trim airspeed
 	float airspeed = _param_fw_airspd_trim.get();
 
 	if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
-		airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
+		airspeed = math::max(0.5f, _airspeed_validated_sub.get().indicated_airspeed_m_s);
 
 	} else {
 		// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -53,7 +53,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
@@ -110,7 +110,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
 	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
 
-	uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
+	uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
 	uORB::Publication<actuator_controls_s>		_actuators_2_pub{ORB_ID(actuator_controls_2)};		/**< actuator control group 1 setpoint (Airframe) */
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};		/**< rate setpoint publication */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -359,21 +359,21 @@ FixedwingPositionControl::airspeed_poll()
 {
 	bool airspeed_valid = _airspeed_valid;
 
-	if (!_parameters.airspeed_disabled && _airspeed_sub.update()) {
+	if (!_parameters.airspeed_disabled && _airspeed_validated_sub.update()) {
 
-		const airspeed_s &as = _airspeed_sub.get();
+		const airspeed_validated_s &airspeed_validated = _airspeed_validated_sub.get();
+		_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed
 
-		if (PX4_ISFINITE(as.indicated_airspeed_m_s)
-		    && PX4_ISFINITE(as.true_airspeed_m_s)
-		    && (as.indicated_airspeed_m_s > 0.0f)
-		    && !_vehicle_status.aspd_use_inhibit) {
+		if (PX4_ISFINITE(airspeed_validated.equivalent_airspeed_m_s)
+		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
+		    && (airspeed_validated.equivalent_airspeed_m_s > 0.0f)) {
 
 			airspeed_valid = true;
 
-			_airspeed_last_valid = as.timestamp;
-			_airspeed = as.indicated_airspeed_m_s;
+			_airspeed_last_valid = airspeed_validated.timestamp;
+			_airspeed = airspeed_validated.equivalent_airspeed_m_s;
 
-			_eas2tas = constrain(as.true_airspeed_m_s / as.indicated_airspeed_m_s, 0.9f, 2.0f);
+			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.equivalent_airspeed_m_s, 0.9f, 2.0f);
 		}
 
 	} else {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -69,7 +69,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_landing_status.h>
@@ -183,7 +183,7 @@ private:
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected */
 	vehicle_status_s		_vehicle_status {};		///< vehicle status */
 
-	SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
+	SubscriptionData<airspeed_validated_s>			_airspeed_validated_sub{ORB_ID(airspeed_validated)};
 	SubscriptionData<vehicle_acceleration_s>	_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
 	perf_counter_t	_loop_perf;				///< loop performance counter */

src/modules/vtol_att_control/standard.cpp

@@ -181,17 +181,30 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (((_params->airspeed_disabled ||
-			      _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) &&
-			     time_since_trans_start > _params->front_trans_time_min) ||
-			    can_transition_on_ground()) {
 
+			const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
+					&& !_params->airspeed_disabled;
+			const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min;
+
+			bool transition_to_fw = false;
+
+			if (minimum_trans_time_elapsed) {
+				if (airspeed_triggers_transition) {
+					transition_to_fw = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
+
+				} else {
+					transition_to_fw = true;
+				}
+			}
+
+			transition_to_fw |= can_transition_on_ground();
+
+			if (transition_to_fw) {
 				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
 
 				// don't set pusher throttle here as it's being ramped up elsewhere
 				_trans_finished_ts = hrt_absolute_time();
 			}
-
 		}
 	}
 
@@ -242,15 +255,16 @@ void Standard::update_transition_state()
 
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
-		    _airspeed->indicated_airspeed_m_s > 0.0f &&
-		    _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
+		    PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s > 0.0f &&
+		    _airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend &&
 		    time_since_trans_start > _params->front_trans_time_min) {
 
-			mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
+			mc_weight = 1.0f - fabsf(_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
-		} else if (_params->airspeed_disabled) {
+		} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) {
 			mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -130,11 +130,24 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				bool airspeed_condition_satisfied = _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed;
-				airspeed_condition_satisfied |= _params->airspeed_disabled;
 
-				// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 mode
-				if ((airspeed_condition_satisfied && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
+				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
+						&& !_params->airspeed_disabled;
+
+				bool transition_to_fw = false;
+
+				if (pitch <= PITCH_TRANSITION_FRONT_P1) {
+					if (airspeed_triggers_transition) {
+						transition_to_fw = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
+
+					} else {
+						transition_to_fw = true;
+					}
+				}
+
+				transition_to_fw |= can_transition_on_ground();
+
+				if (transition_to_fw) {
 					_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
 				}
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -137,20 +137,25 @@ void Tiltrotor::update_vtol_state()
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
-				// allow switch if we are not armed for the sake of bench testing
-				bool transition_to_p2 = can_transition_on_ground();
 
 				float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
-				// check if we have reached airspeed to switch to fw mode
-				transition_to_p2 |= !_params->airspeed_disabled &&
-						    _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed &&
-						    time_since_trans_start > _params->front_trans_time_min;
+				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
+						&& !_params->airspeed_disabled;
 
-				// check if airspeed is invalid and transition by time
-				transition_to_p2 |= _params->airspeed_disabled &&
-						    _tilt_control >= _params_tiltrotor.tilt_transition &&
-						    time_since_trans_start > _params->front_trans_time_openloop;
+				bool transition_to_p2 = false;
+
+				if (time_since_trans_start > _params->front_trans_time_min) {
+					if (airspeed_triggers_transition) {
+						transition_to_p2 = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
+
+					} else {
+						transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
+								   time_since_trans_start > _params->front_trans_time_openloop;;
+					}
+				}
+
+				transition_to_p2 |= can_transition_on_ground();
 
 				if (transition_to_p2) {
 					_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
@@ -244,18 +249,20 @@ void Tiltrotor::update_transition_state()
 		_mc_yaw_weight = 1.0f;
 
 		// reduce MC controls once the plane has picked up speed
-		if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
+		if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
 			_mc_yaw_weight = 0.0f;
 		}
 
-		if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend) {
-			_mc_roll_weight = 1.0f - (_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
+		if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend) {
+			_mc_roll_weight = 1.0f - (_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
 					  (_params->transition_airspeed - _params->airspeed_blend);
 		}
 
 		// without airspeed do timed weight changes
-		if (_params->airspeed_disabled
-		    && time_since_trans_start > _params->front_trans_time_min) {
+		if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) &&
+		    time_since_trans_start > _params->front_trans_time_min) {
 			_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
 					  (_params->front_trans_time_openloop - _params->front_trans_time_min);
 			_mc_yaw_weight = _mc_roll_weight;

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -345,7 +345,7 @@ VtolAttitudeControl::Run()
 		_local_pos_sub.update(&_local_pos);
 		_local_pos_sp_sub.update(&_local_pos_sp);
 		_pos_sp_triplet_sub.update(&_pos_sp_triplet);
-		_airspeed_sub.update(&_airspeed);
+		_airspeed_validated_sub.update(&_airspeed_validated);
 		_tecs_status_sub.update(&_tecs_status);
 		_land_detected_sub.update(&_land_detected);
 		vehicle_cmd_poll();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -65,7 +65,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -116,7 +116,7 @@ public:
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
 	struct actuator_controls_s 			*get_actuators_out0() {return &_actuators_out_0;}
 	struct actuator_controls_s 			*get_actuators_out1() {return &_actuators_out_1;}
-	struct airspeed_s 				*get_airspeed() {return &_airspeed;}
+	struct airspeed_validated_s 				*get_airspeed() {return &_airspeed_validated;}
 	struct position_setpoint_triplet_s		*get_pos_sp_triplet() {return &_pos_sp_triplet;}
 	struct tecs_status_s 				*get_tecs_status() {return &_tecs_status;}
 	struct vehicle_attitude_s 			*get_att() {return &_v_att;}
@@ -136,7 +136,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
 	uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)};
 
-	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};			// airspeed subscription
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};			// airspeed subscription
 	uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};			// setpoint subscription
@@ -166,7 +166,7 @@ private:
 	actuator_controls_s			_actuators_out_0{};	//actuator controls going to the mc mixer
 	actuator_controls_s			_actuators_out_1{};	//actuator controls going to the fw mixer (used for elevons)
 
-	airspeed_s 				_airspeed{};			// airspeed
+	airspeed_validated_s 				_airspeed_validated{};			// airspeed
 	manual_control_setpoint_s		_manual_control_sp{}; //manual control setpoint
 	position_setpoint_triplet_s		_pos_sp_triplet{};
 	tecs_status_s				_tecs_status{};

src/modules/vtol_att_control/vtol_type.cpp

@@ -63,7 +63,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_actuators_fw_in = _attc->get_actuators_fw_in();
 	_local_pos = _attc->get_local_pos();
 	_local_pos_sp = _attc->get_local_pos_sp();
-	_airspeed = _attc->get_airspeed();
+	_airspeed_validated = _attc->get_airspeed();
 	_tecs_status = _attc->get_tecs_status();
 	_land_detected = _attc->get_land_detected();
 	_params = _attc->get_params();

src/modules/vtol_att_control/vtol_type.h

@@ -187,7 +187,7 @@ protected:
 	struct actuator_controls_s			*_actuators_fw_in;			//actuator controls from fw_att_control
 	struct vehicle_local_position_s			*_local_pos;
 	struct vehicle_local_position_setpoint_s	*_local_pos_sp;
-	struct airspeed_s 				*_airspeed;					// airspeed
+	struct airspeed_validated_s 				*_airspeed_validated;					// airspeed
 	struct tecs_status_s				*_tecs_status;
 	struct vehicle_land_detected_s			*_land_detected;