From 4513652592652232a13fa3ee221b1bc701436871 Mon Sep 17 00:00:00 2001
From: fury1895 <yannick@auterion.com>
Date: Tue, 20 Feb 2024 17:16:22 +0100
Subject: [PATCH] fix for differential pressure sensors that take longer to
 start publishing

---
 src/lib/systemlib/system_params.c             |  9 +++++++++
 .../airspeed_selector_main.cpp                |  2 +-
 src/modules/logger/logged_topics.cpp          |  2 +-
 src/modules/sensors/sensors.cpp               | 20 +++----------------
 src/modules/sensors/sensors.hpp               |  5 +++--
 5 files changed, 17 insertions(+), 21 deletions(-)

diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c
index 1417cd37b4..d0f4e8d1f2 100644
--- a/src/lib/systemlib/system_params.c
+++ b/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
  *
diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp
index 4877ee94b6..a3c34869bf 100644
--- a/src/modules/airspeed_selector/airspeed_selector_main.cpp
+++ b/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;
 	}
 
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 89e6abb4ac..c2df7775f3 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -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);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 264a6217ca..b7545c4218 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -60,7 +60,6 @@ Sensors::Sensors(bool hil_enabled) :
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 
-	InitializeVehicleDifferentialPressure();
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@@ -69,7 +68,10 @@ 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");
 
+	_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);
 	}
@@ -444,22 +446,6 @@ void Sensors::InitializeVehicleAirData()
 }
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
-#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
-void Sensors::InitializeVehicleDifferentialPressure()
-{
-	// check for connected differential pressure sensors
-	for (int i = 0; i < _differential_pressure_subs.size(); i++) {
-		if (!_differential_pressure_subs[i].advertised()) {
-			break;
-		}
-
-		_airspeed_pubs[i].advertise();
-
-		_number_of_differential_pressure_sensors = i + 1;
-	}
-}
-#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
-
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 void Sensors::InitializeVehicleGPSPosition()
 {
diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp
index e6936783fd..9c1cb62dd6 100644
--- a/src/modules/sensors/sensors.hpp
+++ b/src/modules/sensors/sensors.hpp
@@ -125,8 +125,6 @@ private:
 
 	void		InitializeVehicleAirData();
 
-	void		InitializeVehicleDifferentialPressure();
-
 	void		InitializeVehicleGPSPosition();
 
 	void		InitializeVehicleIMU();
@@ -265,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
 	)
 };