delete drv_airspeed.h (all IOCTLs), airspeed driver lib

- differential pressure now processed downstream like other sensor data (accel/gyro/mag/etc)
 - rename msg differential_pressure -> sensor_differential_pressure
 - add device id and timestamp_sample to all messages
 - calibration performend directly on raw data and SENS_DPRES_OFF parameter updated
 - remove Airspeed base class from existing differential pressure drivers
 - name differential pressure drivers consistently (no _airspeed suffix)

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -126,21 +126,11 @@ then
 	then
 		if param compare CBRK_AIRSPD_CHK 0
 		then
-			sdp3x_airspeed start -X -q
+			sdp3x start -X -q
-			sdp3x_airspeed start -X -a 0x22 -q
+			sdp3x start -X -a 0x22 -q
 
-			# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
+			ms4525 start -X -q
-			# detected as MS5525 because the chip manufacturer was so
+			ms5525 start -X -q
-			# clever to assign the same I2C address and skip a WHO_AM_I
-			# register.
-			if [ $BOARD_FMUV3 = 21 ]
-			then
-				ms5525_airspeed start -X -b 2 -q
-			else
-				ms5525_airspeed start -X -q
-			fi
-
-			ms4525_airspeed start -X -q
 
 			ets_airspeed start -X -q
 		fi
@@ -150,5 +140,3 @@ fi
 ###############################################################################
 #                            End Optional drivers                             #
 ###############################################################################
-
-sensors start

ROMFS/px4fmu_common/init.d/rcS

@@ -322,11 +322,14 @@ else
 	# Sensors System (start before Commander so Preflight checks are properly run).
 	# Commander needs to be this early for in-air-restarts.
 	#
+	sensors start
+
 	if param greater SYS_HITL 0
 	then
 		set OUTPUT_MODE hil
-		sensors start -h
+
 		commander start -h
+
 		# disable GPS
 		param set GPS_1_CONFIG 0
 
@@ -371,7 +374,6 @@ else
 		set AUX_MODE pwm4
 	fi
 
-
 	# Check if ATS is enabled
 	if param compare FD_EXT_ATS_EN 1
 	then

ROMFS/px4fmu_test/init.d/rc.sensors

@@ -16,11 +16,11 @@ if board_adc start
 then
 fi
 
-if sdp3x_airspeed start -X
+if sdp3x start -X
 then
 fi
 
-if ms5525_airspeed start -X
+if ms5525 start -X
 then
 fi
 

msg/CMakeLists.txt

@@ -51,7 +51,6 @@ set(msg_files
 	commander_state.msg
 	control_allocator_status.msg
 	cpuload.msg
-	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
 	ekf_gps_drift.msg
@@ -127,6 +126,7 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
+	sensor_differential_pressure.msg
 	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg

msg/airspeed_validated.msg

@@ -1,4 +1,7 @@
 uint64 timestamp                                # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id                                # unique device ID for the sensor that does not change between power cycles
 
 float32 indicated_airspeed_m_s			# indicated airspeed in m/s (IAS), set to NAN if invalid
 float32 calibrated_airspeed_m_s     		# calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid

msg/differential_pressure.msg

@@ -1,6 +0,0 @@
-uint64 timestamp				# time since system start (microseconds)
-uint64 error_count				# Number of errors detected by driver
-float32 differential_pressure_raw_pa		# Raw differential pressure reading (may be negative)
-float32 differential_pressure_filtered_pa	# Low pass filtered differential pressure reading
-float32 temperature				# Temperature provided by sensor, -1000.0f if unknown
-uint32 device_id				# unique device ID for the sensor that does not change between power cycles

msg/sensor_differential_pressure.msg

@@ -0,0 +1,10 @@
+uint64 timestamp                     # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id                     # unique device ID for the sensor that does not change between power cycles
+
+float32 differential_pressure_pa     # differential pressure reading
+
+float32 temperature                  # temperature in degrees Celsius
+
+uint32 error_count

msg/tools/uorb_rtps_message_ids.yaml

@@ -33,7 +33,7 @@ rtps:
   - msg: debug_vect
     id: 15
     receive: true
-  - msg: differential_pressure
+  - msg: sensor_differential_pressure
     id: 16
   - msg: distance_sensor
     id: 17

src/drivers/differential_pressure/ets/CMakeLists.txt

@@ -31,11 +31,11 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__ets_airspeed
+	MODULE drivers__differential_pressure__ets_airspeed
 	MAIN ets_airspeed
 	COMPILE_FLAGS
 	SRCS
 		ets_airspeed.cpp
 	DEPENDS
-		drivers__airspeed
+		px4_work_queue
 	)

src/drivers/differential_pressure/ets/ets_airspeed.cpp

@@ -37,13 +37,14 @@
  *
  * Driver for the Eagle Tree Airspeed V3 connected via I2C.
  */
-
-#include <float.h>
-
-#include <drivers/airspeed/airspeed.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <drivers/device/i2c.h>
+#include <uORB/topics/sensor_differential_pressure.h>
+#include <uORB/PublicationMulti.hpp>
+
+#include <float.h>
 
 /* I2C bus address */
 #define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
@@ -60,45 +61,63 @@
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
-class ETSAirspeed : public Airspeed, public I2CSPIDriver<ETSAirspeed>
+class ETSAirspeed : public device::I2C, public I2CSPIDriver<ETSAirspeed>
 {
 public:
 	ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS);
 
-	virtual ~ETSAirspeed() = default;
+	virtual ~ETSAirspeed();
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 					     int runtime_instance);
 	static void print_usage();
 
 	void	RunImpl();
-protected:
+
-	int	measure() override;
+private:
-	int	collect() override;
+	int	probe() override;
+
+	int	measure();
+	int	collect();
+
+	bool _sensor_ok{false};
+	int _measure_interval{0};
+	bool _collect_phase{false};
+	unsigned _conversion_interval{0};
+
+	uORB::PublicationMulti<sensor_differential_pressure_s>	_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 };
 
-/*
+ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
- * Driver 'main' command.
+	I2C(DRV_DIFF_PRESS_DEVTYPE_ETS3, MODULE_NAME, bus, address, bus_frequency),
- */
-extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
-
-ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address)
-	: Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
 {
-	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
 }
 
-int
+ETSAirspeed::~ETSAirspeed()
-ETSAirspeed::measure()
 {
-	int ret;
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
 
+int ETSAirspeed::probe()
+{
+	uint8_t cmd = READ_CMD;
+	int ret = transfer(&cmd, 1, nullptr, 0);
+
+	return ret;
+}
+
+int ETSAirspeed::measure()
+{
 	/*
 	 * Send the command to begin a measurement.
 	 */
 	uint8_t cmd = READ_CMD;
-	ret = transfer(&cmd, 1, nullptr, 0);
+	int ret = transfer(&cmd, 1, nullptr, 0);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
@@ -107,59 +126,46 @@ ETSAirspeed::measure()
 	return ret;
 }
 
-int
+int ETSAirspeed::collect()
-ETSAirspeed::collect()
 {
-	int	ret = -EIO;
-
 	/* read from the sensor */
-	uint8_t val[2] = {0, 0};
-
 	perf_begin(_sample_perf);
 
-	ret = transfer(nullptr, 0, &val[0], 2);
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	uint8_t val[2] {};
+	int ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
 		perf_count(_comms_errors);
 		return ret;
 	}
 
-	float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]);
+	float diff_press_pa_raw = (float)(val[1] << 8 | val[0]);
 
-	differential_pressure_s report{};
+	if (diff_press_pa_raw < FLT_EPSILON) {
-	report.timestamp = hrt_absolute_time();
-
-	if (diff_pres_pa_raw < FLT_EPSILON) {
 		// a zero value indicates no measurement
 		// since the noise floor has been arbitrarily killed
 		// it defeats our stuck sensor detection - the best we
 		// can do is to output some numerical noise to show
 		// that we are still correctly sampling.
-		diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
+		diff_press_pa_raw = 0.001f * (hrt_absolute_time() & 0x01);
 	}
 
-	// The raw value still should be compensated for the known offset
+	sensor_differential_pressure_s report{};
-	diff_pres_pa_raw -= _diff_pres_offset;
+	report.timestamp_sample = timestamp_sample;
-
+	report.device_id = get_device_id();
-	report.error_count = perf_event_count(_comms_errors);
+	report.differential_pressure_pa = diff_press_pa_raw;
-
-	// XXX we may want to smooth out the readings to remove noise.
-	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
-	report.differential_pressure_raw_pa = diff_pres_pa_raw;
 	report.temperature = NAN;
-	report.device_id = _device_id.devid;
+	report.error_count = perf_event_count(_comms_errors);
-
+	report.timestamp = hrt_absolute_time();
-	_airspeed_pub.publish(report);
+	_differential_pressure_pub.publish(report);
-
-	ret = OK;
 
 	perf_end(_sample_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
-void
+void ETSAirspeed::RunImpl()
-ETSAirspeed::RunImpl()
 {
 	int ret;
 
@@ -228,9 +234,7 @@ I2CSPIDriverBase *ETSAirspeed::instantiate(const BusCLIArguments &cli, const Bus
 	return instance;
 }
 
-
+void ETSAirspeed::print_usage()
-void
-ETSAirspeed::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
@@ -239,8 +243,7 @@ ETSAirspeed::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
+extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[])
-ets_airspeed_main(int argc, char *argv[])
 {
 	using ThisDriver = ETSAirspeed;
 	BusCLIArguments cli{true, false};

src/drivers/differential_pressure/ms4525/CMakeLists.txt

@@ -37,6 +37,6 @@ px4_add_module(
 	SRCS
 		ms4525_airspeed.cpp
 	DEPENDS
-		drivers__airspeed
 		mathlib
+		px4_work_queue
 	)

src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp

@@ -49,12 +49,12 @@
  *    - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
  */
 
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/i2c_spi_buses.h>
-
+#include <drivers/device/i2c.h>
-#include <drivers/airspeed/airspeed.h>
+#include <uORB/topics/sensor_differential_pressure.h>
+#include <uORB/PublicationMulti.hpp>
 
 enum MS_DEVICE_TYPE {
 	DEVICE_TYPE_MS4515	= 4515,
@@ -70,11 +70,9 @@ enum MS_DEVICE_TYPE {
 
 /* Measurement rate is 100Hz */
 #define MEAS_RATE 100
-#define MEAS_DRIVER_FILTER_FREQ 1.2f
 #define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
 
-
+class MEASAirspeed : public device::I2C, public I2CSPIDriver<MEASAirspeed>
-class MEASAirspeed : public Airspeed, public I2CSPIDriver<MEASAirspeed>
 {
 public:
 	MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_MS4525DO);
@@ -87,28 +85,31 @@ public:
 
 	void	RunImpl();
 
-protected:
+private:
+	int	measure();
+	int	collect();
 
-	int	measure() override;
+	bool _sensor_ok{false};
-	int	collect() override;
+	int _measure_interval{0};
+	bool _collect_phase{false};
+	unsigned _conversion_interval{0};
 
-	math::LowPassFilter2p	_filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
+	int16_t _dp_raw_prev{0};
+	int16_t _dT_raw_prev{0};
+
+	uORB::PublicationMulti<sensor_differential_pressure_s>	_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 };
 
-/*
+MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
- * Driver 'main' command.
+	I2C(DRV_DIFF_PRESS_DEVTYPE_MS4525, MODULE_NAME, bus, address, bus_frequency),
- */
-extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]);
-
-MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address)
-	: Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
 {
-	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
 }
 
-int
+int MEASAirspeed::measure()
-MEASAirspeed::measure()
 {
 	// Send the command to begin a measurement.
 	uint8_t cmd = 0;
@@ -121,14 +122,15 @@ MEASAirspeed::measure()
 	return ret;
 }
 
-int
+int MEASAirspeed::collect()
-MEASAirspeed::collect()
 {
 	/* read from the sensor */
-	uint8_t val[4] = {0, 0, 0, 0};
+	uint8_t val[4] {};
 
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
 	int ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
@@ -159,12 +161,9 @@ MEASAirspeed::collect()
 		return -EAGAIN;
 	}
 
-	int16_t dp_raw = 0, dT_raw = 0;
-	dp_raw = (val[0] << 8) + val[1];
 	/* mask the used bits */
-	dp_raw = 0x3FFF & dp_raw;
+	int16_t dp_raw = (0x3FFF & ((val[0] << 8) + val[1]));
-	dT_raw = (val[2] << 8) + val[3];
+	int16_t dT_raw = (0xFFE0 & ((val[2] << 8) + val[3])) >> 5;
-	dT_raw = (0xFFE0 & dT_raw) >> 5;
 
 	// dT max is almost certainly an invalid reading
 	if (dT_raw == 2047) {
@@ -172,6 +171,12 @@ MEASAirspeed::collect()
 		return -EAGAIN;
 	}
 
+	// only publish changes
+	if ((dp_raw != _dp_raw_prev) && (dT_raw != _dT_raw_prev)) {
+
+		_dp_raw_prev = dp_raw;
+		_dT_raw_prev = dT_raw;
+
 		float temperature = ((200.0f * dT_raw) / 2047) - 50;
 
 		// Calculate differential pressure. As its centered around 8000
@@ -195,27 +200,22 @@ MEASAirspeed::collect()
 		positive number when the top port is used as a dynamic port
 		and bottom port is used as the static port
 		*/
-
+		sensor_differential_pressure_s report{};
-	differential_pressure_s report{};
+		report.timestamp_sample = timestamp_sample;
-
+		report.device_id = get_device_id();
-	report.timestamp = hrt_absolute_time();
+		report.differential_pressure_pa = diff_press_pa_raw;
-	report.error_count = perf_event_count(_comms_errors);
 		report.temperature = temperature;
-	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
+		report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
+		report.timestamp = hrt_absolute_time();
-	report.device_id = _device_id.devid;
+		_differential_pressure_pub.publish(report);
-
+	}
-	_airspeed_pub.publish(report);
-
-	ret = OK;
 
 	perf_end(_sample_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
-void
+void MEASAirspeed::RunImpl()
-MEASAirspeed::RunImpl()
 {
 	int ret;
 
@@ -251,7 +251,7 @@ MEASAirspeed::RunImpl()
 	/* measurement phase */
 	ret = measure();
 
-	if (OK != ret) {
+	if (PX4_OK != ret) {
 		DEVICE_DEBUG("measure error");
 	}
 
@@ -284,9 +284,7 @@ I2CSPIDriverBase *MEASAirspeed::instantiate(const BusCLIArguments &cli, const Bu
 	return instance;
 }
 
-
+void MEASAirspeed::print_usage()
-void
-MEASAirspeed::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
@@ -296,8 +294,7 @@ MEASAirspeed::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
+extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[])
-ms4525_airspeed_main(int argc, char *argv[])
 {
 	int ch;
 	using ThisDriver = MEASAirspeed;

src/drivers/differential_pressure/ms5525/CMakeLists.txt

@@ -31,13 +31,14 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__ms5525_airspeed
+	MODULE drivers__differential_pressure__ms5525
-	MAIN ms5525_airspeed
+	MAIN ms5525
 	COMPILE_FLAGS
 	SRCS
+		ms5525_main.cpp
 		MS5525.cpp
-		MS5525_main.cpp
+		MS5525.hpp
 	DEPENDS
-		drivers__airspeed
 		mathlib
+		px4_work_queue
 	)

src/drivers/differential_pressure/ms5525/MS5525.cpp

@@ -33,8 +33,18 @@
 
 #include "MS5525.hpp"
 
-int
+MS5525::~MS5525()
-MS5525::measure()
+{
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int MS5525::probe()
+{
+	return init_ms5525() ? PX4_OK : PX4_ERROR;
+}
+
+int MS5525::measure()
 {
 	int ret = PX4_ERROR;
 
@@ -58,8 +68,7 @@ MS5525::measure()
 	return ret;
 }
 
-bool
+bool MS5525::init_ms5525()
-MS5525::init_ms5525()
 {
 	// Step 1 - reset
 	uint8_t cmd = CMD_RESET;
@@ -128,8 +137,7 @@ MS5525::init_ms5525()
 	}
 }
 
-uint8_t
+uint8_t MS5525::prom_crc4(uint16_t n_prom[]) const
-MS5525::prom_crc4(uint16_t n_prom[]) const
 {
 	// see Measurement Specialties AN520
 
@@ -166,11 +174,12 @@ MS5525::prom_crc4(uint16_t n_prom[]) const
 	return (n_rem ^ 0x00);
 }
 
-int
+int MS5525::collect()
-MS5525::collect()
 {
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
 	// read ADC
 	uint8_t cmd = CMD_ADC_READ;
 	int ret = transfer(&cmd, 1, nullptr, 0);
@@ -181,7 +190,7 @@ MS5525::collect()
 	}
 
 	// read 24 bits from the sensor
-	uint8_t val[3];
+	uint8_t val[3] {};
 	ret = transfer(nullptr, 0, &val[0], 3);
 
 	if (ret != PX4_OK) {
@@ -248,28 +257,23 @@ MS5525::collect()
 	static constexpr float PSI_to_Pa = 6894.757f;
 	const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
 
-	const float temperature_c = TEMP * 0.01f;
+	const float temperature = TEMP * 0.01f;
 
-	differential_pressure_s diff_pressure = {
+	sensor_differential_pressure_s report{};
-		.timestamp = hrt_absolute_time(),
+	report.timestamp_sample = timestamp_sample;
-		.error_count = perf_event_count(_comms_errors),
+	report.device_id = get_device_id();
-		.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset,
+	report.differential_pressure_pa = diff_press_pa_raw;
-		.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw) - _diff_pres_offset,
+	report.temperature = temperature;
-		.temperature = temperature_c,
+	report.error_count = perf_event_count(_comms_errors);
-		.device_id = _device_id.devid
+	report.timestamp = hrt_absolute_time();
-	};
+	_differential_pressure_pub.publish(report);
-
-	_airspeed_pub.publish(diff_pressure);
-
-	ret = OK;
 
 	perf_end(_sample_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
-void
+void MS5525::RunImpl()
-MS5525::RunImpl()
 {
 	int ret = PX4_ERROR;
 

src/drivers/differential_pressure/ms5525/MS5525.hpp

@@ -33,31 +33,29 @@
 
 #pragma once
 
-#include <drivers/airspeed/airspeed.h>
-#include <math.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <drivers/device/i2c.h>
+#include <uORB/topics/sensor_differential_pressure.h>
+#include <uORB/PublicationMulti.hpp>
 
 /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
 static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76;
 
 /* Measurement rate is 100Hz */
 static constexpr unsigned MEAS_RATE = 100;
-static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f;
 static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
 
-class MS5525 : public Airspeed, public I2CSPIDriver<MS5525>
+class MS5525 : public device::I2C, public I2CSPIDriver<MS5525>
 {
 public:
 	MS5525(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_MS5525DSO) :
-		Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL),
+		I2C(DRV_DIFF_PRESS_DEVTYPE_MS5525, MODULE_NAME, bus, address, bus_frequency),
 		I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
-	{
+	{}
-	}
 
-	virtual ~MS5525() = default;
+	virtual ~MS5525();
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 					     int runtime_instance);
@@ -66,12 +64,14 @@ public:
 	void	RunImpl();
 
 private:
+	int probe() override;
 
-	int measure() override;
+	bool init_ms5525();
-	int collect() override;
 
-	// temperature is read once every 10 cycles
+	uint8_t prom_crc4(uint16_t n_prom[]) const;
-	math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
+
+	int measure();
+	int collect();
 
 	static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
 	static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
@@ -121,9 +121,16 @@ private:
 	uint32_t D1{0};
 	uint32_t D2{0};
 
-	bool init_ms5525();
 	bool _inited{false};
 
-	uint8_t prom_crc4(uint16_t n_prom[]) const;
+	bool _sensor_ok{false};
+	int _measure_interval{0};
+	bool _collect_phase{false};
+	unsigned _conversion_interval{0};
+
+	uORB::PublicationMulti<sensor_differential_pressure_s>	_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 
 };

src/drivers/differential_pressure/ms5525/ms5525_main.cpp

@@ -33,8 +33,6 @@
 
 #include "MS5525.hpp"
 
-extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]);
-
 I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				      int runtime_instance)
 {
@@ -54,19 +52,16 @@ I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInsta
 	return instance;
 }
 
-
+void MS5525::print_usage()
-void
-MS5525::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver");
+	PRINT_MODULE_USAGE_NAME("ms5525", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
+extern "C" __EXPORT int ms5525_main(int argc, char *argv[])
-ms5525_airspeed_main(int argc, char *argv[])
 {
 	using ThisDriver = MS5525;
 	BusCLIArguments cli{true, false};

src/drivers/differential_pressure/sdp3x/CMakeLists.txt

@@ -31,13 +31,14 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__sdp3x_airspeed
+	MODULE drivers__differential_pressure__sdp3x
-	MAIN sdp3x_airspeed
+	MAIN sdp3x
 	COMPILE_FLAGS
 	SRCS
+		sdp3x_main.cpp
 		SDP3X.cpp
-		SDP3X_main.cpp
+		SDP3X.hpp
 	DEPENDS
-		drivers__airspeed
 		mathlib
+		px4_work_queue
 	)

src/drivers/differential_pressure/sdp3x/SDP3X.cpp

@@ -42,8 +42,13 @@
 
 using namespace time_literals;
 
-int
+SDP3X::~SDP3X()
-SDP3X::probe()
+{
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int SDP3X::probe()
 {
 	bool require_initialization = !init_sdp3x();
 
@@ -63,14 +68,12 @@ int SDP3X::write_command(uint16_t command)
 	return transfer(&cmd[0], 2, nullptr, 0);
 }
 
-bool
+bool SDP3X::init_sdp3x()
-SDP3X::init_sdp3x()
 {
 	return configure() == 0;
 }
 
-int
+int SDP3X::configure()
-SDP3X::configure()
 {
 	int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE);
 
@@ -86,8 +89,7 @@ SDP3X::configure()
 	return ret;
 }
 
-int
+int SDP3X::read_scale()
-SDP3X::read_scale()
 {
 	// get scale
 	uint8_t val[9];
@@ -109,15 +111,15 @@ SDP3X::read_scale()
 
 	switch (_scale) {
 	case SDP3X_SCALE_PRESSURE_SDP31:
-		_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31;
+		set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP31);
 		break;
 
 	case SDP3X_SCALE_PRESSURE_SDP32:
-		_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP32;
+		set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP32);
 		break;
 
 	case SDP3X_SCALE_PRESSURE_SDP33:
-		_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP33;
+		set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP33);
 		break;
 	}
 
@@ -130,13 +132,14 @@ void SDP3X::start()
 	ScheduleDelayed(10_ms);
 }
 
-int
+int SDP3X::collect()
-SDP3X::collect()
 {
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
 	// read 6 bytes from the sensor
-	uint8_t val[6];
+	uint8_t val[6] {};
 	int ret = transfer(nullptr, 0, &val[0], sizeof(val));
 
 	if (ret != PX4_OK) {
@@ -154,26 +157,23 @@ SDP3X::collect()
 	int16_t temp = (((int16_t)val[3]) << 8) | val[4];
 
 	float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
-	float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
+	float temperature = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
 
-	differential_pressure_s report{};
+	sensor_differential_pressure_s report{};
-
+	report.timestamp_sample = timestamp_sample;
-	report.timestamp = hrt_absolute_time();
+	report.device_id = get_device_id();
+	report.differential_pressure_pa = diff_press_pa_raw;
+	report.temperature = temperature;
 	report.error_count = perf_event_count(_comms_errors);
-	report.temperature = temperature_c;
+	report.timestamp = hrt_absolute_time();
-	report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
+	_differential_pressure_pub.publish(report);
-	report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
-	report.device_id = _device_id.devid;
-
-	_airspeed_pub.publish(report);
 
 	perf_end(_sample_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
-void
+void SDP3X::RunImpl()
-SDP3X::RunImpl()
 {
 	switch (_state) {
 	case State::RequireConfig:

src/drivers/differential_pressure/sdp3x/SDP3X.hpp

@@ -36,17 +36,17 @@
  *
  * Driver for Sensirion SDP3X Differential Pressure Sensor
  *
- * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_Differential_Pressure_Sensors_SDP3x_Digital_Datasheet_V0.8.pdf
+ * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_sensor_differential_pressure_sensors_SDP3x_Digital_Datasheet_V0.8.pdf
  */
 
 #pragma once
 
-#include <drivers/airspeed/airspeed.h>
-#include <math.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <drivers/device/i2c.h>
+#include <uORB/topics/sensor_differential_pressure.h>
+#include <uORB/PublicationMulti.hpp>
 
 #define I2C_ADDRESS_1_SDP3X		0x21
 #define I2C_ADDRESS_2_SDP3X		0x22
@@ -63,21 +63,20 @@
 
 // Measurement rate is 20Hz
 #define SPD3X_MEAS_RATE 100
-#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f
 #define CONVERSION_INTERVAL	(1000000 / SPD3X_MEAS_RATE)	/* microseconds */
 
-class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
+class SDP3X : public device::I2C, public I2CSPIDriver<SDP3X>
 {
 public:
 	SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X,
 	      bool keep_retrying = false) :
-		Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL),
+		I2C(DRV_DIFF_PRESS_DEVTYPE_SDP33, MODULE_NAME, bus, address, bus_frequency),
 		I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address),
 		_keep_retrying{keep_retrying}
 	{
 	}
 
-	virtual ~SDP3X() = default;
+	virtual ~SDP3X();
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 					     int runtime_instance);
@@ -94,14 +93,13 @@ private:
 		Running
 	};
 
-	int	measure() override { return 0; }
+	int	measure() { return 0; }
-	int	collect() override;
+	int	collect();
+
 	int	probe() override;
 	int	configure();
 	int	read_scale();
 
-	math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
-
 	bool init_sdp3x();
 
 	/**
@@ -114,7 +112,17 @@ private:
 	 */
 	int write_command(uint16_t command);
 
+	bool _sensor_ok{false};
+	int _measure_interval{0};
+	bool _collect_phase{false};
+	unsigned _conversion_interval{0};
+
 	uint16_t _scale{0};
 	const bool _keep_retrying;
 	State _state{State::RequireConfig};
+
+	uORB::PublicationMulti<sensor_differential_pressure_s>	_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 };

src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp

@@ -33,8 +33,6 @@
 
 #include "SDP3X.hpp"
 
-extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]);
-
 I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				     int runtime_instance)
 {
@@ -55,11 +53,9 @@ I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstan
 	return instance;
 }
 
-
+void SDP3X::print_usage()
-void
-SDP3X::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver");
+	PRINT_MODULE_USAGE_NAME("sdp3x", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -68,8 +64,7 @@ SDP3X::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
+extern "C" __EXPORT int sdp3x_main(int argc, char *argv[])
-sdp3x_airspeed_main(int argc, char *argv[])
 {
 	using ThisDriver = SDP3X;
 	BusCLIArguments cli{true, false};

src/drivers/drv_airspeed.h

@@ -1,72 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_airspeed.h
- *
- * Airspeed driver interface.
- *
- * @author Simon Wilks
- */
-
-#ifndef _DRV_AIRSPEED_H
-#define _DRV_AIRSPEED_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-#include "drv_sensor.h"
-#include "drv_orb_dev.h"
-
-#define AIRSPEED_BASE_DEVICE_PATH "/dev/airspeed"
-#define AIRSPEED0_DEVICE_PATH	"/dev/airspeed0"
-
-/*
- * ioctl() definitions
- *
- * Airspeed drivers also implement the generic sensor driver
- * interfaces from drv_sensor.h
- */
-
-#define _AIRSPEEDIOCBASE		(0x7700)
-#define __AIRSPEEDIOC(_n)		(_PX4_IOC(_AIRSPEEDIOCBASE, _n))
-
-#define AIRSPEEDIOCSSCALE		__AIRSPEEDIOC(0)
-
-/** airspeed scaling factors; out = (in * Vscale) + offset */
-struct airspeed_scale {
-	float	offset_pa;
-	float	scale;
-};
-
-#endif /* _DRV_AIRSPEED_H */

src/drivers/uavcan/sensors/airspeed.cpp

@@ -47,7 +47,7 @@ UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) :
 	_sub_ias_data(node),
 	_sub_tas_data(node),
 	_sub_oat_data(node)
-{ }
+{}
 
 int UavcanAirspeedBridge::init()
 {
@@ -75,36 +75,26 @@ int UavcanAirspeedBridge::init()
 	return 0;
 }
 
-void
+void UavcanAirspeedBridge::oat_sub_cb(const
-UavcanAirspeedBridge::oat_sub_cb(const
 				      uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
 {
 	_last_outside_air_temp_k = msg.static_temperature;
 }
 
-void
+void UavcanAirspeedBridge::tas_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed>
-UavcanAirspeedBridge::tas_sub_cb(const
+				      &msg)
-				 uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &msg)
 {
 	_last_tas_m_s = msg.true_airspeed;
 }
-void
+
-UavcanAirspeedBridge::ias_sub_cb(const
+void UavcanAirspeedBridge::ias_sub_cb(const
 				      uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &msg)
 {
 	airspeed_s report{};
-
+	report.timestamp_sample   	= hrt_absolute_time();
-	/*
-	 * FIXME HACK
-	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
-	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
-	 * to use an independent time source (based on hardware TIM5) instead of HRT.
-	 * The proper solution is to be developed.
-	 */
-	report.timestamp   		= hrt_absolute_time();
 	report.indicated_airspeed_m_s   = msg.indicated_airspeed;
 	report.true_airspeed_m_s   	= _last_tas_m_s;
 	report.air_temperature_celsius 	= _last_outside_air_temp_k + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
-
+	report.timestamp   		= hrt_absolute_time();
 	publish(msg.getSrcNodeID().get(), &report);
 }

src/drivers/uavcan/sensors/airspeed.hpp

@@ -38,7 +38,6 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-#include <drivers/drv_airspeed.h>
 #include <uORB/topics/airspeed.h>
 
 #include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -37,7 +37,6 @@
 
 #include "differential_pressure.hpp"
 
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
 #include <parameters/param.h>
@@ -46,16 +45,13 @@
 const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure";
 
 UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure)),
+	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(sensor_differential_pressure)),
 	_sub_air(node)
 {
 }
 
 int UavcanDifferentialPressureBridge::init()
 {
-	// Initialize the calibration offset
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
-
 	int res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb));
 
 	if (res < 0) {
@@ -72,17 +68,13 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
 	_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
 
-	float diff_press_pa = msg.differential_pressure;
+	sensor_differential_pressure_s report{};
-	float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	report.timestamp_sample = hrt_absolute_time();
-
+	report.device_id = get_device_id();
-	differential_pressure_s report = {
+	report.differential_pressure_pa = msg.differential_pressure;
-		.timestamp = hrt_absolute_time(),
+	report.temperature = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
-		.error_count = 0,
+	report.error_count = 0;
-		.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset,
+	report.timestamp = hrt_absolute_time();
-		.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter
-		.temperature = temperature_c,
-		.device_id = _device_id.devid
-	};
 
 	publish(msg.getSrcNodeID().get(), &report);
 }

src/drivers/uavcan/sensors/differential_pressure.hpp

@@ -37,9 +37,7 @@
 
 #pragma once
 
-#include <uORB/uORB.h>
+#include <uORB/topics/sensor_differential_pressure.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include "sensor_bridge.hpp"
 
@@ -57,10 +55,6 @@ public:
 	int init() override;
 
 private:
-	float _diff_pres_offset{0.f};
-
-	math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
-
 	void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);
 
 	typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *,

src/drivers/uavcannode/Publishers/RawAirData.hpp

@@ -38,7 +38,7 @@
 #include <uavcan/equipment/air_data/RawAirData.hpp>
 
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 
 namespace uavcannode
 {
@@ -51,7 +51,7 @@ class RawAirData :
 public:
 	RawAirData(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(uavcan::equipment::air_data::RawAirData::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(differential_pressure)),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_differential_pressure)),
 		uavcan::Publisher<uavcan::equipment::air_data::RawAirData>(node)
 	{}
 
@@ -68,13 +68,13 @@ public:
 	void BroadcastAnyUpdates() override
 	{
 		// differential_pressure -> uavcan::equipment::air_data::RawAirData
-		differential_pressure_s diff_press;
+		sensor_differential_pressure_s diff_press;
 
 		if (uORB::SubscriptionCallbackWorkItem::update(&diff_press)) {
 			uavcan::equipment::air_data::RawAirData raw_air_data{};
 
 			// raw_air_data.static_pressure =
-			raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa;
+			raw_air_data.differential_pressure = diff_press.differential_pressure_pa;
 			// raw_air_data.static_pressure_sensor_temperature =
 			raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
 			raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;

src/lib/drivers/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 add_subdirectory(accelerometer)
-add_subdirectory(airspeed)
 add_subdirectory(barometer)
 add_subdirectory(device)
 add_subdirectory(gyroscope)

src/lib/drivers/airspeed/CMakeLists.txt

@@ -1,35 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(drivers__airspeed airspeed.cpp)
-target_link_libraries(drivers__airspeed PRIVATE drivers__device)

src/lib/drivers/airspeed/airspeed.cpp

@@ -1,130 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file airspeed.cpp
- * @author Simon Wilks <simon@px4.io>
- * @author Lorenz Meier <lorenz@px4.io>
- *
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <drivers/device/device.h>
-
-#include <drivers/device/i2c.h>
-
-#include <systemlib/err.h>
-#include <parameters/param.h>
-#include <perf/perf_counter.h>
-
-#include <drivers/drv_airspeed.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/differential_pressure.h>
-
-#include <drivers/airspeed/airspeed.h>
-
-Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval) :
-	I2C(0, "Airspeed", bus, address, bus_frequency),
-	_sensor_ok(false),
-	_measure_interval(conversion_interval),
-	_collect_phase(false),
-	_diff_pres_offset(0.0f),
-	_airspeed_orb_class_instance(-1),
-	_class_instance(-1),
-	_conversion_interval(conversion_interval),
-	_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "aspd_com_err"))
-{
-}
-
-Airspeed::~Airspeed()
-{
-	if (_class_instance != -1) {
-		unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
-	}
-
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-}
-
-int
-Airspeed::init()
-{
-	/* do I2C init (and probe) first */
-	if (I2C::init() != PX4_OK) {
-		return PX4_ERROR;
-	}
-
-	/* register alternate interfaces if we have to */
-	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
-
-	/* advertise sensor topic, measure manually to initialize valid report */
-	measure();
-
-	return PX4_OK;
-}
-
-int
-Airspeed::probe()
-{
-	/* on initial power up the device may need more than one retry
-	   for detection. Once it is running the number of retries can
-	   be reduced
-	*/
-	_retries = 4;
-	int ret = measure();
-
-	// drop back to 2 retries once initialised
-	_retries = 2;
-	return ret;
-}
-
-int
-Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case AIRSPEEDIOCSSCALE: {
-			struct airspeed_scale *s = (struct airspeed_scale *)arg;
-			_diff_pres_offset = s->offset_pa;
-			return OK;
-		}
-
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-

src/lib/drivers/airspeed/airspeed.h

@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <string.h>
-#include <drivers/device/i2c.h>
-#include <drivers/drv_airspeed.h>
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <perf/perf_counter.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/PublicationMulti.hpp>
-
-class __EXPORT Airspeed : public device::I2C
-{
-public:
-	Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval);
-	virtual ~Airspeed();
-
-	int	init() override;
-
-	int	ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
-
-private:
-	Airspeed(const Airspeed &) = delete;
-	Airspeed &operator=(const Airspeed &) = delete;
-
-protected:
-	int	probe() override;
-
-	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
-	virtual int	measure() = 0;
-	virtual int	collect() = 0;
-
-	bool			_sensor_ok;
-	int				_measure_interval;
-	bool			_collect_phase;
-	float			_diff_pres_offset;
-
-	uORB::PublicationMulti<differential_pressure_s>	_airspeed_pub{ORB_ID(differential_pressure)};
-
-	int			_airspeed_orb_class_instance;
-
-	int			_class_instance;
-
-	unsigned		_conversion_interval;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_comms_errors;
-};
-
-

src/modules/commander/airspeed_calibration.cpp

@@ -44,18 +44,16 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <math.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_airspeed.h>
+#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
-#include <uORB/topics/differential_pressure.h>
+#include <lib/systemlib/mavlink_log.h>
-#include <systemlib/mavlink_log.h>
+#include <lib/parameters/param.h>
-#include <parameters/param.h>
+#include <uORB/SubscriptionBlocking.hpp>
-#include <systemlib/err.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 
-static const char *sensor_name = "airspeed";
+using namespace time_literals;
+
+static constexpr char sensor_name[] {"airspeed"};
 
 static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
 {
@@ -67,114 +65,58 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 {
 	const hrt_abstime calibration_started = hrt_absolute_time();
 
-	int result = PX4_OK;
+	static constexpr unsigned MAX_COUNT = 2400;
-	unsigned calibration_counter = 0;
+	static constexpr unsigned CALIBRATION_COUNT = (MAX_COUNT * 2) / 3;
-	const unsigned maxcount = 2400;
 
 	/* give directions */
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
 
-	const unsigned calibration_count = (maxcount * 2) / 3;
+	uORB::SubscriptionBlocking<sensor_differential_pressure_s> diff_pres_sub{ORB_ID(sensor_differential_pressure)};
+	sensor_differential_pressure_s diff_pres{};
+	diff_pres_sub.copy(&diff_pres);
 
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	// store initial error count
-	struct differential_pressure_s diff_pres;
+	uint32_t error_count = diff_pres.error_count;
 
-	float diff_pres_offset = 0.0f;
+	float diff_pres_total = 0.0f;
+	unsigned calibration_counter = 0;
 
-	/* Reset sensor parameters */
-	struct airspeed_scale airscale = {
-		diff_pres_offset,
-		1.0f,
-	};
-
-	bool paramreset_successful = false;
-	int  fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-
-	if (fd >= 0) {
-		if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-			paramreset_successful = true;
-
-		} else {
-			calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
-		}
-
-		px4_close(fd);
-	}
-
-	if (!paramreset_successful) {
-
-		/* only warn if analog scaling is zero */
-		float analog_scaling = 0.0f;
-		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
-
-		if (fabsf(analog_scaling) < 0.1f) {
-			calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
-			goto error_return;
-		}
-
-		/* set scaling offset parameter */
-		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-			goto error_return;
-		}
-	}
-
-	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
 	px4_usleep(500 * 1000);
+	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
 
-	while (calibration_counter < calibration_count) {
+	while (calibration_counter < CALIBRATION_COUNT) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
-		/* wait blocking for new data */
+		if (diff_pres_sub.updateBlocking(diff_pres, 1_s)) {
-		px4_pollfd_struct_t fds[1];
-		fds[0].fd = diff_pres_sub;
-		fds[0].events = POLLIN;
 
-		int poll_ret = px4_poll(fds, 1, 1000);
+			diff_pres_total += diff_pres.differential_pressure_pa;
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-
-			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			/* any differential pressure failure a reason to abort */
-			if (diff_pres.error_count != 0) {
+			if (diff_pres.error_count > error_count) {
-				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
+				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%d)", diff_pres.error_count);
-							 diff_pres.error_count);
 				calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
 				feedback_calibration_failed(mavlink_log_pub);
-				goto error_return;
+				return PX4_ERROR;
 			}
 
-			if (calibration_counter % (calibration_count / 20) == 0) {
+			if (calibration_counter % (CALIBRATION_COUNT / 20) == 0) {
-				calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
+				calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / CALIBRATION_COUNT);
 			}
 
-		} else if (poll_ret == 0) {
+		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
-			goto error_return;
+			return PX4_ERROR;
 		}
 	}
 
-	diff_pres_offset = diff_pres_offset / calibration_count;
+	float diff_pres_offset = diff_pres_total / calibration_counter;
 
-	if (PX4_ISFINITE(diff_pres_offset)) {
+	if ((calibration_counter > 0) && PX4_ISFINITE(diff_pres_offset)) {
-
-		int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-		airscale.offset_pa = diff_pres_offset;
-
-		if (fd_scale >= 0) {
-			if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
-			}
-
-			px4_close(fd_scale);
-		}
 
 		// Prevent a completely zero param
 		// since this is used to detect a missing calibration
@@ -184,89 +126,80 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 			diff_pres_offset = 0.00000001f;
 		}
 
-		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+		if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) {
 			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 	} else {
 		feedback_calibration_failed(mavlink_log_pub);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
-	calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+	calibration_log_info(mavlink_log_pub, "[cal] Offset of %.1f Pascal", (double)diff_pres_offset);
 
-	/* wait 500 ms to ensure parameter propagated through the system */
+	/* wait 100 ms to ensure parameter propagated through the system */
-	px4_usleep(500 * 1000);
+	px4_usleep(100 * 1000);
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");
 
+	AlphaFilter<float> diff_pres_filtered{0.1f};
 	calibration_counter = 0;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
-	while (calibration_counter < maxcount) {
+	while (calibration_counter < MAX_COUNT) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
-		/* wait blocking for new data */
+		if (diff_pres_sub.updateBlocking(diff_pres, 1_s)) {
-		px4_pollfd_struct_t fds[1];
-		fds[0].fd = diff_pres_sub;
-		fds[0].events = POLLIN;
 
-		int poll_ret = px4_poll(fds, 1, 1000);
+			diff_pres_filtered.update(diff_pres.differential_pressure_pa - diff_pres_offset);
 
-		if (poll_ret) {
+			if (fabsf(diff_pres_filtered.getState()) > 50.0f) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+				if (diff_pres_filtered.getState() > 0) {
-
+					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%.1f Pa)", (double)diff_pres_filtered.getState());
-			if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
-				if (diff_pres.differential_pressure_filtered_pa > 0) {
-					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
-							     (int)diff_pres.differential_pressure_filtered_pa);
 					break;
 
 				} else {
 					/* do not allow negative values */
-					calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
+					calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%.1f Pa)",
-								 (int)diff_pres.differential_pressure_filtered_pa);
+								 (double)diff_pres_filtered.getState());
 					calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
 
 					/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
-					diff_pres_offset = 0.0f;
+					if (param_reset(param_find("SENS_DPRES_OFF")) != PX4_OK) {
-
-					if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 						calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-						goto error_return;
+						return PX4_ERROR;
 					}
 
 					/* save */
 					calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
-					param_save_default();
 
 					feedback_calibration_failed(mavlink_log_pub);
-					goto error_return;
+					return PX4_ERROR;
 				}
 			}
 
 			if (calibration_counter % 500 == 0) {
-				calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
+				calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %.1f, wanted: 50 Pa)",
-						     (int)diff_pres.differential_pressure_filtered_pa);
+						     (double)diff_pres_filtered.getState());
 				tune_neutral(true);
 			}
 
 			calibration_counter++;
 
-		} else if (poll_ret == 0) {
+		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
-			goto error_return;
+			return PX4_ERROR;
 		}
 	}
 
-	if (calibration_counter == maxcount) {
+	if (calibration_counter == MAX_COUNT) {
 		feedback_calibration_failed(mavlink_log_pub);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
@@ -274,19 +207,5 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
 
-	/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
+	return PX4_OK;
-	 * the followup preflight checks might fail. */
-	px4_usleep(2e6);
-
-normal_return:
-	px4_close(diff_pres_sub);
-
-	// This give a chance for the log messages to go out of the queue before someone else stomps on then
-	px4_sleep(1);
-
-	return result;
-
-error_return:
-	result = PX4_ERROR;
-	goto normal_return;
 }

src/modules/mavlink/mavlink_messages.cpp

@@ -63,7 +63,7 @@
 #include <uORB/topics/camera_capture.h>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/cpuload.h>
-#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
@@ -923,7 +923,7 @@ private:
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
-	uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
+	uORB::Subscription _sensor_differential_pressure_sub{ORB_ID(sensor_differential_pressure)};
 	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
 	uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
 
@@ -1026,14 +1026,14 @@ protected:
 				_air_data_sub.copy(&air_data);
 			}
 
-			differential_pressure_s differential_pressure{};
+			sensor_differential_pressure_s differential_pressure{};
 
-			if (_differential_pressure_sub.update(&differential_pressure)) {
+			if (_sensor_differential_pressure_sub.update(&differential_pressure)) {
 				/* mark fourth group (dpres field) dimensions as changed */
 				fields_updated |= (1 << 10);
 
 			} else {
-				_differential_pressure_sub.copy(&differential_pressure);
+				_sensor_differential_pressure_sub.copy(&differential_pressure);
 			}
 
 			const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
@@ -1055,7 +1055,7 @@ protected:
 			msg.ymag = mag(1);
 			msg.zmag = mag(2);
 			msg.abs_pressure = air_data.baro_pressure_pa;
-			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
+			msg.diff_pressure = differential_pressure.differential_pressure_pa;
 			msg.pressure_alt = air_data.baro_alt_meter;
 			msg.temperature = air_data.baro_temp_celcius;
 			msg.fields_updated = fields_updated;
@@ -1089,7 +1089,7 @@ public:
 	}
 
 private:
-	uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
+	uORB::Subscription _sensor_differential_pressure_sub{ORB_ID(sensor_differential_pressure)};
 	uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), N};
 
 	/* do not allow top copying this class */
@@ -1102,16 +1102,16 @@ protected:
 
 	bool send() override
 	{
-		if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) {
+		if (_sensor_baro_sub.updated() || _sensor_differential_pressure_sub.updated()) {
 			sensor_baro_s sensor_baro{};
-			differential_pressure_s differential_pressure{};
+			sensor_differential_pressure_s differential_pressure{};
 			_sensor_baro_sub.copy(&sensor_baro);
-			_differential_pressure_sub.copy(&differential_pressure);
+			_sensor_differential_pressure_sub.copy(&differential_pressure);
 
 			typename Derived::mav_msg_type msg{};
 			msg.time_boot_ms = sensor_baro.timestamp / 1000;
 			msg.press_abs = sensor_baro.pressure;
-			msg.press_diff = differential_pressure.differential_pressure_raw_pa;
+			msg.press_diff = differential_pressure.differential_pressure_pa;
 			msg.temperature = sensor_baro.temperature;
 
 			Derived::send(_mavlink->get_channel(), &msg);

src/modules/mavlink/mavlink_receiver.cpp

@@ -2345,12 +2345,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	// differential pressure
 	if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
-		differential_pressure_s report{};
+		sensor_differential_pressure_s report{};
-		report.timestamp = timestamp;
+		report.timestamp_sample = timestamp;
 		report.temperature = hil_sensor.temperature;
-		report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
-		report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.timestamp = hrt_absolute_time();
-
 		_differential_pressure_pub.publish(report);
 	}
 

src/modules/mavlink/mavlink_receiver.h

@@ -63,7 +63,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
-#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
@@ -356,7 +356,7 @@ private:
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
-	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<sensor_differential_pressure_s>		_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
 	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<gimbal_manager_set_manual_control_s>	_gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};

src/modules/sensors/airspeed/Airspeed.cpp

@@ -34,11 +34,12 @@
 #include "Airspeed.hpp"
 
 #include <px4_platform_common/log.h>
-#include <drivers/drv_airspeed.h>
 #include <lib/ecl/geo/geo.h>
 
 #include <lib/airspeed/airspeed.h>
 
+#include <drivers/drv_sensor.h>
+
 namespace sensors
 {
 
@@ -94,23 +95,6 @@ void Airspeed::ParametersUpdate(bool force)
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
-
-		/* update airspeed scale */
-		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-
-		/* this sensor is optional, abort without error */
-		if (fd >= 0) {
-			struct airspeed_scale airscale = {
-				_param_sens_dpres_off.get(),
-				1.0f,
-			};
-
-			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				PX4_ERR("failed to set offset for differential pressure sensor");
-			}
-
-			px4_close(fd);
-		}
 	}
 }
 
@@ -169,15 +153,17 @@ void Airspeed::Run()
 		}
 
 		if (_advertised[uorb_index]) {
-			differential_pressure_s diff_pres;
+			sensor_differential_pressure_s diff_pres;
 
 			while (_sensor_sub[uorb_index].update(&diff_pres)) {
 				updated[uorb_index] = true;
 
 				_device_id[uorb_index] = diff_pres.device_id;
 
-				float vect[3] {diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.f};
+				float differential_pressure_pa = diff_pres.differential_pressure_pa - _param_sens_dpres_off.get();
-				_voter.put(uorb_index, diff_pres.timestamp, vect, diff_pres.error_count, _priority[uorb_index]);
+
+				float vect[3] {differential_pressure_pa, diff_pres.temperature, 0.f};
+				_voter.put(uorb_index, diff_pres.timestamp_sample, vect, diff_pres.error_count, _priority[uorb_index]);
 
 
 				float air_temperature_celsius = NAN;
@@ -195,8 +181,8 @@ void Airspeed::Run()
 				}
 
 				// average raw data for all instances
-				_timestamp_sample_sum[uorb_index] += diff_pres.timestamp;
+				_timestamp_sample_sum[uorb_index] += diff_pres.timestamp_sample;
-				_differential_pressure_sum[uorb_index] += diff_pres.differential_pressure_filtered_pa;
+				_sensor_differential_pressure_sum[uorb_index] += differential_pressure_pa;
 				_temperature_sum[uorb_index] += air_temperature_celsius;
 				_sum_count[uorb_index]++;
 			}
@@ -288,13 +274,13 @@ void Airspeed::Publish(uint8_t instance, bool multi)
 	if ((_param_sens_dpres_rate.get() > 0)
 	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_dpres_rate.get())) {
 
-		const float differential_pressure = _differential_pressure_sum[instance] / _sum_count[instance];
+		const float differential_pressure = _sensor_differential_pressure_sum[instance] / _sum_count[instance];
 		const float temperature = _temperature_sum[instance] / _sum_count[instance];
 		const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _sum_count[instance];
 
 		// reset
 		_timestamp_sample_sum[instance] = 0;
-		_differential_pressure_sum[instance] = 0;
+		_sensor_differential_pressure_sum[instance] = 0;
 		_temperature_sum[instance] = 0;
 		_sum_count[instance] = 0;
 

src/modules/sensors/airspeed/Airspeed.hpp

@@ -48,7 +48,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_air_data.h>
 
@@ -88,10 +88,10 @@ private:
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
-		{this, ORB_ID(differential_pressure), 0},
+		{this, ORB_ID(sensor_differential_pressure), 0},
-		{this, ORB_ID(differential_pressure), 1},
+		{this, ORB_ID(sensor_differential_pressure), 1},
-		{this, ORB_ID(differential_pressure), 2},
+		{this, ORB_ID(sensor_differential_pressure), 2},
-		{this, ORB_ID(differential_pressure), 3},
+		{this, ORB_ID(sensor_differential_pressure), 3},
 	};
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -105,11 +105,11 @@ private:
 	uint32_t _device_id[MAX_SENSOR_COUNT] {};
 	uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
 	hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
-	float _differential_pressure_sum[MAX_SENSOR_COUNT] {};
+	float _sensor_differential_pressure_sum[MAX_SENSOR_COUNT] {};
 	float _temperature_sum[MAX_SENSOR_COUNT] {};
 	int _sum_count[MAX_SENSOR_COUNT] {};
 
-	differential_pressure_s _last_data[MAX_SENSOR_COUNT] {};
+	sensor_differential_pressure_s _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
 
 	uint8_t _priority[MAX_SENSOR_COUNT] {100, 100, 100, 100};

src/modules/sensors/sensors.cpp

@@ -42,7 +42,6 @@
  */
 
 #include <drivers/drv_adc.h>
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
@@ -59,13 +58,12 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/vehicle_air_data.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include "voted_sensors_update.h"
 #include "airspeed/Airspeed.hpp"
@@ -82,7 +80,7 @@ using namespace time_literals;
 class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	explicit Sensors(bool hil_enabled);
+	explicit Sensors();
 	~Sensors() override;
 
 	/** @see ModuleBase */
@@ -103,8 +101,8 @@ public:
 	bool init();
 
 private:
-	const bool	_hil_enabled;			/**< if true, HIL is active */
 	bool _armed{false};				/**< arming status of the vehicle */
+	bool _hil_enabled{false};			/**< if true, HIL is active */
 
 	hrt_abstime     _last_config_update{0};
 	hrt_abstime     _sensor_combined_prev_timestamp{0};
@@ -120,38 +118,17 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
-
+	uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
-	hrt_abstime	_last_adc{0};			/**< last time we took input from the ADC */
+	uORB::PublicationMulti<sensor_differential_pressure_s> _diff_pres_pub{ORB_ID(sensor_differential_pressure)};		/**< differential_pressure */
-
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};		/**< adc_report sub */
-	differential_pressure_s	_diff_pres {};
-	uORB::PublicationMulti<differential_pressure_s>	_diff_pres_pub{ORB_ID(differential_pressure)};		/**< differential_pressure */
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-
-	struct Parameters {
-		float diff_pres_offset_pa;
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-		float diff_pres_analog_scale;
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	} _parameters{}; /**< local copies of interesting parameters */
-
-	struct ParameterHandles {
-		param_t diff_pres_offset_pa;
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-		param_t diff_pres_analog_scale;
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	} _parameter_handles{};		/**< handles for interesting parameters */
-
 	VotedSensorsUpdate _voted_sensors_update;
 
 	VehicleAcceleration	_vehicle_acceleration;
@@ -164,11 +141,6 @@ private:
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
-	/**
-	 * Update our local parameter cache.
-	 */
-	int		parameters_update();
-
 	/**
 	 * Check for changes in parameters.
 	 */
@@ -189,25 +161,22 @@ private:
 	void		InitializeVehicleMagnetometer();
 
 	DEFINE_PARAMETERS(
+#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
+		(ParamFloat<px4::params::SENS_DPRES_ANSC>) _param_sens_dpres_ansc,
+#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
+
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
 		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
 };
 
-Sensors::Sensors(bool hil_enabled) :
+Sensors::Sensors() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_hil_enabled(hil_enabled),
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
-	_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
+	_voted_sensors_update(_vehicle_imu_sub)
 {
-	/* 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 */
-
 	param_find("SYS_FAC_CAL_MODE");
 
 	// Parameters controlling the on-board sensor thermal calibrator
@@ -266,25 +235,7 @@ bool Sensors::init()
 	return true;
 }
 
-int Sensors::parameters_update()
+void Sensors::parameter_update_poll(bool forced)
-{
-	if (_armed) {
-		return 0;
-	}
-
-	/* Airspeed offset */
-	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-	param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	_voted_sensors_update.parametersUpdate();
-
-	return PX4_OK;
-}
-
-void
-Sensors::parameter_update_poll(bool forced)
 {
 	// check for parameter updates
 	if (_parameter_update_sub.updated() || forced) {
@@ -293,8 +244,11 @@ Sensors::parameter_update_poll(bool forced)
 		_parameter_update_sub.copy(&pupdate);
 
 		// update parameters from storage
-		parameters_update();
 		updateParams();
+
+		if (!_armed) {
+			_voted_sensors_update.parametersUpdate();
+		}
 	}
 }
 
@@ -305,25 +259,15 @@ void Sensors::adc_poll()
 		return;
 	}
 
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
 
-	if (_parameters.diff_pres_analog_scale > 0.0f) {
+	if (_param_sens_dpres_ansc.get() > 0.0f) {
-
-		const hrt_abstime t = hrt_absolute_time();
-
-		/* rate limit to 100 Hz */
-		if (t - _last_adc >= 10000) {
 		adc_report_s adc;
 
 		if (_adc_report_sub.update(&adc)) {
 			/* Read add channels we got */
 			for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
-					if (adc.channel_id[i] == -1) {
-						continue;	// skip non-exist channels
-					}
-
 				if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
-
 					/* calculate airspeed, raw is the difference from */
 					const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
 
@@ -336,23 +280,16 @@ void Sensors::adc_poll()
 					 * vref. Those devices require no divider at all.
 					 */
 					if (voltage > 0.4f) {
-							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
+						sensor_differential_pressure_s diff_pres{};
-
+						diff_pres.timestamp_sample = adc.timestamp;
-							_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
+						diff_pres.differential_pressure_pa = voltage * _param_sens_dpres_ansc.get();
-							_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
+						diff_pres.temperature = NAN;
-									(diff_pres_pa_raw * 0.1f);
+						diff_pres.timestamp = hrt_absolute_time();
-							_diff_pres.temperature = NAN;
+						_diff_pres_pub.publish(diff_pres);
-
-							_diff_pres.timestamp = hrt_absolute_time();
-
-							_diff_pres_pub.publish(_diff_pres);
 					}
 				}
 			}
 		}
-
-			_last_adc = t;
-		}
 	}
 
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@@ -361,7 +298,7 @@ void Sensors::adc_poll()
 void Sensors::InitializeAirspeed()
 {
 	if (_airspeed == nullptr) {
-		if (orb_exists(ORB_ID(differential_pressure), 0) == PX4_OK) {
+		if (orb_exists(ORB_ID(sensor_differential_pressure), 0) == PX4_OK) {
 			_airspeed = new Airspeed();
 
 			if (_airspeed) {
@@ -483,11 +420,18 @@ void Sensors::Run()
 	ScheduleDelayed(10_ms);
 
 	// check vehicle status for changes to publication state
-	if (_vcontrol_mode_sub.updated()) {
+	if (_vehicle_status_sub.updated()) {
-		vehicle_control_mode_s vcontrol_mode{};
+		vehicle_status_s vehicle_status;
 
-		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = vcontrol_mode.flag_armed;
+			const bool hil_prev = _hil_enabled;
+
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_hil_enabled = (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+			if (!hil_prev && _hil_enabled) {
+				_voted_sensors_update.set_hil_enabled();
+			}
 		}
 	}
 
@@ -524,35 +468,7 @@ void Sensors::Run()
 
 int Sensors::task_spawn(int argc, char *argv[])
 {
-	bool hil_enabled = false;
+	Sensors *instance = new Sensors();
-	bool error_flag = false;
-
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-
-	while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'h':
-			hil_enabled = true;
-			break;
-
-		case '?':
-			error_flag = true;
-			break;
-
-		default:
-			PX4_WARN("unrecognized flag");
-			error_flag = true;
-			break;
-		}
-	}
-
-	if (error_flag) {
-		return PX4_ERROR;
-	}
-
-	Sensors *instance = new Sensors(hil_enabled);
 
 	if (instance) {
 		_object.store(instance);
@@ -649,7 +565,6 @@ It runs in its own thread and polls on the currently selected gyro topic.
 
 	PRINT_MODULE_USAGE_NAME("sensors", "system");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/sensors/voted_sensors_update.cpp

@@ -46,18 +46,11 @@
 
 using namespace sensors;
 using namespace matrix;
-using namespace time_literals;
 
-VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
+VotedSensorsUpdate::VotedSensorsUpdate(uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) :
-				       uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) :
 	ModuleParams(nullptr),
-	_vehicle_imu_sub(vehicle_imu_sub),
+	_vehicle_imu_sub(vehicle_imu_sub)
-	_hil_enabled(hil_enabled)
 {
-	if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
-		_gyro.voter.set_timeout(500000);
-		_accel.voter.set_timeout(500000);
-	}
 }
 
 int VotedSensorsUpdate::init(sensor_combined_s &raw)
@@ -259,7 +252,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 
 bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name)
 {
-	if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
+	if (sensor.last_failover_count != sensor.voter.failover_count()) {
 
 		uint32_t flags = sensor.voter.failover_state();
 		int failover_index = sensor.voter.failover_index();

src/modules/sensors/voted_sensors_update.h

@@ -58,6 +58,8 @@
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_imu_status.h>
 
+using namespace time_literals;
+
 namespace sensors
 {
 
@@ -75,7 +77,7 @@ public:
 	 * @param parameters parameter values. These do not have to be initialized when constructing this object.
 	 * Only when calling init(), they have to be initialized.
 	 */
-	VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
+	VotedSensorsUpdate(uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
 
 	/**
 	 * initialize subscriptions etc.
@@ -107,6 +109,13 @@ public:
 	 */
 	void setRelativeTimestamps(sensor_combined_s &raw);
 
+	void set_hil_enabled()
+	{
+		// HIL has less accurate timing so increase the timeouts a bit
+		_gyro.voter.set_timeout(1_s);
+		_accel.voter.set_timeout(1_s);
+	}
+
 private:
 
 	static constexpr uint8_t DEFAULT_PRIORITY = 50;
@@ -167,8 +176,6 @@ private:
 
 	sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {};	/**< latest sensor data from all sensors instances */
 
-	const bool _hil_enabled{false};			/**< is hardware-in-the-loop mode enabled? */
-
 	bool _selection_changed{true};			/**< true when a sensor selection has changed and not been published */
 
 	matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {};		/**< filtered accel differences between IMU units (m/s/s) */

src/modules/simulator/simulator.h

@@ -58,7 +58,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -189,7 +189,7 @@ private:
 	perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")};
 
 	// uORB publisher handlers
-	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<sensor_differential_pressure_s>	_differential_pressure_pub{ORB_ID(sensor_differential_pressure)};
 	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};

src/modules/simulator/simulator_mavlink.cpp

@@ -265,12 +265,11 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 
 	// differential pressure
 	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
-		differential_pressure_s report{};
+		sensor_differential_pressure_s report{};
-		report.timestamp = time;
+		report.timestamp_sample = time;
 		report.temperature = _sensors_temperature;
-		report.differential_pressure_filtered_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.differential_pressure_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar;
-		report.differential_pressure_raw_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.timestamp = hrt_absolute_time();
-
 		_differential_pressure_pub.publish(report);
 	}
 }