AirspeedSelector: FW attitude, FW position controller and VTOL attitu…

…de controller: subscribe to airspeed_validated topic

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

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -94,6 +94,13 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 
 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) */
@@ -123,11 +130,11 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 	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)*/
+	int32_t _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) */
 
-	int _valid_airspeed_index{-1}; /**< index of currently chosen (valid) airspeed sensor */
-	int _prev_airspeed_index{-1}; /**< previously chosen airspeed sensor index */
+	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) */
@@ -147,9 +154,9 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 		(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_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 */
@@ -164,7 +171,6 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 	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) */
 
 };
 
@@ -220,24 +226,38 @@ AirspeedModule::Run()
 	 * instances (N = number of airspeed sensors detected)
 	 */
 	if (!_initialized) {
-		for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
-			if (orb_exists(ORB_ID(airspeed), i) != 0) {
-				continue;
+		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;
+				}
+
+				_number_of_airspeed_sensors = i + 1;
 			}
 
-			_number_of_airspeed_sensors = i + 1;
+		} else {
+			_number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
 		}
 
+
 		_airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors];
+		_valid_airspeed_index =
+			_param_airspeed_primary_index.get(); // set index to the one provided in the parameter ASPD_PRIMARY
 
 		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
 			}
 		}
 
+		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);
+			mavlink_and_console_log_info(&_mavlink_log_pub, "Primary airspeed index bigger than number connected sensors.");
+		}
+
+		_prev_airspeed_index = _valid_airspeed_index;
+
 		_initialized = true;
 	}
 
@@ -349,7 +369,7 @@ 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) {
+		if (_valid_airspeed_index > 0) {
 			_airspeed_validator[0].set_airspeed_scale(
 				-1.0f);  // set it to a negative value to start estimation inside wind estimator
 
@@ -362,14 +382,14 @@ void AirspeedModule::update_params()
 		/* 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(_param_west_airspeed_scale.get());
 
 			mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
-						     (double)_airspeed_validator[_valid_airspeed_index].get_EAS_scale());
+						     (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.");
@@ -393,9 +413,6 @@ void AirspeedModule::poll_topics()
 
 	_vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s)
 					&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
-
-
-
 }
 
 void AirspeedModule::update_wind_estimator_sideslip()
@@ -445,37 +462,30 @@ 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).
-		 Checks are enabled with ASPD_DO_CHECKS, switch between -1 and -2 is not affected*/
-	if ((_number_of_airspeed_sensors > 0 && _prev_airspeed_index < 0) ||
-	    (_prev_airspeed_index >= 0 && !_airspeed_validator[_prev_airspeed_index].get_airspeed_valid()
-	     && _param_airspeed_checks_on.get()) ||
-	    _prev_airspeed_index == -2) {
-
-		find_new_valid_index = true;
-	}
-
-	if (find_new_valid_index) {
-		_valid_airspeed_index = -1;
+	/* Find new valid index if primary sensor selected is real sensor, checks are enabled and airspeed currently invalid(but we have sensors). */
+	if (_param_airspeed_primary_index.get() > 0 && _param_airspeed_checks_on.get() &&
+	    (_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
+	     || !_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid())) {
+		_valid_airspeed_index = 0;
 
 		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;
+	/* No valid airspeed sensor available, or Primary set to 0 or -1. Thus set */
+	if (_param_airspeed_primary_index.get() >= airspeed_index::GROUND_MINUS_WIND_INDEX
+	    && _valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) {
+
+		if (_param_airspeed_fallback.get() && _vehicle_local_position_valid) {
+			_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 */
@@ -500,11 +510,11 @@ void AirspeedModule::select_airspeed_and_publish()
 	airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
 
 	switch (_valid_airspeed_index) {
-	case -2:
+	case airspeed_index::DISABLED_INDEX:
 		break;
 
-	case -1:
-		if (_param_airspeed_fallback_on.get()) {
+	case airspeed_index::GROUND_MINUS_WIND_INDEX:
+		if (_param_airspeed_fallback.get()) {
 			/* 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;
@@ -517,9 +527,9 @@ void AirspeedModule::select_airspeed_and_publish()
 		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.airspeed_sensor_measurement_valid = true;

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -92,58 +92,74 @@ PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
  */
 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.
+ * 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 for airspeed estimation (take groundspeed minus windspeed)
- *
- * If set to true then the airspeed is estimated using groundspeed minus windspeed if no valid airspeed sensor present.
+ * 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 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.
+ * 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 Commander
- * @category Developer
+ * @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 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.
+ * 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 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.
  *
  * @unit s
  * @max 30.0
- * @group Commander
- * @category Developer
+ * @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. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
+ * Delay before stopping use of airspeed sensor if checks indicate sensor is bad.
  *
  * @unit s
- * @group Commander
- * @category Developer
+ * @group Airspeed Validator
  * @min 1
  * @max 10
  */
@@ -152,11 +168,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. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
+ * Delay before switching back to using airspeed sensor if checks indicate sensor is good.
  *
  * @unit s
- * @group Commander
- * @category Developer
+ * @group Airspeed Validator
  * @min 10
  * @max 1000
  */
@@ -165,10 +180,9 @@ 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. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter.
+ * 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 Commander
- * @category Developer
+ * @group Airspeed Validator
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(ASPD_STALL, 10.0f);

src/modules/commander/Commander.cpp

@@ -610,7 +610,7 @@ Commander::Commander() :
 
 Commander::~Commander()
 {
-	// delete[] _airspeed_fault_type;
+
 }
 
 bool

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -404,17 +404,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 = _parameters.airspeed_trim;
 
 	if (!_parameters.airspeed_disabled && 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

@@ -52,7 +52,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>
@@ -108,7 +108,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	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 */