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/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);