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

.ci/Jenkinsfile-hardware

@@ -705,18 +705,21 @@ void quickCalibrate() {
 
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"'
 
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"'
 
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"'
 
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"'
 
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"'
 
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"'
+
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
 }

msg/CMakeLists.txt

@@ -152,6 +152,7 @@ set(msg_files
 	sensor_preflight_mag.msg
 	sensor_selection.msg
 	sensors_status_imu.msg
+	sensors_status.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg

msg/sensor_baro.msg

@@ -3,8 +3,10 @@ uint64 timestamp_sample
 
 uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
-uint32 error_count
+float32 pressure          # static pressure measurement in Pascals
+
+float32 temperature       # temperature in degrees Celsius
 
-float32 pressure	# static pressure measurement in millibar
+uint32 error_count
 
-float32 temperature	# static temperature measurement in deg Celsius
+uint8 ORB_QUEUE_LENGTH = 4

msg/sensors_status.msg

@@ -0,0 +1,15 @@
+#
+# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated.
+#
+uint64 timestamp # time since system start (microseconds)
+
+uint32 device_id_primary       # current primary device id for reference
+
+uint32[4] device_ids
+float32[4] inconsistency       # magnitude of difference between sensor instance and mean
+bool[4] healthy                # sensor healthy
+uint8[4] priority
+bool[4] enabled
+bool[4] external
+
+# TOPICS sensors_status sensors_status_baro sensors_status_mag

msg/vehicle_air_data.msg

@@ -10,3 +10,5 @@ float32 baro_temp_celcius		# Temperature in degrees Celsius
 float32 baro_pressure_pa		# Absolute pressure in Pascals
 
 float32 rho				# air density
+
+uint8 calibration_count     # Calibration changed counter. Monotonically increases whenever calibration changes.

src/drivers/barometer/bmp280/BMP280.cpp

@@ -35,7 +35,6 @@
 
 BMP280::BMP280(const I2CSPIDriverConfig &config, bmp280::IBMP280 *interface) :
 	I2CSPIDriver(config),
-	_px4_baro(interface->get_device_id()),
 	_interface(interface),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
 	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -172,11 +171,15 @@ BMP280::collect()
 	float pf = ((float) p_raw + x1) / x2;
 	const float P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7;
 
-	_px4_baro.set_error_count(perf_event_count(_comms_errors));
-	_px4_baro.set_temperature(T);
-
-	float pressure = P / 100.0f; // to mbar
-	_px4_baro.update(timestamp_sample, pressure);
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = P;
+	sensor_baro.temperature = T;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
 
 	perf_end(_sample_perf);
 

src/drivers/barometer/bmp280/BMP280.hpp

@@ -39,7 +39,8 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
 #include <lib/perf/perf_counter.h>
 
 class BMP280 : public I2CSPIDriver<BMP280>
@@ -61,7 +62,7 @@ class BMP280 : public I2CSPIDriver<BMP280>
 	int			measure(); //start measure
 	int			collect(); //get results and publish
 
-	PX4Barometer		_px4_baro;
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	bmp280::IBMP280		*_interface;
 

src/drivers/barometer/bmp280/CMakeLists.txt

@@ -42,6 +42,5 @@ px4_add_module(
 
 		bmp280_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/bmp388/CMakeLists.txt

@@ -40,6 +40,5 @@ px4_add_module(
 		bmp388.cpp
 		bmp388_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/bmp388/bmp388.cpp

@@ -43,7 +43,6 @@
 
 BMP388::BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface) :
 	I2CSPIDriver(config),
-	_px4_baro(interface->get_device_id()),
 	_interface(interface),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -165,14 +164,18 @@ BMP388::collect()
 		return -EIO;
 	}
 
-	_px4_baro.set_error_count(perf_event_count(_comms_errors));
-
 	float temperature = (float)(data.temperature / 100.0f);
 	float pressure = (float)(data.pressure / 100.0f); // to Pascal
-	pressure = pressure / 100.0f; // to mbar
 
-	_px4_baro.set_temperature(temperature);
-	_px4_baro.update(timestamp_sample, pressure);
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = pressure;
+	sensor_baro.temperature = temperature;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
 
 	perf_end(_sample_perf);
 

src/drivers/barometer/bmp388/bmp388.h

@@ -44,7 +44,8 @@
 #include <perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
 
 #include "board_config.h"
 
@@ -325,7 +326,7 @@ class BMP388 : public I2CSPIDriver<BMP388>
 	static constexpr uint8_t			odr{BMP3_ODR_50_HZ};			// output data rate (not used)
 	static constexpr uint8_t			iir_coef{BMP3_IIR_FILTER_DISABLE};	// IIR coefficient
 
-	PX4Barometer		_px4_baro;
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 	IBMP388			*_interface{nullptr};
 
 	unsigned		_measure_interval{0};			// interval in microseconds needed to measure

src/drivers/barometer/dps310/CMakeLists.txt

@@ -40,6 +40,5 @@ px4_add_module(
 		DPS310_SPI.cpp
 		dps310_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/dps310/DPS310.cpp

@@ -48,7 +48,6 @@ static void getTwosComplement(T &raw, uint8_t length)
 
 DPS310::DPS310(const I2CSPIDriverConfig &config, device::Device *interface) :
 	I2CSPIDriver(config),
-	_px4_barometer(interface->get_device_id()),
 	_interface(interface),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comm errors"))
@@ -233,9 +232,15 @@ DPS310::RunImpl()
 
 	const float Tcomp = c0 * 0.5f + c1 * Traw_sc;
 
-	_px4_barometer.set_error_count(perf_event_count(_comms_errors));
-	_px4_barometer.set_temperature(Tcomp);
-	_px4_barometer.update(timestamp_sample, Pcomp / 100.0f); // Pascals -> Millibar
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = Pcomp;
+	sensor_baro.temperature = Tcomp;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
 
 	perf_end(_sample_perf);
 }

src/drivers/barometer/dps310/DPS310.hpp

@@ -40,7 +40,8 @@
 #pragma once
 
 #include <drivers/device/Device.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -79,7 +80,7 @@ class DPS310 : public I2CSPIDriver<DPS310>
 
 	static constexpr uint32_t SAMPLE_RATE{32};
 
-	PX4Barometer		_px4_barometer;
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	device::Device		*_interface;
 

src/drivers/barometer/goertek/spl06/CMakeLists.txt

@@ -41,6 +41,5 @@ px4_add_module(
 		SPL06_SPI.cpp
 		spl06_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/goertek/spl06/SPL06.cpp

@@ -35,7 +35,6 @@
 
 SPL06::SPL06(const I2CSPIDriverConfig &config, spl06::ISPL06 *interface) :
 	I2CSPIDriver(config),
-	_px4_baro(interface->get_device_id()),
 	_interface(interface),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
 	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -224,10 +223,16 @@ SPL06::collect()
 	float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3;
 	float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc;
 
-	_px4_baro.set_error_count(perf_event_count(_comms_errors));
-	_px4_baro.set_temperature(temperature);
-	_px4_baro.update(timestamp_sample, fp / 100.0f);   // to millbar
-	//PX4_DEBUG("%d",(int)fp);
+
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = fp;
+	sensor_baro.temperature = temperature;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
 	perf_end(_sample_perf);
 
 	return OK;

src/drivers/barometer/goertek/spl06/SPL06.hpp

@@ -39,8 +39,9 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
 
 class SPL06 : public I2CSPIDriver<SPL06>
 {
@@ -62,7 +63,7 @@ class SPL06 : public I2CSPIDriver<SPL06>
 	int			collect(); //get results and publish
 	int			calibrate();
 
-	PX4Barometer		_px4_baro;
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	spl06::ISPL06		*_interface;
 	spl06::data_s		_data;

src/drivers/barometer/invensense/icp10100/CMakeLists.txt

@@ -41,6 +41,5 @@ px4_add_module(
 		ICP10100.hpp
 		icp10100_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/invensense/icp10100/ICP10100.cpp

@@ -37,8 +37,7 @@ using namespace time_literals;
 
 ICP10100::ICP10100(const I2CSPIDriverConfig &config) :
 	I2C(config),
-	I2CSPIDriver(config),
-	_px4_baro(get_device_id())
+	I2CSPIDriver(config)
 {
 }
 
@@ -236,12 +235,17 @@ ICP10100::RunImpl()
 
 				const hrt_abstime nowx = hrt_absolute_time();
 				float temperature = _temperature_C;
-				float pressure = _pressure_Pa; // to Pascal
-				pressure = pressure / 100.0f; // to mbar
-
-				_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
-				_px4_baro.set_temperature(temperature);
-				_px4_baro.update(nowx, pressure);
+				float pressure = _pressure_Pa;
+
+				// publish
+				sensor_baro_s sensor_baro{};
+				sensor_baro.timestamp_sample = now;
+				sensor_baro.device_id = get_device_id();
+				sensor_baro.pressure = pressure;
+				sensor_baro.temperature = temperature;
+				sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
+				sensor_baro.timestamp = hrt_absolute_time();
+				_sensor_baro_pub.publish(sensor_baro);
 
 				success = true;
 

src/drivers/barometer/invensense/icp10100/ICP10100.hpp

@@ -37,7 +37,8 @@
 
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/i2c.h>
-#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
@@ -69,7 +70,7 @@ class ICP10100 : public device::I2C, public I2CSPIDriver<ICP10100>
 	int send_command(Cmd cmd);
 	int send_command(Cmd cmd, uint8_t *data, uint8_t len);
 
-	PX4Barometer _px4_baro;
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
 	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};

src/drivers/barometer/invensense/icp10111/CMakeLists.txt

@@ -41,6 +41,5 @@ px4_add_module(
 		ICP10111.hpp
 		icp10111_main.cpp
 	DEPENDS
-		drivers_barometer
 		px4_work_queue
 	)

src/drivers/barometer/invensense/icp10111/ICP10111.cpp

@@ -37,8 +37,7 @@ using namespace time_literals;
 
 ICP10111::ICP10111(const I2CSPIDriverConfig &config) :
 	I2C(config),
-	I2CSPIDriver(config),
-	_px4_baro(get_device_id())
+	I2CSPIDriver(config)
 {
 }
 
@@ -234,14 +233,18 @@ ICP10111::RunImpl()
 				float b = (_pcal[0] - a) * (s1 + c);
 				float _pressure_Pa = a + b / (c + _raw_p);
 
-				const hrt_abstime nowx = hrt_absolute_time();
 				float temperature = _temperature_C;
 				float pressure = _pressure_Pa; // to Pascal
-				pressure = pressure / 100.0f; // to mbar
 
-				_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
-				_px4_baro.set_temperature(temperature);
-				_px4_baro.update(nowx, pressure);
+				// publish
+				sensor_baro_s sensor_baro{};
+				sensor_baro.timestamp_sample = now;
+				sensor_baro.device_id = get_device_id();
+				sensor_baro.pressure = pressure;
+				sensor_baro.temperature = temperature;
+				sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
+				sensor_baro.timestamp = hrt_absolute_time();
+				_sensor_baro_pub.publish(sensor_baro);
 
 				success = true;