differential pressure remove filters from drivers and average in sensors/airspeed

msg/airspeed.msg

@@ -1,9 +1,10 @@
-uint64 timestamp			# time since system start (microseconds)
+uint64 timestamp                 # time since system start (microseconds)
+uint64 timestamp_sample
 
-float32 indicated_airspeed_m_s		# indicated airspeed in m/s
+float32 indicated_airspeed_m_s   # indicated airspeed in m/s
 
-float32 true_airspeed_m_s		# true filtered airspeed in m/s
+float32 true_airspeed_m_s        # true filtered airspeed in m/s
 
-float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
+float32 air_temperature_celsius  # air temperature in degrees celsius, -1000 if unknown
 
-float32 confidence			# confidence value from 0 to 1 for this sensor
+float32 confidence               # confidence value from 0 to 1 for this sensor

msg/differential_pressure.msg

@@ -1,6 +1,10 @@
-uint64 timestamp				# time since system start (microseconds)
+uint64 timestamp                     # time since system start (microseconds)
-uint64 error_count				# Number of errors detected by driver
+uint64 timestamp_sample
-float32 differential_pressure_raw_pa		# Raw differential pressure reading (may be negative)
+
-float32 differential_pressure_filtered_pa	# Low pass filtered differential pressure reading
+uint32 device_id                     # unique device ID for the sensor that does not change between power cycles
-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
+float32 differential_pressure_pa     # differential pressure reading in Pascals (may be negative)
+
+float32 temperature                  # Temperature provided by sensor in celcius, NAN if unknown
+
+uint32 error_count                   # Number of errors detected by driver

src/drivers/differential_pressure/ets/ets_airspeed.cpp

@@ -122,14 +122,12 @@ ETSAirspeed::measure()
 int
 ETSAirspeed::collect()
 {
-	int	ret = -EIO;
-
 	/* read from the sensor */
 	uint8_t val[2] = {0, 0};
 
 	perf_begin(_sample_perf);
-
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
-	ret = transfer(nullptr, 0, &val[0], 2);
+	int ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
 		perf_count(_comms_errors);
@@ -138,25 +136,22 @@ ETSAirspeed::collect()
 
 	float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]);
 
-	differential_pressure_s report{};
-	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_pres_pa_raw = 0.001f * (timestamp_sample & 0x01);
 	}
 
-	report.error_count = perf_event_count(_comms_errors);
+	differential_pressure_s report{};
-
+	report.timestamp_sample = timestamp_sample;
-	// 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 = -1000.0f;
 	report.device_id = _device_id.devid;
+	report.differential_pressure_pa = diff_pres_pa_raw;
+	report.temperature = NAN;
+	report.error_count = perf_event_count(_comms_errors);
+	report.timestamp = hrt_absolute_time();
 
 	_airspeed_pub.publish(report);
 

src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp

@@ -49,7 +49,6 @@
  *    - 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>
@@ -70,7 +69,6 @@ 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 */
 
 
@@ -91,8 +89,6 @@ protected:
 
 	int	measure() override;
 	int	collect() override;
-
-	math::LowPassFilter2p<float> _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
 };
 
 /*
@@ -136,10 +132,10 @@ int
 MEASAirspeed::collect()
 {
 	/* read from the sensor */
-	uint8_t val[4] = {0, 0, 0, 0};
-
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	uint8_t val[4] = {0, 0, 0, 0};
 	int ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
@@ -199,32 +195,25 @@ MEASAirspeed::collect()
 	  port on the pitot and top port is used as the dynamic port
 	 */
 	float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
-	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
+	float diff_press_pa = diff_press_PSI * PSI_to_Pa;
 
 	/*
 	  With the above calculation the MS4525 sensor will produce a
 	  positive number when the top port is used as a dynamic port
 	  and bottom port is used as the static port
 	 */
-
+	differential_pressure_s report;
-	if (PX4_ISFINITE(diff_press_pa_raw)) {
+	report.timestamp_sample = timestamp_sample;
-		differential_pressure_s report{};
+	report.device_id = get_device_id();
-
+	report.differential_pressure_pa = diff_press_pa;
-		report.error_count = perf_event_count(_comms_errors);
+	report.temperature = temperature;
-		report.temperature = temperature;
+	report.error_count = perf_event_count(_comms_errors);
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
+	report.timestamp = hrt_absolute_time();
-		report.differential_pressure_raw_pa = diff_press_pa_raw;
+	_airspeed_pub.publish(report);
-		report.device_id = _device_id.devid;
-		report.timestamp = hrt_absolute_time();
-
-		_airspeed_pub.publish(report);
-	}
-
-	ret = OK;
 
 	perf_end(_sample_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
 void

src/drivers/differential_pressure/ms5525/MS5525.cpp

@@ -184,6 +184,7 @@ MS5525::collect()
 
 	// read ADC
 	uint8_t cmd = CMD_ADC_READ;
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
 	int ret = transfer(&cmd, 1, nullptr, 0);
 
 	if (ret != PX4_OK) {
@@ -257,22 +258,18 @@ MS5525::collect()
 
 	// 1 PSI = 6894.76 Pascals
 	static constexpr float PSI_to_Pa = 6894.757f;
-	const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
+	const float diff_press_pa = diff_press_PSI * PSI_to_Pa;
 
 	const float temperature_c = TEMP * 0.01f;
 
-	if (PX4_ISFINITE(diff_press_pa_raw)) {
+	differential_pressure_s diff_pressure;
-		differential_pressure_s diff_pressure{};
+	diff_pressure.timestamp_sample = timestamp_sample;
-
+	diff_pressure.device_id = get_device_id();
-		diff_pressure.error_count = perf_event_count(_comms_errors);
+	diff_pressure.differential_pressure_pa = diff_press_pa;
-		diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw;
+	diff_pressure.temperature = temperature_c;
-		diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
+	diff_pressure.error_count = perf_event_count(_comms_errors);
-		diff_pressure.temperature = temperature_c;
+	diff_pressure.timestamp = hrt_absolute_time();
-		diff_pressure.device_id = _device_id.devid;
+	_airspeed_pub.publish(diff_pressure);
-		diff_pressure.timestamp = hrt_absolute_time();
-
-		_airspeed_pub.publish(diff_pressure);
-	}
 
 	ret = OK;
 

src/drivers/differential_pressure/ms5525/MS5525.hpp

@@ -35,7 +35,6 @@
 
 #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>
@@ -45,7 +44,6 @@ 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>
@@ -70,9 +68,6 @@ private:
 	int measure() override;
 	int collect() override;
 
-	// temperature is read once every 10 cycles
-	math::LowPassFilter2p<float> _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
-
 	static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
 	static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
 

src/drivers/differential_pressure/sdp3x/SDP3X.cpp

@@ -146,6 +146,8 @@ SDP3X::collect()
 {
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
 	// read 6 bytes from the sensor
 	uint8_t val[6];
 	int ret = transfer(nullptr, 0, &val[0], sizeof(val));
@@ -164,21 +166,17 @@ SDP3X::collect()
 	int16_t P = (((int16_t)val[0]) << 8) | val[1];
 	int16_t temp = (((int16_t)val[3]) << 8) | val[4];
 
-	float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
+	float diff_press_pa = static_cast<float>(P) / static_cast<float>(_scale);
 	float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
 
-	if (PX4_ISFINITE(diff_press_pa_raw)) {
+	differential_pressure_s report;
-		differential_pressure_s report{};
+	report.device_id = get_device_id();
-
+	report.timestamp_sample = timestamp_sample;
-		report.error_count = perf_event_count(_comms_errors);
+	report.differential_pressure_pa = diff_press_pa;
-		report.temperature = temperature_c;
+	report.temperature = temperature_c;
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
+	report.error_count = perf_event_count(_comms_errors);
-		report.differential_pressure_raw_pa = diff_press_pa_raw;
+	report.timestamp = hrt_absolute_time();
-		report.device_id = _device_id.devid;
+	_airspeed_pub.publish(report);
-		report.timestamp = hrt_absolute_time();
-
-		_airspeed_pub.publish(report);
-	}
 
 	perf_end(_sample_perf);
 

src/drivers/differential_pressure/sdp3x/SDP3X.hpp

@@ -43,7 +43,6 @@
 
 #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>
@@ -64,7 +63,6 @@
 
 // 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>
@@ -98,8 +96,6 @@ private:
 	int	configure();
 	int	read_scale();
 
-	math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
-
 	bool init_sdp3x();
 
 	/**

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -65,21 +65,20 @@ int UavcanDifferentialPressureBridge::init()
 void UavcanDifferentialPressureBridge::air_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg)
 {
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
 	_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;
 	float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
 
-	if (PX4_ISFINITE(diff_press_pa)) {
+	differential_pressure_s report{};
-		differential_pressure_s report{};
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id.devid;
+	report.differential_pressure_pa = diff_press_pa;
+	report.temperature = temperature_c;
+	report.timestamp = hrt_absolute_time();
 
-		report.differential_pressure_raw_pa = diff_press_pa;
+	publish(msg.getSrcNodeID().get(), &report);
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
-		report.temperature = temperature_c;
-		report.device_id = _device_id.devid;
-		report.timestamp = hrt_absolute_time();
-
-		publish(msg.getSrcNodeID().get(), &report);
-	}
 }

src/drivers/uavcan/sensors/differential_pressure.hpp

@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <uORB/topics/differential_pressure.h>
 
 #include "sensor_bridge.hpp"
@@ -56,8 +55,6 @@ public:
 	int init() override;
 
 private:
-	math::LowPassFilter2p<float> _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

@@ -76,7 +76,7 @@ public:
 			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/modules/commander/airspeed_calibration.cpp

@@ -76,38 +76,38 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	const unsigned calibration_count = (maxcount * 2) / 3;
 
-	uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
-
 	float diff_pres_offset = 0.0f;
 
 	if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) {
 		calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
 	px4_usleep(500 * 1000);
 
+	uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
+
 	while (calibration_counter < calibration_count) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 		differential_pressure_s diff_pres;
 
 		if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
 
-			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
+			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
 			/* any differential pressure failure a reason to abort */
 			if (diff_pres.error_count != 0) {
-				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 (%" PRIu32 ")",
 							 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) {
@@ -117,7 +117,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
-			goto error_return;
+			return PX4_ERROR;
 		}
 	}
 
@@ -134,12 +134,12 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 		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);
@@ -149,28 +149,35 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
 
+	float differential_pressure_sum = 0.f;
+	int differential_pressure_sum_count = 0;
+
 	calibration_counter = 0;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 		differential_pressure_s diff_pres;
 
 		if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
-			if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
+			differential_pressure_sum += diff_pres.differential_pressure_pa;
-				if (diff_pres.differential_pressure_filtered_pa > 0) {
+			differential_pressure_sum_count++;
-					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
+
-							     (int)diff_pres.differential_pressure_filtered_pa);
+			const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset;
+
+			if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) {
+				if (differential_pressure_pa > 0) {
+					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa);
 					break;
 
 				} else {
 					/* do not allow negative values */
 					calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
-								 (int)diff_pres.differential_pressure_filtered_pa);
+								 (int)differential_pressure_pa);
 					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 */
@@ -178,7 +185,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 					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 */
@@ -186,14 +193,18 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 					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)",
-						     (int)diff_pres.differential_pressure_filtered_pa);
+						     (int)differential_pressure_pa);
 				tune_neutral(true);
+
+				// reset average
+				differential_pressure_sum = 0.f;
+				differential_pressure_sum_count = 0;
 			}
 
 			calibration_counter++;
@@ -201,13 +212,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		} 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) {
 		feedback_calibration_failed(mavlink_log_pub);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
@@ -215,13 +226,8 @@ 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);
 
-normal_return:
 	// 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/ekf2/EKF2.cpp

@@ -1483,7 +1483,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			    && (airspeed.indicated_airspeed_m_s > 0.f)
 			   ) {
 				airspeedSample airspeed_sample {
-					.time_us = airspeed.timestamp,
+					.time_us = airspeed.timestamp_sample,
 					.true_airspeed = true_airspeed_m_s,
 					.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
 				};

src/modules/mavlink/mavlink_receiver.cpp

@@ -2322,11 +2322,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{};
-		report.timestamp = timestamp;
+		report.timestamp_sample = timestamp;
+		report.device_id = 0; // TODO
 		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/streams/HIGHRES_IMU.hpp

@@ -191,7 +191,7 @@ private:
 			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;

src/modules/mavlink/streams/SCALED_PRESSURE.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/mavlink/streams/SCALED_PRESSURE2.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/mavlink/streams/SCALED_PRESSURE3.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/sensors/sensors.cpp

@@ -139,13 +139,16 @@ private:
 
 	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
 
+	uint64_t _airspeed_last_publish{0};
+	uint64_t _diff_pres_timestamp_sum{0};
+	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};
+
 #ifdef 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<differential_pressure_s> _diff_pres_pub{ORB_ID(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 */
 
 
@@ -393,6 +396,18 @@ void Sensors::diff_pres_poll()
 
 	if (_diff_pres_sub.update(&diff_pres)) {
 
+		if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
+			// ignore invalid data and reset accumulated
+
+			// 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;
+		}
+
 		vehicle_air_data_s air_data{};
 		_vehicle_air_data_sub.copy(&air_data);
 
@@ -413,54 +428,71 @@ void Sensors::diff_pres_poll()
 			}
 		}
 
-		airspeed_s airspeed{};
+		// push raw data into validator
-		airspeed.timestamp = diff_pres.timestamp;
+		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?
 
-		// apply calibration offset (SENS_DPRES_OFF)
+		// accumulate average for publication
-		const float differential_pressure_raw_pa = diff_pres.differential_pressure_raw_pa - _parameters.diff_pres_offset_pa;
+		_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
-		const float differential_pressure_filtered_pa = diff_pres.differential_pressure_filtered_pa -
+		_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
-				_parameters.diff_pres_offset_pa;
+		_diff_pres_temperature_sum += air_temperature_celsius;
+		_baro_pressure_sum += air_data.baro_pressure_pa;
+		_diff_pres_count++;
 
-		/* push data into validator */
+		if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 100_ms) {
-		float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
 
-		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
+			// 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;
 
-		airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
+			// 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) {
+			enum AIRSPEED_SENSOR_MODEL smodel;
-		case DRV_DIFF_PRESS_DEVTYPE_SDP31:
 
-		/* fallthrough */
+			switch ((diff_pres.device_id >> 16) & 0xFF) {
-		case DRV_DIFF_PRESS_DEVTYPE_SDP32:
+			case DRV_DIFF_PRESS_DEVTYPE_SDP31:
 
-		/* fallthrough */
+			// fallthrough
-		case DRV_DIFF_PRESS_DEVTYPE_SDP33:
+			case DRV_DIFF_PRESS_DEVTYPE_SDP32:
-			/* fallthrough */
-			smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
-			break;
 
-		default:
+			// fallthrough
-			smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
+			case DRV_DIFF_PRESS_DEVTYPE_SDP33:
-			break;
+				smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
-		}
+				break;
 
-		/* don't risk to feed negative airspeed into the system */
+			default:
-		airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
+				smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
-						  _parameters.air_cmodel,
+				break;
-						  smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
+			}
-						  differential_pressure_filtered_pa, air_data.baro_pressure_pa,
-						  air_temperature_celsius);
 
-		airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
+			float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
-					     air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
+						       smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
+						       differential_pressure_pa, baro_pressure_pa, temperature);
 
-		airspeed.air_temperature_celsius = air_temperature_celsius;
+			// 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(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
+			if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
-			_airspeed_pub.publish(airspeed);
+
+				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);
+
+				_airspeed_last_publish = airspeed.timestamp;
+			}
 		}
 	}
 }
@@ -490,49 +522,41 @@ void Sensors::adc_poll()
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	if (_parameters.diff_pres_analog_scale > 0.0f) {
+		adc_report_s adc;
 
-		hrt_abstime t = hrt_absolute_time();
+		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
+				}
 
-		/* rate limit to 100 Hz */
+				if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
-		if (t - _last_adc >= 10000) {
-			adc_report_s adc;
 
-			if (_adc_report_sub.update(&adc)) {
+					/* calculate airspeed, raw is the difference from */
-				/* Read add channels we got */
+					const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
-				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]) {
+					/**
+					 * The voltage divider pulls the signal down, only act on
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
+					 *
+					 * Notice: This won't work on devices which have PGA controlled
+					 * vref. Those devices require no divider at all.
+					 */
+					if (voltage > 0.4f) {
+						const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
 
-						/* calculate airspeed, raw is the difference from */
+						differential_pressure_s diff_pres{};
-						const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
+						diff_pres.timestamp_sample = adc.timestamp;
+						diff_pres.differential_pressure_pa = diff_pres_pa_raw;
+						diff_pres.temperature = NAN;
+						diff_pres.timestamp = hrt_absolute_time();
 
-						/**
+						_diff_pres_pub.publish(diff_pres);
-						 * The voltage divider pulls the signal down, only act on
-						 * a valid voltage from a connected sensor. Also assume a non-
-						 * zero offset from the sensor if its connected.
-						 *
-						 * Notice: This won't work on devices which have PGA controlled
-						 * vref. Those devices require no divider at all.
-						 */
-						if (voltage > 0.4f) {
-							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
-
-							_diff_pres.timestamp = t;
-							_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
-							_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
-									(diff_pres_pa_raw * 0.1f);
-							_diff_pres.temperature = -1000.0f;
-
-							_diff_pres_pub.publish(_diff_pres);
-						}
 					}
 				}
 			}
-
-			_last_adc = t;
 		}
 	}
 

src/modules/simulator/simulator_mavlink.cpp

@@ -352,11 +352,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{};
-		report.timestamp = time;
+		report.timestamp_sample = time;
+		report.device_id = 0; // TODO
+		report.differential_pressure_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
 		report.temperature = _sensors_temperature;
-		report.differential_pressure_filtered_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
+		report.timestamp = hrt_absolute_time();
-		report.differential_pressure_raw_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
-
 		_differential_pressure_pub.publish(report);
 	}
 }