fix for differential pressure sensors that take longer to start publishing

src/lib/systemlib/system_params.c

@@ -242,6 +242,15 @@ PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0);
  */
 PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0);
 
+/**
+ * Number of differential pressure sensors to check being available
+ *
+ * @group System
+ * @min 0
+ * @max 4
+ */
+PARAM_DEFINE_INT32(SYS_HAS_NUM_DPRS, 0);
+
 /**
  * Enable factory calibration mode
  *

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -298,7 +298,7 @@ AirspeedModule::Run()
 
 	// do not run the airspeed selector until 2s after system boot,
 	// as data from airspeed sensor	and estimator may not be valid yet
-	if (_time_now_usec < 2_s) {
+	if (_time_now_usec < 3_s) {
 		return;
 	}
 

src/modules/logger/logged_topics.cpp

@@ -48,7 +48,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("action_request");
 	add_topic("actuator_armed");
 	add_optional_topic("actuator_controls_status_0", 300);
-	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
 	add_optional_topic("autotune_attitude_control_status", 100);
 	add_optional_topic("camera_capture");
@@ -139,6 +138,7 @@ void LoggedTopics::add_default_topics()
 
 	// multi topics
 	add_optional_topic_multi("actuator_outputs", 100, 3);
+	add_optional_topic_multi("airspeed", 1000, 3);
 	add_optional_topic_multi("airspeed_wind", 1000, 4);
 	add_optional_topic_multi("control_allocator_status", 200, 2);
 	add_optional_topic_multi("rate_ctrl_status", 200, 2);
@@ -212,7 +212,7 @@ void LoggedTopics::add_default_topics()
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
 	add_topic_multi("battery_status", 200, 2);
-	add_topic_multi("differential_pressure", 1000, 2);
+	add_topic_multi("differential_pressure", 1000, 3);
 	add_topic_multi("distance_sensor", 1000, 2);
 	add_optional_topic_multi("sensor_accel", 1000, 4);
 	add_optional_topic_multi("sensor_baro", 1000, 4);

src/modules/sensors/sensors.cpp

@@ -59,6 +59,7 @@ Sensors::Sensors(bool hil_enabled) :
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
+
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@@ -67,8 +68,14 @@ Sensors::Sensors(bool hil_enabled) :
 	_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
 
-	_airspeed_validator.set_timeout(300000);
-	_airspeed_validator.set_equal_value_threshold(100);
+	_number_of_differential_pressure_sensors = _param_sys_has_num_dprs.get();
+
+	for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
+		_airspeed_pubs[i].advertise();
+		_diff_pres_data[i].airspeed_validator.set_timeout(300000);
+		_diff_pres_data[i].airspeed_validator.set_equal_value_threshold(100);
+	}
+
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 #if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
@@ -260,106 +267,113 @@ int Sensors::parameters_update()
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 void Sensors::diff_pres_poll()
 {
-	differential_pressure_s diff_pres{};
+	vehicle_air_data_s air_data{};
+	_vehicle_air_data_sub.copy(&air_data);
 
-	if (_diff_pres_sub.update(&diff_pres)) {
+	for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
 
-		if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
-			// ignore invalid data and reset accumulated
+		// poll raw differential sensor topic of the i-th sensor
+		differential_pressure_s diff_pres{};
 
-			// reset
-			_diff_pres_timestamp_sum = 0;
-			_diff_pres_pressure_sum = 0;
-			_diff_pres_temperature_sum = 0;
-			_baro_pressure_sum = 0;
-			_diff_pres_count = 0;
-			return;
-		}
+		if (_differential_pressure_subs[i].update(&diff_pres)) {
 
-		vehicle_air_data_s air_data{};
-		_vehicle_air_data_sub.copy(&air_data);
+			if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
+				// ignore invalid data and reset accumulated
 
-		float air_temperature_celsius = NAN;
-
-		// assume anything outside of a (generous) operating range of -40C to 125C is invalid
-		if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
-
-			air_temperature_celsius = diff_pres.temperature;
-
-		} else {
-			// differential pressure temperature invalid, check barometer
-			if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
-			    && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
-
-				// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
-				air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
-			}
-		}
-
-		// push raw data into validator
-		float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
-		_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
-
-		// accumulate average for publication
-		_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
-		_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
-		_diff_pres_temperature_sum += air_temperature_celsius;
-		_baro_pressure_sum += air_data.baro_pressure_pa;
-		_diff_pres_count++;
-
-		if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) {
-
-			// average data and apply calibration offset (SENS_DPRES_OFF)
-			const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
-			const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa;
-			const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count;
-			const float temperature = _diff_pres_temperature_sum / _diff_pres_count;
-
-			// reset
-			_diff_pres_timestamp_sum = 0;
-			_diff_pres_pressure_sum = 0;
-			_diff_pres_temperature_sum = 0;
-			_baro_pressure_sum = 0;
-			_diff_pres_count = 0;
-
-
-			enum AIRSPEED_SENSOR_MODEL smodel;
-
-			switch ((diff_pres.device_id >> 16) & 0xFF) {
-			case DRV_DIFF_PRESS_DEVTYPE_SDP31:
-
-			// fallthrough
-			case DRV_DIFF_PRESS_DEVTYPE_SDP32:
-
-			// fallthrough
-			case DRV_DIFF_PRESS_DEVTYPE_SDP33:
-				smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
-				break;
-
-			default:
-				smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
-				break;
+				// reset
+				_diff_pres_data[i].diff_pres_timestamp_sum = 0;
+				_diff_pres_data[i].diff_pres_pressure_sum = 0;
+				_diff_pres_data[i].diff_pres_temperature_sum = 0;
+				_diff_pres_data[i].baro_pressure_sum = 0;
+				_diff_pres_data[i].diff_pres_count = 0;
+				return;
 			}
 
-			float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
-						       smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
-						       differential_pressure_pa, baro_pressure_pa, temperature);
+			float air_temperature_celsius = NAN;
 
-			// assume that CAS = IAS as we don't have an CAS-scale here
-			float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
+			// assume anything outside of a (generous) operating range of -40C to 125C is invalid
+			if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
 
-			if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
+				air_temperature_celsius = diff_pres.temperature;
 
-				airspeed_s airspeed;
-				airspeed.timestamp_sample = timestamp_sample;
-				airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
-				airspeed.true_airspeed_m_s = true_airspeed_m_s;
-				airspeed.air_temperature_celsius = temperature;
-				airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
-				airspeed.timestamp = hrt_absolute_time();
-				_airspeed_pub.publish(airspeed);
+			} else {
+				// differential pressure temperature invalid, check barometer
+				if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
+				    && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
 
-				_airspeed_last_publish = airspeed.timestamp;
+					// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
+					air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
+				}
+			}
+
+			// push raw data into validator
+			float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
+			_diff_pres_data[i].airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count,
+					100); // TODO: real priority?
+
+			// accumulate average for publication
+			_diff_pres_data[i].diff_pres_timestamp_sum += diff_pres.timestamp_sample;
+			_diff_pres_data[i].diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
+			_diff_pres_data[i].diff_pres_temperature_sum += air_temperature_celsius;
+			_diff_pres_data[i].baro_pressure_sum += air_data.baro_pressure_pa;
+			_diff_pres_data[i].diff_pres_count++;
+
+			if ((_diff_pres_data[i].diff_pres_count > 0) && hrt_elapsed_time(&_diff_pres_data[i].airspeed_last_publish) >= 50_ms) {
+
+				// average data and apply calibration offset (SENS_DPRES_OFF)
+				const uint64_t timestamp_sample = _diff_pres_data[i].diff_pres_timestamp_sum / _diff_pres_data[i].diff_pres_count;
+				const float differential_pressure_pa = _diff_pres_data[i].diff_pres_pressure_sum / _diff_pres_data[i].diff_pres_count -
+								       _parameters.diff_pres_offset_pa;
+				const float baro_pressure_pa = _diff_pres_data[i].baro_pressure_sum / _diff_pres_data[i].diff_pres_count;
+				const float temperature = _diff_pres_data[i].diff_pres_temperature_sum / _diff_pres_data[i].diff_pres_count;
+
+				// reset
+				_diff_pres_data[i].diff_pres_timestamp_sum = 0;
+				_diff_pres_data[i].diff_pres_pressure_sum = 0;
+				_diff_pres_data[i].diff_pres_temperature_sum = 0;
+				_diff_pres_data[i].baro_pressure_sum = 0;
+				_diff_pres_data[i].diff_pres_count = 0;
+
+
+				enum AIRSPEED_SENSOR_MODEL smodel;
+
+				switch ((diff_pres.device_id >> 16) & 0xFF) {
+				case DRV_DIFF_PRESS_DEVTYPE_SDP31:
+
+				// fallthrough
+				case DRV_DIFF_PRESS_DEVTYPE_SDP32:
+
+				// fallthrough
+				case DRV_DIFF_PRESS_DEVTYPE_SDP33:
+					smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
+					break;
+
+				default:
+					smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
+					break;
+				}
+
+				float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
+							       smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
+							       differential_pressure_pa, baro_pressure_pa, temperature);
+
+				// assume that CAS = IAS as we don't have an CAS-scale here
+				float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
+
+				if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
+
+					airspeed_s airspeed;
+					airspeed.timestamp_sample = timestamp_sample;
+					airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
+					airspeed.true_airspeed_m_s = true_airspeed_m_s;
+					airspeed.air_temperature_celsius = temperature;
+					airspeed.confidence = _diff_pres_data[i].airspeed_validator.confidence(hrt_absolute_time());
+					airspeed.timestamp = hrt_absolute_time();
+
+					_airspeed_pubs[i].publish(airspeed);
+
+					_diff_pres_data[i].airspeed_last_publish = airspeed.timestamp;
+				}
 			}
 		}
 	}
@@ -708,9 +722,16 @@ int Sensors::print_status()
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
+
 	PX4_INFO_RAW("\n");
 	PX4_INFO_RAW("Airspeed status:\n");
-	_airspeed_validator.print();
+
+	for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
+		PX4_INFO_RAW("Instance #%i:\n", i);
+		_diff_pres_data[i].airspeed_validator.print();
+		PX4_INFO_RAW("\n");
+	}
+
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)

src/modules/sensors/sensors.hpp

@@ -49,6 +49,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include "voted_sensors_update.h"
 #include "vehicle_imu/VehicleIMU.hpp"
 
@@ -179,21 +180,22 @@ private:
 	 */
 	void adc_poll();
 
-	uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)};
+	uORB::SubscriptionMultiArray < differential_pressure_s, MAX_SENSOR_COUNT - 1 > _differential_pressure_subs{ORB_ID::differential_pressure};
+	int _number_of_differential_pressure_sensors{0};
+
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
-	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
+	uORB::PublicationMulti<airspeed_s> _airspeed_pubs[MAX_SENSOR_COUNT - 1] {ORB_ID(airspeed), ORB_ID(airspeed), ORB_ID(airspeed)};
 
-	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
-
-	float _diff_pres_pressure_sum{0.f};
-	float _diff_pres_temperature_sum{0.f};
-	float _baro_pressure_sum{0.f};
-
-	int _diff_pres_count{0};
-
-	uint64_t _airspeed_last_publish{0};
-	uint64_t _diff_pres_timestamp_sum{0};
+	struct DifferentialPressureData {
+		DataValidator airspeed_validator;
+		float diff_pres_pressure_sum;
+		float diff_pres_temperature_sum;
+		int diff_pres_count;
+		uint64_t airspeed_last_publish;
+		uint64_t diff_pres_timestamp_sum;
+		float baro_pressure_sum;
+	} _diff_pres_data[MAX_SENSOR_COUNT - 1] {};
 
 # ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 	uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
@@ -261,6 +263,9 @@ private:
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
+		(ParamInt<px4::params::SYS_HAS_NUM_DPRS>) _param_sys_has_num_dprs,
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
 };