sensors: add baro calibration and cleanup

- sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting

src/modules/commander/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	SRCS
 		accelerometer_calibration.cpp
 		airspeed_calibration.cpp
+		baro_calibration.cpp
 		calibration_routines.cpp
 		Commander.cpp
 		commander_helper.cpp

src/modules/commander/Commander.cpp

@@ -231,6 +231,10 @@ int Commander::custom_command(int argc, char *argv[])
 					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);
 				}
 
+			} else if (!strcmp(argv[1], "baro")) {
+				// baro calibration: param3 = 1
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 1.f, 0.f, 0.0, 0.0, 0.f);
+
 			} else if (!strcmp(argv[1], "accel")) {
 				if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
 					// accelerometer quick calibration: param5 = 3
@@ -1392,8 +1396,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					_worker_thread.startTask(WorkerThread::Request::MagCalibration);
 
 				} else if ((int)(cmd.param3) == 1) {
-					/* zero-altitude pressure calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+					/* baro calibration */
+					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					_status_flags.calibration_enabled = true;
+					_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
@@ -4421,7 +4427,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true);
 #ifndef CONSTRAINED_FLASH
 	PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration");
-	PRINT_MODULE_USAGE_ARG("mag|accel|gyro|level|esc|airspeed", "Calibration type", false);
+	PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
 	PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
 	PRINT_MODULE_USAGE_COMMAND("arm");

src/modules/commander/baro_calibration.cpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 baro_calibration.cpp
+ *
+ * Simple barometer calibration routine
+ */
+
+#include "baro_calibration.h"
+
+#include "commander_helper.h"
+#include "calibration_routines.h"
+#include "calibration_messages.h"
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+#include <drivers/drv_hrt.h>
+#include <matrix/math.hpp>
+#include <lib/geo/geo.h>
+#include <lib/sensor_calibration/Barometer.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <lib/systemlib/err.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gps.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr char sensor_name[] {"baro"};
+
+static constexpr int MAX_SENSOR_COUNT = 4;
+
+static float PressureToAltitude(float pressure_pa, float temperature)
+{
+	// calculate altitude using the hypsometric equation
+	static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
+	static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
+
+	// current pressure at MSL in kPa (QNH in hPa)
+	const float p1 = 1013.25f * 0.1f;
+
+	// measured pressure in kPa
+	const float p = pressure_pa * 0.001f;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
+
+	return altitude;
+}
+
+int do_baro_calibration(orb_advert_t *mavlink_log_pub)
+{
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
+
+	// GPS (used for reference)
+	uORB::SubscriptionMultiArray<sensor_gps_s, 3> sensor_gps_subs{ORB_ID::sensor_gps};
+	float gps_altitude_sum = NAN;
+	int gps_altitude_sum_count = 0;
+
+
+	uORB::SubscriptionMultiArray<sensor_baro_s, MAX_SENSOR_COUNT> sensor_baro_subs{ORB_ID::sensor_baro};
+	calibration::Barometer calibration[MAX_SENSOR_COUNT] {};
+
+	uint64_t timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
+	float data_sum[MAX_SENSOR_COUNT] {};
+	float temperature_sum[MAX_SENSOR_COUNT] {};
+	int data_sum_count[MAX_SENSOR_COUNT] {};
+
+	const hrt_abstime time_start_us = hrt_absolute_time();
+
+	while (hrt_elapsed_time(&time_start_us) < 3_s) {
+
+		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+			sensor_baro_s sensor_baro;
+
+			while (sensor_baro_subs[instance].update(&sensor_baro)) {
+				calibration[instance].set_device_id(sensor_baro.device_id);
+
+				// pressure corrected with offset (if available)
+				const float pressure_corrected = calibration[instance].Correct(sensor_baro.pressure);
+
+				timestamp_sample_sum[instance] += sensor_baro.timestamp_sample;
+				data_sum[instance] += pressure_corrected;
+				temperature_sum[instance] += sensor_baro.temperature;
+				data_sum_count[instance]++;
+			}
+		}
+
+		for (auto &gps_sub : sensor_gps_subs) {
+			sensor_gps_s sensor_gps;
+
+			if (gps_sub.update(&sensor_gps)) {
+				if ((hrt_elapsed_time(&sensor_gps.timestamp) < 1_s)
+				    && (sensor_gps.fix_type >= 2) && (sensor_gps.epv < 100)) {
+
+					float alt = sensor_gps.alt * 0.001f;
+
+					if (PX4_ISFINITE(gps_altitude_sum)) {
+						gps_altitude_sum += alt;
+						gps_altitude_sum_count++;
+
+					} else {
+						gps_altitude_sum = alt;
+						gps_altitude_sum_count = 1;
+					}
+				}
+			}
+		}
+
+		px4_usleep(100_ms);
+	}
+
+	float gps_altitude = NAN;
+
+	if (PX4_ISFINITE(gps_altitude_sum) && (gps_altitude_sum_count > 0)) {
+		gps_altitude = gps_altitude_sum / gps_altitude_sum_count;
+	}
+
+	if (!PX4_ISFINITE(gps_altitude)) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "GPS required for baro cal");
+		return PX4_ERROR;
+	}
+
+	bool param_save = false;
+
+	for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+		if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
+
+			const float pressure_pa = data_sum[instance] / data_sum_count[instance];
+			const float temperature = temperature_sum[instance] / data_sum_count[instance];
+
+			float pressure_altitude = PressureToAltitude(pressure_pa, temperature);
+
+			// Use GPS altitude as a reference to compute the baro bias measurement
+			const float baro_bias = pressure_altitude - gps_altitude;
+
+			float altitude = pressure_altitude - baro_bias;
+
+			// find pressure offset that aligns baro altitude with GPS via binary search
+			float front = -10000.f;
+			float middle = NAN;
+			float last = 10000.f;
+
+			float bias = NAN;
+
+			// perform a binary search
+			while (front <= last) {
+				middle = front + (last - front) / 2;
+				float altitude_calibrated = PressureToAltitude(pressure_pa - middle, temperature);
+
+				if (altitude_calibrated > altitude + 0.1f) {
+					last = middle;
+
+				} else if (altitude_calibrated < altitude - 0.1f) {
+					front = middle;
+
+				} else {
+					bias = middle;
+					break;
+				}
+			}
+
+			if (PX4_ISFINITE(bias)) {
+				float offset = calibration[instance].BiasCorrectedSensorOffset(bias);
+
+				calibration[instance].set_offset(offset);
+
+				if (calibration[instance].ParametersSave(instance, true)) {
+					calibration[instance].PrintStatus();
+					param_save = true;
+				}
+			}
+		}
+	}
+
+	if (param_save) {
+		param_notify_changes();
+	}
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+	return PX4_OK;
+}

src/modules/commander/baro_calibration.h

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <math.h>
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_baro_calibration(orb_advert_t *mavlink_log_pub);

src/modules/commander/worker_thread.cpp

@@ -35,6 +35,7 @@
 #include "accelerometer_calibration.h"
 #include "airspeed_calibration.h"
 #include "calibration_routines.h"
+#include "baro_calibration.h"
 #include "esc_calibration.h"
 #include "gyro_calibration.h"
 #include "level_calibration.h"
@@ -144,6 +145,10 @@ void WorkerThread::threadEntry()
 		_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
 		break;
 
+	case Request::BaroCalibration:
+		_ret_value = do_baro_calibration(&_mavlink_log_pub);
+		break;
+
 	case Request::ParamLoadDefault:
 		_ret_value = param_load_default();
 

src/modules/commander/worker_thread.hpp

@@ -58,6 +58,7 @@ public:
 		AirspeedCalibration,
 		ESCCalibration,
 		MagCalibrationQuick,
+		BaroCalibration,
 
 		ParamLoadDefault,
 		ParamSaveDefault,

src/modules/ekf2/EKF2.cpp

@@ -1511,6 +1511,28 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 			perf_count(_msg_missed_air_data_perf);
 		}
 
+		bool reset = false;
+
+		// check if barometer has changed
+		if (airdata.baro_device_id != _device_id_baro) {
+			if (_device_id_baro != 0) {
+				PX4_WARN("%d - baro sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_baro, airdata.baro_device_id);
+			}
+
+			reset = true;
+
+		} else if (airdata.calibration_count > _baro_calibration_count) {
+			// existing calibration has changed, reset saved baro bias
+			PX4_DEBUG("%d - baro %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_baro);
+			reset = true;
+		}
+
+		if (reset) {
+			// TODO: reset baro ref and bias estimate?
+			_device_id_baro = airdata.baro_device_id;
+			_baro_calibration_count = airdata.calibration_count;
+		}
+
 		_ekf.set_air_density(airdata.rho);
 
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});

src/modules/ekf2/EKF2.hpp

@@ -219,6 +219,7 @@ private:
 	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
 
 	uint8_t _accel_calibration_count{0};
+	uint8_t _baro_calibration_count{0};
 	uint8_t _gyro_calibration_count{0};
 	uint8_t _mag_calibration_count{0};
 

src/modules/mavlink/CMakeLists.txt

@@ -101,7 +101,6 @@ px4_add_module(
 		airspeed
 		component_general_json # for checksums.h
 		drivers_accelerometer
-		drivers_barometer
 		drivers_gyroscope
 		drivers_magnetometer
 		conversion

src/modules/mavlink/mavlink_receiver.cpp

@@ -73,7 +73,6 @@ MavlinkReceiver::~MavlinkReceiver()
 {
 	delete _tune_publisher;
 	delete _px4_accel;
-	delete _px4_baro;
 	delete _px4_gyro;
 	delete _px4_mag;
 #if !defined(CONSTRAINED_FLASH)
@@ -2309,15 +2308,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	// baro
 	if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
-		if (_px4_baro == nullptr) {
-			// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
-			_px4_baro = new PX4Barometer(6620172);
-		}
-
-		if (_px4_baro != nullptr) {
-			_px4_baro->set_temperature(hil_sensor.temperature);
-			_px4_baro->update(timestamp, hil_sensor.abs_pressure);
-		}
+		// publish
+		sensor_baro_s sensor_baro{};
+		sensor_baro.timestamp_sample = timestamp;
+		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.pressure = hil_sensor.abs_pressure;
+		sensor_baro.temperature = hil_sensor.temperature;
+		sensor_baro.error_count = 0;
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pub.publish(sensor_baro);
 	}
 
 	// differential pressure

src/modules/mavlink/mavlink_receiver.h

@@ -51,7 +51,6 @@
 
 #include <geo/geo.h>
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/systemlib/mavlink_log.h>
@@ -92,6 +91,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
@@ -290,7 +290,7 @@ private:
 	uORB::Publication<actuator_controls_s>			_actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
-	uORB::Publication<camera_status_s>					_camera_status_pub{ORB_ID(camera_status)};
+	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_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)};
@@ -308,7 +308,6 @@ private:
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
-	uORB::Publication<sensor_gps_s>				_sensor_gps_pub{ORB_ID(sensor_gps)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
@@ -333,9 +332,11 @@ private:
 	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::PublicationMulti<distance_sensor_s>		_flow_distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::PublicationMulti<input_rc_s>			_rc_pub{ORB_ID(input_rc)};
-	uORB::PublicationMulti<manual_control_setpoint_s>		_manual_control_input_pub{ORB_ID(manual_control_input)};
+	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_input_pub{ORB_ID(manual_control_input)};
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping)};
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
+	uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
 
 	// ORB publications (queue length > 1)
 	uORB::Publication<gps_inject_data_s>     _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@@ -364,7 +365,6 @@ private:
 		DIFF_PRESS	= 0b10000000000
 	};
 	PX4Accelerometer *_px4_accel{nullptr};
-	PX4Barometer *_px4_baro{nullptr};
 	PX4Gyroscope *_px4_gyro{nullptr};
 	PX4Magnetometer *_px4_mag{nullptr};
 

src/modules/sensors/data_validator/DataValidator.cpp

@@ -51,7 +51,6 @@ void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in,
 
 void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t priority_in)
 {
-
 	_event_count++;
 
 	if (error_count_in > _error_count) {
@@ -65,31 +64,33 @@ void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_
 	_priority = priority_in;
 
 	for (unsigned i = 0; i < dimensions; i++) {
-		if (_time_last == 0) {
-			_mean[i] = 0;
-			_lp[i] = val[i];
-			_M2[i] = 0;
-
-		} else {
-			float lp_val = val[i] - _lp[i];
-
-			float delta_val = lp_val - _mean[i];
-			_mean[i] += delta_val / _event_count;
-			_M2[i] += delta_val * (lp_val - _mean[i]);
-			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
-
-			if (fabsf(_value[i] - val[i]) < 0.000001f) {
-				_value_equal_count++;
+		if (PX4_ISFINITE(val[i])) {
+			if (_time_last == 0) {
+				_mean[i] = 0;
+				_lp[i] = val[i];
+				_M2[i] = 0;
 
 			} else {
-				_value_equal_count = 0;
+				float lp_val = val[i] - _lp[i];
+
+				float delta_val = lp_val - _mean[i];
+				_mean[i] += delta_val / _event_count;
+				_M2[i] += delta_val * (lp_val - _mean[i]);
+				_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+
+				if (fabsf(_value[i] - val[i]) < 0.000001f) {
+					_value_equal_count++;
+
+				} else {
+					_value_equal_count = 0;
+				}
 			}
+
+			// XXX replace with better filter, make it auto-tune to update rate
+			_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
+
+			_value[i] = val[i];
 		}
-
-		// XXX replace with better filter, make it auto-tune to update rate
-		_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
-
-		_value[i] = val[i];
 	}
 
 	_time_last = timestamp;

src/modules/sensors/module.yaml

@@ -159,6 +159,45 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
+        # Barometer calibration
+        CAL_BARO${i}_ID:
+            description:
+                short: Barometer ${i} calibration device ID
+                long: Device ID of the barometer this calibration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        CAL_BARO${i}_PRIO:
+            description:
+                short: Barometer ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        CAL_BARO${i}_OFF:
+            description:
+                short: Barometer ${i} offset
+            category: System
+            type: float
+            default: 0.0
+            #unit: Pa
+            volatile: true
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
         # Gyroscope calibration
         CAL_GYRO${i}_ID:
             description:

src/modules/sensors/vehicle_air_data/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-2022 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
@@ -35,4 +35,8 @@ px4_add_library(vehicle_air_data
 	VehicleAirData.cpp
 	VehicleAirData.hpp
 )
-target_link_libraries(vehicle_air_data PRIVATE px4_work_queue)
+target_link_libraries(vehicle_air_data
+	PRIVATE
+		px4_work_queue
+		sensor_calibration
+)

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -75,44 +75,6 @@ void VehicleAirData::Stop()
 	}
 }
 
-void VehicleAirData::SensorCorrectionsUpdate(bool force)
-{
-	if (_sensor_correction_sub.updated() || force) {
-		sensor_correction_s corrections;
-
-		if (_sensor_correction_sub.copy(&corrections)) {
-			for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
-				// find sensor (by device id) in sensor_correction
-				const uint32_t device_id = _last_data[baro_index].device_id;
-
-				if (device_id != 0) {
-					for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
-						if (corrections.baro_device_ids[correction_index] == device_id) {
-							switch (correction_index) {
-							case 0:
-								_thermal_offset[baro_index] = corrections.baro_offset_0;
-								break;
-
-							case 1:
-								_thermal_offset[baro_index] = corrections.baro_offset_1;
-								break;
-
-							case 2:
-								_thermal_offset[baro_index] = corrections.baro_offset_2;
-								break;
-
-							case 3:
-								_thermal_offset[baro_index] = corrections.baro_offset_3;
-								break;
-							}
-						}
-					}
-				}
-			}
-		}
-	}
-}
-
 void VehicleAirData::AirTemperatureUpdate()
 {
 	differential_pressure_s differential_pressure;
@@ -124,20 +86,43 @@ void VehicleAirData::AirTemperatureUpdate()
 	// limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight
 	if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature)
 	    && fabsf(differential_pressure.temperature) > FLT_EPSILON) {
+
 		_air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius,
 					   temperature_max_celsius);
 	}
 }
 
-bool VehicleAirData::ParametersUpdate()
+bool VehicleAirData::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
+	if (_parameter_update_sub.updated() || force) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
+
+		// update priority
+		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+
+			const int32_t priority_old = _calibration[instance].priority();
+
+			_calibration[instance].ParametersUpdate();
+
+			const int32_t priority_new = _calibration[instance].priority();
+
+			if (priority_old != priority_new) {
+				if (_priority[instance] == priority_old) {
+					_priority[instance] = priority_new;
+
+				} else {
+					// change relative priority to incorporate any sensor faults
+					int priority_change = priority_new - priority_old;
+					_priority[instance] = math::constrain(_priority[instance] + priority_change, 1, 100);
+				}
+			}
+		}
+
 		return true;
 	}
 
@@ -152,53 +137,71 @@ void VehicleAirData::Run()
 
 	const bool parameter_update = ParametersUpdate();
 
-	SensorCorrectionsUpdate();
-
 	AirTemperatureUpdate();
 
 	bool updated[MAX_SENSOR_COUNT] {};
 
 	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
 
+		const bool was_advertised = _advertised[uorb_index];
+
 		if (!_advertised[uorb_index]) {
 			// use data's timestamp to throttle advertisement checks
-			if ((_last_data[uorb_index].timestamp == 0) || (time_now_us > _last_data[uorb_index].timestamp + 1_s)) {
-				if (_sensor_sub[uorb_index].advertised()) {
-					if (uorb_index > 0) {
-						/* the first always exists, but for each further sensor, add a new validator */
-						if (!_voter.add_new_validator()) {
-							PX4_ERR("failed to add validator for %s %i", "BARO", uorb_index);
-						}
-					}
+			if ((_last_publication_timestamp[uorb_index] == 0)
+			    || (time_now_us > _last_publication_timestamp[uorb_index] + 1_s)) {
 
+				if (_sensor_sub[uorb_index].advertised()) {
 					_advertised[uorb_index] = true;
 
-					// force temperature correction update
-					SensorCorrectionsUpdate(true);
-
-					if (_selected_sensor_sub_index < 0) {
-						_sensor_sub[uorb_index].registerCallback();
-					}
-
 				} else {
-					_last_data[uorb_index].timestamp = time_now_us;
+					_last_publication_timestamp[uorb_index] = time_now_us;
 				}
 			}
 		}
 
 		if (_advertised[uorb_index]) {
-			updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
+			sensor_baro_s report;
 
-			if (updated[uorb_index]) {
-				// millibar to Pa
-				const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
+			while (_sensor_sub[uorb_index].update(&report)) {
 
-				// pressure corrected with offset (if available)
-				const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]);
+				if (_calibration[uorb_index].device_id() != report.device_id) {
+					_calibration[uorb_index].set_device_id(report.device_id);
+					_priority[uorb_index] = _calibration[uorb_index].priority();
+				}
 
-				float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f};
-				_voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count,
-					   _priority[uorb_index]);
+				if (_calibration[uorb_index].enabled()) {
+
+					if (!was_advertised) {
+						if (uorb_index > 0) {
+							/* the first always exists, but for each further sensor, add a new validator */
+							if (!_voter.add_new_validator()) {
+								PX4_ERR("failed to add validator for %s %i", _calibration[uorb_index].SensorString(), uorb_index);
+							}
+						}
+
+						if (_selected_sensor_sub_index < 0) {
+							_sensor_sub[uorb_index].registerCallback();
+						}
+
+						ParametersUpdate(true);
+					}
+
+					// pressure corrected with offset (if available)
+					_calibration[uorb_index].SensorCorrectionsUpdate();
+					const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure);
+
+					float data_array[3] {pressure_corrected, report.temperature, PressureToAltitude(pressure_corrected)};
+					_voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]);
+
+					_timestamp_sample_sum[uorb_index] += report.timestamp_sample;
+					_data_sum[uorb_index] += pressure_corrected;
+					_temperature_sum[uorb_index] += report.temperature;
+					_data_sum_count[uorb_index]++;
+
+					_last_data[uorb_index] = pressure_corrected;
+
+					updated[uorb_index] = true;
+				}
 			}
 		}
 	}
@@ -216,7 +219,8 @@ void VehicleAirData::Run()
 			}
 
 			if (_selected_sensor_sub_index >= 0) {
-				PX4_INFO("%s switch from #%" PRIu8 " -> #%d", "BARO", _selected_sensor_sub_index, best_index);
+				PX4_INFO("%s switch from #%" PRId8 " -> #%d", _calibration[_selected_sensor_sub_index].SensorString(),
+					 _selected_sensor_sub_index, best_index);
 			}
 
 			_selected_sensor_sub_index = best_index;
@@ -224,79 +228,112 @@ void VehicleAirData::Run()
 		}
 	}
 
-	if ((_selected_sensor_sub_index >= 0)
-	    && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
-	    && updated[_selected_sensor_sub_index]) {
+	// Publish
+	if (_param_sens_baro_rate.get() > 0) {
+		int interval_us = 1e6f / _param_sens_baro_rate.get();
 
-		const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
+		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+			if (updated[instance] && (_data_sum_count[instance] > 0)) {
 
-		_baro_timestamp_sum += baro.timestamp_sample;
-		_baro_sum += baro.pressure;
-		_baro_sum_count++;
+				const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance];
 
-		if ((_param_sens_baro_rate.get() > 0)
-		    && ((_last_publication_timestamp == 0)
-			|| (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) {
+				if (timestamp_sample >= _last_publication_timestamp[instance] + interval_us) {
 
-			const float pressure = _baro_sum / _baro_sum_count;
-			const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;
+					bool publish = (time_now_us <= timestamp_sample + 1_s);
 
-			// reset
-			_baro_timestamp_sum = 0;
-			_baro_sum = 0.f;
-			_baro_sum_count = 0;
+					if (publish) {
+						publish = (_selected_sensor_sub_index >= 0)
+							  && (instance == _selected_sensor_sub_index)
+							  && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
+					}
 
-			// populate vehicle_air_data with primary baro and publish
-			vehicle_air_data_s out{};
-			out.timestamp_sample = timestamp_sample;
-			out.baro_device_id = baro.device_id;
-			out.baro_temp_celcius = baro.temperature;
+					if (publish) {
+						const float pressure_pa = _data_sum[instance] / _data_sum_count[instance];
+						const float temperature = _temperature_sum[instance] / _data_sum_count[instance];
 
-			// Convert from millibar to Pa and apply temperature compensation
-			out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index];
+						float altitude = PressureToAltitude(pressure_pa, temperature);
 
-			// calculate altitude using the hypsometric equation
-			static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
-			static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre
+						// calculate air density
+						float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius -
+										   CONSTANTS_ABSOLUTE_NULL_CELSIUS));
 
-			// current pressure at MSL in kPa (QNH in hPa)
-			const float p1 = _param_sens_baro_qnh.get() * 0.1f;
+						// populate vehicle_air_data with and publish
+						vehicle_air_data_s out{};
+						out.timestamp_sample = timestamp_sample;
+						out.baro_device_id = _calibration[instance].device_id();
+						out.baro_alt_meter = altitude;
+						out.baro_temp_celcius = temperature;
+						out.baro_pressure_pa = pressure_pa;
+						out.rho = air_density;
+						out.calibration_count = _calibration[instance].calibration_count();
+						out.timestamp = hrt_absolute_time();
 
-			// measured pressure in kPa
-			const float p = out.baro_pressure_pa * 0.001f;
+						_vehicle_air_data_pub.publish(out);
+					}
 
-			/*
-			 * Solve:
-			 *
-			 *     /        -(aR / g)     \
-			 *    | (p / p1)          . T1 | - T1
-			 *     \                      /
-			 * h = -------------------------------  + h1
-			 *                   a
-			 */
-			out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
+					_last_publication_timestamp[instance] = timestamp_sample;
 
-			// calculate air density
-			out.rho = out.baro_pressure_pa  / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius -
-							   CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-
-			out.timestamp = hrt_absolute_time();
-			_vehicle_air_data_pub.publish(out);
-
-			_last_publication_timestamp = out.timestamp;
+					// reset
+					_timestamp_sample_sum[instance] = 0;
+					_data_sum[instance] = 0;
+					_temperature_sum[instance] = 0;
+					_data_sum_count[instance] = 0;
+				}
+			}
 		}
 	}
 
+	if (!parameter_update) {
+		CheckFailover(time_now_us);
+	}
+
+	UpdateStatus();
+
+	// reschedule timeout
+	ScheduleDelayed(50_ms);
+
+	perf_end(_cycle_perf);
+}
+
+float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const
+{
+	// calculate altitude using the hypsometric equation
+	static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
+	static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
+
+	// current pressure at MSL in kPa (QNH in hPa)
+	const float p1 = _param_sens_baro_qnh.get() * 0.1f;
+
+	// measured pressure in kPa
+	const float p = pressure_pa * 0.001f;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
+
+	return altitude;
+}
+
+void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us)
+{
 	// check failover and report (save failover report for a cycle where parameters didn't update)
-	if (_last_failover_count != _voter.failover_count() && !parameter_update) {
+	if (_last_failover_count != _voter.failover_count()) {
 		uint32_t flags = _voter.failover_state();
 		int failover_index = _voter.failover_index();
 
 		if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
-			if (failover_index != -1) {
+			if (failover_index >= 0 && failover_index < MAX_SENSOR_COUNT) {
+
 				if (time_now_us > _last_error_message + 3_s) {
 					mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t",
-							      "BARO",
+							      _calibration[failover_index].SensorString(),
 							      failover_index,
 							      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
 							      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
@@ -334,27 +371,65 @@ void VehicleAirData::Run()
 
 		_last_failover_count = _voter.failover_count();
 	}
+}
 
-	// reschedule timeout
-	ScheduleDelayed(50_ms);
+void VehicleAirData::UpdateStatus()
+{
+	if (_selected_sensor_sub_index >= 0) {
+		sensors_status_s sensors_status{};
+		sensors_status.device_id_primary = _calibration[_selected_sensor_sub_index].device_id();
 
-	perf_end(_cycle_perf);
+		float mean{};
+		int sensor_count = 0;
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if ((_calibration[sensor_index].device_id() != 0) && (_calibration[sensor_index].enabled())) {
+				sensor_count++;
+				mean += _last_data[sensor_index];
+			}
+		}
+
+		if (sensor_count > 0) {
+			mean /= sensor_count;
+		}
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if (_calibration[sensor_index].device_id() != 0) {
+
+				_sensor_diff[sensor_index] = 0.95f * _sensor_diff[sensor_index] + 0.05f * (_last_data[sensor_index] - mean);
+
+				sensors_status.device_ids[sensor_index] = _calibration[sensor_index].device_id();
+				sensors_status.inconsistency[sensor_index] = _sensor_diff[sensor_index];
+				sensors_status.healthy[sensor_index] = (_voter.get_sensor_state(sensor_index) == DataValidator::ERROR_FLAG_NO_ERROR);
+				sensors_status.priority[sensor_index] = _voter.get_sensor_priority(sensor_index);
+				sensors_status.enabled[sensor_index] = _calibration[sensor_index].enabled();
+				sensors_status.external[sensor_index] = _calibration[sensor_index].external();
+
+			} else {
+				sensors_status.inconsistency[sensor_index] = NAN;
+			}
+		}
+
+		sensors_status.timestamp = hrt_absolute_time();
+		_sensors_status_baro_pub.publish(sensors_status);
+	}
 }
 
 void VehicleAirData::PrintStatus()
 {
 	if (_selected_sensor_sub_index >= 0) {
-		PX4_INFO_RAW("[vehicle_air_data] selected barometer: %" PRIu32 " (%" PRId8 ")\n",
-			     _last_data[_selected_sensor_sub_index].device_id,
-			     _selected_sensor_sub_index);
-
-		if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) {
-			PX4_INFO_RAW("%" PRIu32 " temperature offset: %.4f\n", _last_data[_selected_sensor_sub_index].device_id,
-				     (double)_thermal_offset[_selected_sensor_sub_index]);
-		}
+		PX4_INFO_RAW("[vehicle_air_data] selected %s: %" PRIu32 " (%" PRId8 ")\n",
+			     _calibration[_selected_sensor_sub_index].SensorString(),
+			     _calibration[_selected_sensor_sub_index].device_id(), _selected_sensor_sub_index);
 	}
 
 	_voter.print();
+
+	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+		if (_advertised[i] && (_priority[i] > 0)) {
+			_calibration[i].PrintStatus();
+		}
+	}
 }
 
 }; // namespace sensors

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -35,6 +35,7 @@
 
 #include "data_validator/DataValidatorGroup.hpp"
 
+#include <lib/sensor_calibration/Barometer.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -44,12 +45,14 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensors_status.h>
 #include <uORB/topics/vehicle_air_data.h>
 
 using namespace time_literals;
@@ -71,17 +74,21 @@ public:
 private:
 	void Run() override;
 
-	bool ParametersUpdate();
-	void SensorCorrectionsUpdate(bool force = false);
 	void AirTemperatureUpdate();
+	void CheckFailover(const hrt_abstime &time_now_us);
+	bool ParametersUpdate(bool force = false);
+	void UpdateStatus();
+
+	float PressureToAltitude(float pressure_pa, float temperature = 15.f) const;
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
+	uORB::Publication<sensors_status_s> _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};
+
 	uORB::Publication<vehicle_air_data_s> _vehicle_air_data_pub{ORB_ID(vehicle_air_data)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 	uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@@ -91,23 +98,26 @@ private:
 		{this, ORB_ID(sensor_baro), 3},
 	};
 
+	calibration::Barometer _calibration[MAX_SENSOR_COUNT];
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
-	hrt_abstime _last_publication_timestamp{0};
 	hrt_abstime _last_error_message{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	DataValidatorGroup _voter{1};
 	unsigned _last_failover_count{0};
 
-	uint64_t _baro_timestamp_sum{0};
-	float _baro_sum{0.f};
-	int _baro_sum_count{0};
+	uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
+	float _data_sum[MAX_SENSOR_COUNT] {};
+	float _temperature_sum[MAX_SENSOR_COUNT] {};
+	int _data_sum_count[MAX_SENSOR_COUNT] {};
+	hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
 
-	sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
+	float _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
 
-	float _thermal_offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
+	float _sensor_diff[MAX_SENSOR_COUNT] {}; // filtered differences between sensor instances
 
 	uint8_t _priority[MAX_SENSOR_COUNT] {};
 

src/modules/sensors/vehicle_air_data/params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -38,9 +38,6 @@
  * @max 1500
  * @group Sensors
  * @unit hPa
- *
- * @reboot_required true
- *
  */
 PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
 
@@ -54,8 +51,5 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
  * @max 200
  * @group Sensors
  * @unit Hz
- *
- * @reboot_required true
- *
  */
 PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);

src/modules/sih/CMakeLists.txt

@@ -43,7 +43,6 @@ px4_add_module(
 	DEPENDS
 		mathlib
 		drivers_accelerometer
-		drivers_barometer
 		drivers_gyroscope
 		drivers_magnetometer
 		px4_work_queue

src/modules/sih/sih.cpp

@@ -152,8 +152,16 @@ void Sih::Run()
 	if (_now - _baro_time >= 50_ms
 	    && fabs(_baro_offset_m) < 10000) {
 		_baro_time = _now;
-		_px4_baro.set_temperature(_baro_temp_c);
-		_px4_baro.update(_now, _baro_p_mBar);
+
+		// publish
+		sensor_baro_s sensor_baro{};
+		sensor_baro.timestamp_sample = _now;
+		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.pressure = _baro_p_mBar * 100.f;
+		sensor_baro.temperature = _baro_temp_c;
+		sensor_baro.error_count = 0;
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pub.publish(sensor_baro);
 	}
 
 	// gps published at 20Hz

src/modules/sih/sih.hpp

@@ -65,7 +65,6 @@
 #include <lib/geo/geo.h>        // to get the physical constants
 #include <drivers/drv_hrt.h>        // to get the real time
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/perf/perf_counter.h>
@@ -75,6 +74,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_angular_velocity.h>   // to publish groundtruth
 #include <uORB/topics/vehicle_attitude.h>           // to publish groundtruth
 #include <uORB/topics/vehicle_global_position.h>    // to publish groundtruth
@@ -117,7 +117,8 @@ private:
 	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 	PX4Gyroscope     _px4_gyro{1310988};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 	PX4Magnetometer  _px4_mag{197388};    //  197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
-	PX4Barometer     _px4_baro{6620172};  // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	// to publish the gps position
 	sensor_gps_s			_sensor_gps{};

src/modules/simulator/CMakeLists.txt

@@ -65,7 +65,6 @@ px4_add_module(
 		conversion
 		geo
 		drivers_accelerometer
-		drivers_barometer
 		drivers_gyroscope
 		drivers_magnetometer
 	)

src/modules/simulator/sensor_baro_sim/CMakeLists.txt

@@ -39,7 +39,6 @@ px4_add_module(
 		SensorBaroSim.cpp
 		SensorBaroSim.hpp
 	DEPENDS
-		drivers_barometer
 		geo
 		px4_work_queue
 	)

src/modules/simulator/sensor_baro_sim/SensorBaroSim.cpp

@@ -161,8 +161,15 @@ void SensorBaroSim::Run()
 			// calculate temperature in Celsius
 			float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
 
-			_px4_baro.set_temperature(temperature);
-			_px4_baro.update(gpos.timestamp, pressure);
+			// publish
+			sensor_baro_s sensor_baro{};
+			sensor_baro.timestamp_sample = gpos.timestamp;
+			sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+			sensor_baro.pressure = pressure;
+			sensor_baro.temperature = temperature;
+			sensor_baro.timestamp = hrt_absolute_time();
+			_sensor_baro_pub.publish(sensor_baro);
+
 
 			_last_update_time = gpos.timestamp;
 		}

src/modules/simulator/sensor_baro_sim/SensorBaroSim.hpp

@@ -33,16 +33,17 @@
 
 #pragma once
 
-#include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_global_position.h>
 
 using namespace time_literals;
@@ -80,12 +81,12 @@ private:
 
 	hrt_abstime _last_update_time{0};
 
-	PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
-		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_t
+		(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
 	)
 };

src/modules/simulator/sensor_baro_sim/parameters.c

@@ -42,5 +42,6 @@ PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f);
  * simulated barometer temperature offset
  *
  * @group Simulator
+ * @unit celcius
  */
 PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f);

src/modules/simulator/simulator.h

@@ -44,7 +44,6 @@
 
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/geo/geo.h>
@@ -64,6 +63,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -179,8 +179,7 @@ private:
 	PX4Magnetometer		_px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 	PX4Magnetometer		_px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
 
-	PX4Barometer		_px4_baro_0{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
-	PX4Barometer		_px4_baro_1{6620428}; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pubs[2] {{ORB_ID(sensor_baro)}, {ORB_ID(sensor_baro)}};
 
 	float _sensors_temperature{0};
 
@@ -291,6 +290,9 @@ private:
 	float _last_magz{0.0f};
 	bool _use_dynamic_mixing{false};
 
+	float _last_baro_pressure{0.0f};
+	float _last_baro_temperature{0.0f};
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -326,18 +326,27 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 
 	// baro
 	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
-		if (_baro_stuck) {
-			_px4_baro_0.update(time, _px4_baro_0.get().pressure);
-			_px4_baro_0.set_temperature(_px4_baro_0.get().temperature);
-			_px4_baro_1.update(time, _px4_baro_1.get().pressure);
-			_px4_baro_1.set_temperature(_px4_baro_1.get().temperature);
 
-		} else {
-			_px4_baro_0.update(time, sensors.abs_pressure);
-			_px4_baro_0.set_temperature(sensors.temperature);
-			_px4_baro_1.update(time, sensors.abs_pressure);
-			_px4_baro_1.set_temperature(sensors.temperature);
+		if (!_baro_stuck) {
+			_last_baro_pressure = sensors.abs_pressure;
+			_last_baro_temperature = sensors.temperature;
 		}
+
+		// publish
+		sensor_baro_s sensor_baro{};
+		sensor_baro.timestamp_sample = time;
+		sensor_baro.pressure = _last_baro_pressure;
+		sensor_baro.temperature = _last_baro_temperature;
+
+		// publish 1st baro
+		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pubs[0].publish(sensor_baro);
+
+		// publish 2nd baro
+		sensor_baro.device_id = 6620428; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pubs[1].publish(sensor_baro);
 	}
 
 	// differential pressure