diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index 82772eba20c..fc030d015be 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -35,6 +35,7 @@ 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")), @@ -176,14 +177,9 @@ BMP280::collect() const float P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7; // 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(T); + _px4_baro.update(timestamp_sample, P); perf_end(_sample_perf); diff --git a/src/drivers/barometer/bmp280/BMP280.hpp b/src/drivers/barometer/bmp280/BMP280.hpp index bb5aa28f833..0da9585bced 100644 --- a/src/drivers/barometer/bmp280/BMP280.hpp +++ b/src/drivers/barometer/bmp280/BMP280.hpp @@ -36,6 +36,7 @@ #include "bmp280.h" #include +#include #include #include #include @@ -62,7 +63,7 @@ private: int measure(); //start measure int collect(); //get results and publish - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; bmp280::IBMP280 *_interface; diff --git a/src/drivers/barometer/bmp280/CMakeLists.txt b/src/drivers/barometer/bmp280/CMakeLists.txt index 8d53de6231b..7bdb1ea18a9 100644 --- a/src/drivers/barometer/bmp280/CMakeLists.txt +++ b/src/drivers/barometer/bmp280/CMakeLists.txt @@ -42,5 +42,6 @@ px4_add_module( bmp280_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp388/CMakeLists.txt b/src/drivers/barometer/bmp388/CMakeLists.txt index 0b4cac9e509..35474254de7 100644 --- a/src/drivers/barometer/bmp388/CMakeLists.txt +++ b/src/drivers/barometer/bmp388/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( bmp388.cpp bmp388_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index ab9557f5042..6a91ac332b5 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/src/drivers/barometer/bmp388/bmp388.cpp @@ -43,6 +43,7 @@ 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")), @@ -83,6 +84,9 @@ BMP388::init() this->_item_name = "bmp390"; } + // device type may have been refined above (BMP388 vs BMP390); latch the final device id + _px4_baro.set_device_id(_interface->get_device_id()); + if (_interface->get_reg(BMP3_REV_ID_ADDR, &_chip_rev_id) != OK) { PX4_WARN("failed to get chip rev id"); return -EIO; @@ -188,14 +192,9 @@ BMP388::collect() float pressure = (float)(data.pressure / 100.0f); // to Pascal // 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, pressure); perf_end(_sample_perf); diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h index a72b95aef4f..02a7a7bfc5c 100644 --- a/src/drivers/barometer/bmp388/bmp388.h +++ b/src/drivers/barometer/bmp388/bmp388.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -330,7 +331,7 @@ private: 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 - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; IBMP388 *_interface{nullptr}; unsigned _measure_interval{0}; // interval in microseconds needed to measure diff --git a/src/drivers/barometer/bmp581/CMakeLists.txt b/src/drivers/barometer/bmp581/CMakeLists.txt index c5de8e20b3f..5be1bf60e57 100644 --- a/src/drivers/barometer/bmp581/CMakeLists.txt +++ b/src/drivers/barometer/bmp581/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( bmp581.cpp bmp581_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp581/bmp581.cpp b/src/drivers/barometer/bmp581/bmp581.cpp index 2c4c469d790..ddb027cb4bb 100644 --- a/src/drivers/barometer/bmp581/bmp581.cpp +++ b/src/drivers/barometer/bmp581/bmp581.cpp @@ -36,6 +36,7 @@ BMP581::BMP581(const I2CSPIDriverConfig &config, IBMP581 *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")), @@ -157,14 +158,9 @@ int BMP581::collect() } //publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = _interface->get_device_id(); - sensor_baro.pressure = data.pressure; - sensor_baro.temperature = data.temperature; - sensor_baro.error_count = perf_event_count(_comms_errors); - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(data.temperature); + _px4_baro.update(timestamp_sample, data.pressure); perf_end(_sample_perf); diff --git a/src/drivers/barometer/bmp581/bmp581.h b/src/drivers/barometer/bmp581/bmp581.h index 47e336a6057..18ac2cf43de 100644 --- a/src/drivers/barometer/bmp581/bmp581.h +++ b/src/drivers/barometer/bmp581/bmp581.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -282,7 +283,7 @@ private: static constexpr uint8_t INTERRUPT_POLARITY{BMP5_INT_POL_ACTIVE_HIGH}; static constexpr uint8_t INTERRUPT_DRIVE{BMP5_INT_OD_PUSHPULL}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; IBMP581 *_interface{nullptr}; unsigned _measure_interval{0}; // interval in microseconds needed to measure diff --git a/src/drivers/barometer/dps310/CMakeLists.txt b/src/drivers/barometer/dps310/CMakeLists.txt index 5da4c1faa8b..7c202c5d302 100644 --- a/src/drivers/barometer/dps310/CMakeLists.txt +++ b/src/drivers/barometer/dps310/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( dps310_main.cpp DEPENDS drivers__device + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/dps310/DPS310.cpp b/src/drivers/barometer/dps310/DPS310.cpp index a9e1f73a0d7..b060212ba0e 100644 --- a/src/drivers/barometer/dps310/DPS310.cpp +++ b/src/drivers/barometer/dps310/DPS310.cpp @@ -48,6 +48,7 @@ static void getTwosComplement(T &raw, uint8_t length) DPS310::DPS310(const I2CSPIDriverConfig &config, device::Device *interface) : I2CSPIDriver(config), + _px4_baro{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,14 +234,9 @@ DPS310::RunImpl() const float Tcomp = c0 * 0.5f + c1 * Traw_sc; // 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(Tcomp); + _px4_baro.update(timestamp_sample, Pcomp); perf_end(_sample_perf); } diff --git a/src/drivers/barometer/dps310/DPS310.hpp b/src/drivers/barometer/dps310/DPS310.hpp index 498fa01a635..96711c927a2 100644 --- a/src/drivers/barometer/dps310/DPS310.hpp +++ b/src/drivers/barometer/dps310/DPS310.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -80,7 +81,7 @@ private: static constexpr uint32_t SAMPLE_RATE{32}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; device::Device *_interface; diff --git a/src/drivers/barometer/goertek/spa06/CMakeLists.txt b/src/drivers/barometer/goertek/spa06/CMakeLists.txt index 5dbc4efc68c..8e83b97e9ed 100644 --- a/src/drivers/barometer/goertek/spa06/CMakeLists.txt +++ b/src/drivers/barometer/goertek/spa06/CMakeLists.txt @@ -43,5 +43,6 @@ px4_add_module( MODULE_CONFIG parameters.yaml DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/goertek/spa06/SPA06.cpp b/src/drivers/barometer/goertek/spa06/SPA06.cpp index a1b1b4118fc..09d811d9b22 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.cpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.cpp @@ -35,6 +35,7 @@ SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *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")), @@ -235,14 +236,9 @@ SPA06::collect() float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; - 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, fp); perf_end(_sample_perf); diff --git a/src/drivers/barometer/goertek/spa06/SPA06.hpp b/src/drivers/barometer/goertek/spa06/SPA06.hpp index 255da708195..aebf7e34869 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.hpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -63,7 +64,7 @@ private: int collect(); //get results and publish int calibrate(); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; spa06::ISPA06 *_interface; spa06::data_s _data; diff --git a/src/drivers/barometer/goertek/spl06/CMakeLists.txt b/src/drivers/barometer/goertek/spl06/CMakeLists.txt index 1df05ebe5b3..c4d03649e88 100644 --- a/src/drivers/barometer/goertek/spl06/CMakeLists.txt +++ b/src/drivers/barometer/goertek/spl06/CMakeLists.txt @@ -43,5 +43,6 @@ px4_add_module( MODULE_CONFIG parameters.yaml DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/goertek/spl06/SPL06.cpp b/src/drivers/barometer/goertek/spl06/SPL06.cpp index 9179996b321..eb9454b0e99 100644 --- a/src/drivers/barometer/goertek/spl06/SPL06.cpp +++ b/src/drivers/barometer/goertek/spl06/SPL06.cpp @@ -35,6 +35,7 @@ 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,14 +225,9 @@ SPL06::collect() float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; - 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, fp); perf_end(_sample_perf); diff --git a/src/drivers/barometer/goertek/spl06/SPL06.hpp b/src/drivers/barometer/goertek/spl06/SPL06.hpp index e62deb93712..c96eba9e3cf 100644 --- a/src/drivers/barometer/goertek/spl06/SPL06.hpp +++ b/src/drivers/barometer/goertek/spl06/SPL06.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -63,7 +64,7 @@ private: int collect(); //get results and publish int calibrate(); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; spl06::ISPL06 *_interface; spl06::data_s _data; diff --git a/src/drivers/barometer/invensense/icp101xx/CMakeLists.txt b/src/drivers/barometer/invensense/icp101xx/CMakeLists.txt index beb253f95da..cb0d122b187 100755 --- a/src/drivers/barometer/invensense/icp101xx/CMakeLists.txt +++ b/src/drivers/barometer/invensense/icp101xx/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( ICP101XX.hpp icp101xx_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/invensense/icp101xx/ICP101XX.cpp b/src/drivers/barometer/invensense/icp101xx/ICP101XX.cpp index 659ae9cb120..ad08cbcb94f 100755 --- a/src/drivers/barometer/invensense/icp101xx/ICP101XX.cpp +++ b/src/drivers/barometer/invensense/icp101xx/ICP101XX.cpp @@ -37,7 +37,8 @@ using namespace time_literals; ICP101XX::ICP101XX(const I2CSPIDriverConfig &config) : I2C(config), - I2CSPIDriver(config) + I2CSPIDriver(config), + _px4_baro{get_device_id()} { } @@ -237,14 +238,9 @@ ICP101XX::RunImpl() 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); + _px4_baro.set_error_count(perf_event_count(_bad_transfer_perf)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(now, pressure); success = true; diff --git a/src/drivers/barometer/invensense/icp101xx/ICP101XX.hpp b/src/drivers/barometer/invensense/icp101xx/ICP101XX.hpp index 284e4aad81b..e29f751a80d 100755 --- a/src/drivers/barometer/invensense/icp101xx/ICP101XX.hpp +++ b/src/drivers/barometer/invensense/icp101xx/ICP101XX.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -70,7 +71,7 @@ private: int send_command(Cmd cmd); int send_command(Cmd cmd, uint8_t *data, uint8_t len); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_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")}; diff --git a/src/drivers/barometer/invensense/icp201xx/CMakeLists.txt b/src/drivers/barometer/invensense/icp201xx/CMakeLists.txt index 4b44b97b300..f62648ac6ed 100755 --- a/src/drivers/barometer/invensense/icp201xx/CMakeLists.txt +++ b/src/drivers/barometer/invensense/icp201xx/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( ICP201XX.hpp icp201xx_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp index 4cc21739323..a4a8a48c831 100755 --- a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp +++ b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp @@ -38,7 +38,8 @@ using namespace time_literals; ICP201XX::ICP201XX(const I2CSPIDriverConfig &config) : I2C(config), - I2CSPIDriver(config) + I2CSPIDriver(config), + _px4_baro{get_device_id()} { } @@ -304,14 +305,9 @@ ICP201XX::RunImpl() perf_end(_sample_perf); perf_begin(_sample_perf); - 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); + _px4_baro.set_error_count(perf_event_count(_bad_transfer_perf)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(now, pressure); success = true; diff --git a/src/drivers/barometer/invensense/icp201xx/ICP201XX.hpp b/src/drivers/barometer/invensense/icp201xx/ICP201XX.hpp index cbc746321f4..2db61535a05 100755 --- a/src/drivers/barometer/invensense/icp201xx/ICP201XX.hpp +++ b/src/drivers/barometer/invensense/icp201xx/ICP201XX.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -72,7 +73,7 @@ private: bool flush_fifo(); bool configure(); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_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")}; diff --git a/src/drivers/barometer/lps22hb/CMakeLists.txt b/src/drivers/barometer/lps22hb/CMakeLists.txt index 3014c8c2846..41652d1c519 100644 --- a/src/drivers/barometer/lps22hb/CMakeLists.txt +++ b/src/drivers/barometer/lps22hb/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( LPS22HB_I2C.cpp LPS22HB_SPI.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps22hb/LPS22HB.cpp b/src/drivers/barometer/lps22hb/LPS22HB.cpp index a29b2189db6..ab7f617d3da 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.cpp @@ -44,6 +44,7 @@ LPS22HB::LPS22HB(const I2CSPIDriverConfig &config, device::Device *interface) : I2CSPIDriver(config), + _px4_baro{interface->get_device_id()}, _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) @@ -164,14 +165,9 @@ int LPS22HB::collect() float pressure_pa = pressure * 100.f; // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = _interface->get_device_id(); - sensor_baro.pressure = pressure_pa; - 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, pressure_pa); perf_end(_sample_perf); return PX4_OK; diff --git a/src/drivers/barometer/lps22hb/LPS22HB.hpp b/src/drivers/barometer/lps22hb/LPS22HB.hpp index b0f2141690d..7fb41fb4c9e 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.hpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -93,7 +94,7 @@ public: void RunImpl(); private: - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; device::Device *_interface; diff --git a/src/drivers/barometer/lps25h/CMakeLists.txt b/src/drivers/barometer/lps25h/CMakeLists.txt index 4cc2841f0e7..778221bd41c 100644 --- a/src/drivers/barometer/lps25h/CMakeLists.txt +++ b/src/drivers/barometer/lps25h/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( lps25h_i2c.cpp lps25h_spi.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps25h/LPS25H.cpp b/src/drivers/barometer/lps25h/LPS25H.cpp index 4a74b4be6f8..ebba3782dec 100644 --- a/src/drivers/barometer/lps25h/LPS25H.cpp +++ b/src/drivers/barometer/lps25h/LPS25H.cpp @@ -35,6 +35,7 @@ LPS25H::LPS25H(const I2CSPIDriverConfig &config, device::Device *interface) : I2CSPIDriver(config), + _px4_baro{interface->get_device_id()}, _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")) @@ -176,14 +177,9 @@ int LPS25H::collect() float pressure_pa = pressure * 100.f; // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = _interface->get_device_id(); - sensor_baro.pressure = pressure_pa; - 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, pressure_pa); perf_end(_sample_perf); return PX4_OK; diff --git a/src/drivers/barometer/lps25h/LPS25H.hpp b/src/drivers/barometer/lps25h/LPS25H.hpp index bf323e62e59..5a1b7775943 100644 --- a/src/drivers/barometer/lps25h/LPS25H.hpp +++ b/src/drivers/barometer/lps25h/LPS25H.hpp @@ -42,6 +42,7 @@ #include "lps25h.h" #include +#include #include #include #include @@ -177,7 +178,7 @@ private: int measure(); int collect(); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; device::Device *_interface; diff --git a/src/drivers/barometer/lps33hw/CMakeLists.txt b/src/drivers/barometer/lps33hw/CMakeLists.txt index 800262a1ab7..6e8ce1c9eab 100644 --- a/src/drivers/barometer/lps33hw/CMakeLists.txt +++ b/src/drivers/barometer/lps33hw/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( lps33hw_spi.cpp lps33hw_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps33hw/lps33hw.cpp b/src/drivers/barometer/lps33hw/lps33hw.cpp index 00d821791ca..8dd9bf3c8e4 100644 --- a/src/drivers/barometer/lps33hw/lps33hw.cpp +++ b/src/drivers/barometer/lps33hw/lps33hw.cpp @@ -49,6 +49,7 @@ static void getTwosComplement(T &raw, uint8_t length) LPS33HW::LPS33HW(const I2CSPIDriverConfig &config, device::Device *interface) : I2CSPIDriver(config), + _px4_baro{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")), @@ -179,14 +180,9 @@ LPS33HW::RunImpl() float pressure_pa = pressure_hPa * 100.f; // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = _interface->get_device_id(); - sensor_baro.pressure = pressure_pa; - sensor_baro.temperature = temp; - sensor_baro.error_count = perf_event_count(_comms_errors); - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(temp); + _px4_baro.update(timestamp_sample, pressure_pa); perf_end(_sample_perf); ScheduleDelayed(1000000 / SAMPLE_RATE); diff --git a/src/drivers/barometer/lps33hw/lps33hw.hpp b/src/drivers/barometer/lps33hw/lps33hw.hpp index d499e708391..d18b1862150 100644 --- a/src/drivers/barometer/lps33hw/lps33hw.hpp +++ b/src/drivers/barometer/lps33hw/lps33hw.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -83,7 +84,7 @@ private: static constexpr uint32_t SAMPLE_RATE{75}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; device::Device *_interface; diff --git a/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt b/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt index c778044cb58..02f2e39a103 100644 --- a/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt +++ b/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( MPC2520.cpp MPC2520.hpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp b/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp index 165c7853c40..4210bbf8abf 100644 --- a/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp +++ b/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp @@ -44,7 +44,8 @@ static constexpr int32_t combine(uint8_t h, uint8_t m, uint8_t l) MPC2520::MPC2520(const I2CSPIDriverConfig &config) : I2C(config), - I2CSPIDriver(config) + I2CSPIDriver(config), + _px4_baro{get_device_id()} { //_debug_enabled = true; } @@ -243,14 +244,9 @@ void MPC2520::RunImpl() Traw_sc * Praw_sc * (_prom.c11 + Praw_sc * _prom.c21); // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = now; - sensor_baro.device_id = get_device_id(); - sensor_baro.pressure = Pcomp; - sensor_baro.temperature = Tcomp; - sensor_baro.error_count = perf_event_count(_bad_transfer_perf) + perf_event_count(_bad_register_perf); - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + _px4_baro.set_error_count(perf_event_count(_bad_transfer_perf) + perf_event_count(_bad_register_perf)); + _px4_baro.set_temperature(Tcomp); + _px4_baro.update(now, Pcomp); success = true; diff --git a/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp b/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp index 7cc110dbb96..9ea1468a625 100644 --- a/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp +++ b/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -77,7 +78,7 @@ private: void RegisterWrite(Register reg, uint8_t value); void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; diff --git a/src/drivers/barometer/mpl3115a2/CMakeLists.txt b/src/drivers/barometer/mpl3115a2/CMakeLists.txt index 6ad33fcdce4..c96c24dfd6c 100644 --- a/src/drivers/barometer/mpl3115a2/CMakeLists.txt +++ b/src/drivers/barometer/mpl3115a2/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( MPL3115A2.cpp mpl3115a2_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp index 8c4348b705c..eb869748bec 100644 --- a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp @@ -56,6 +56,7 @@ MPL3115A2::MPL3115A2(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), + _px4_baro{get_device_id()}, _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) @@ -268,14 +269,9 @@ int MPL3115A2::collect() float P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f); // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(T); + _px4_baro.update(timestamp_sample, P); perf_end(_sample_perf); diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp index ba0ebe9159f..243a349fa36 100644 --- a/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -77,7 +78,7 @@ private: int RegisterRead(uint8_t reg, void *data, unsigned count = 1); int RegisterWrite(uint8_t reg, uint8_t data); - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; bool _collect_phase{false}; diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index ce693bf63a2..6faa5b7a460 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -44,5 +44,6 @@ px4_add_module( DEPENDS cdev drivers__device + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/ms5611/MS5611.hpp b/src/drivers/barometer/ms5611/MS5611.hpp index ff360b795fa..7c8def34027 100644 --- a/src/drivers/barometer/ms5611/MS5611.hpp +++ b/src/drivers/barometer/ms5611/MS5611.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -115,7 +116,7 @@ public: protected: void print_status() override; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; device::Device *_interface; diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index d925a30af89..c490dcc7868 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -43,6 +43,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config) : I2CSPIDriver(config), + _px4_baro{interface->get_device_id()}, _interface(interface), _prom(prom_buf.s), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), @@ -133,6 +134,9 @@ MS5611::init() break; } + // device type was just refined (MS5611 vs MS5607); latch the final device id + _px4_baro.set_device_id(_interface->get_device_id()); + ret = OK; break; @@ -342,14 +346,9 @@ MS5611::collect() // publish if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) { - 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 = _last_temperature; - sensor_baro.error_count = perf_event_count(_comms_errors); - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(_last_temperature); + _px4_baro.update(timestamp_sample, P); } } diff --git a/src/drivers/barometer/ms5837/CMakeLists.txt b/src/drivers/barometer/ms5837/CMakeLists.txt index 027d0484081..532a31f6a5f 100644 --- a/src/drivers/barometer/ms5837/CMakeLists.txt +++ b/src/drivers/barometer/ms5837/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( MS5837.cpp MS5837.hpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/ms5837/MS5837.cpp b/src/drivers/barometer/ms5837/MS5837.cpp index 365df5d8222..e8b913b09ef 100644 --- a/src/drivers/barometer/ms5837/MS5837.cpp +++ b/src/drivers/barometer/ms5837/MS5837.cpp @@ -41,6 +41,7 @@ MS5837::MS5837(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), + _px4_baro{get_device_id()}, _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) @@ -340,14 +341,9 @@ int MS5837::_collect() int32_t pressure_pascal = ((((raw * _SENS) >> 21) - _OFF) >> 13) * 10; // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = get_device_id(); - sensor_baro.pressure = pressure_pascal; - sensor_baro.temperature = _last_temperature; - sensor_baro.error_count = perf_event_count(_comms_errors); - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(_last_temperature); + _px4_baro.update(timestamp_sample, pressure_pascal); } /* update the measurement state machine */ diff --git a/src/drivers/barometer/ms5837/MS5837.hpp b/src/drivers/barometer/ms5837/MS5837.hpp index 4de1c38462c..5aba3db12b2 100644 --- a/src/drivers/barometer/ms5837/MS5837.hpp +++ b/src/drivers/barometer/ms5837/MS5837.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -80,7 +81,7 @@ public: private: int probe() override; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; ms5837::prom_u _prom{}; diff --git a/src/drivers/barometer/tcbp001ta/CMakeLists.txt b/src/drivers/barometer/tcbp001ta/CMakeLists.txt index 4af09cb91a5..56a8badfe0d 100644 --- a/src/drivers/barometer/tcbp001ta/CMakeLists.txt +++ b/src/drivers/barometer/tcbp001ta/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( tcbp001ta_spi.cpp tcbp001ta_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp b/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp index c903a910b73..e34228601db 100644 --- a/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp +++ b/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp @@ -45,6 +45,7 @@ TCBP001TA::TCBP001TA(tcbp001ta::ITCBP001TA *interface) : ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), + _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")), @@ -251,14 +252,9 @@ TCBP001TA::collect() Praw_sc * Praw_sc * (_fcal.c11 + Praw_sc * _fcal.c21); // 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); + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(T); + _px4_baro.update(timestamp_sample, P); perf_end(_sample_perf); diff --git a/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp b/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp index 1abe63ea177..ca17862c25c 100644 --- a/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp +++ b/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -64,7 +65,7 @@ private: int measure(); //start measure int collect(); //get results and publish - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro; tcbp001ta::ITCBP001TA *_interface; diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 051b40ab0c5..7975147ef2a 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(accelerometer) +add_subdirectory(barometer) add_subdirectory(device) add_subdirectory(gyroscope) add_subdirectory(led) diff --git a/src/lib/drivers/barometer/CMakeLists.txt b/src/lib/drivers/barometer/CMakeLists.txt new file mode 100644 index 00000000000..a1f4d4f90ca --- /dev/null +++ b/src/lib/drivers/barometer/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2026 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(drivers_barometer PX4Barometer.cpp) diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp new file mode 100644 index 00000000000..84273ff3e0d --- /dev/null +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + + +#include "PX4Barometer.hpp" + +PX4Barometer::PX4Barometer(uint32_t device_id) +{ + _report.device_id = device_id; + _report.temperature = 15; // if no temperature is set, report the sea level standard temperature of 15°C. As used in VehicleAirData +} + +void PX4Barometer::update(const hrt_abstime ×tamp_sample, float pressure) +{ + if (!PX4_ISFINITE(pressure)) { + return; + } + + _report.timestamp_sample = timestamp_sample; + _report.pressure = pressure; + _report.timestamp = hrt_absolute_time(); + + _sensor_pub.publish(_report); +} diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp new file mode 100644 index 00000000000..b8a0cbf4116 --- /dev/null +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 + +#include +#include +#include + +class PX4Barometer +{ +public: + PX4Barometer(uint32_t device_id); + + void set_device_id(uint32_t device_id) { _report.device_id = device_id; } + void set_error_count(uint32_t error_count) { _report.error_count = error_count; } + void set_temperature(float temperature) { _report.temperature = temperature; } + + void update(const hrt_abstime ×tamp_sample, float pressure); + + int get_instance() { return _sensor_pub.get_instance(); }; + uint32_t get_device_id() const { return _report.device_id; } + +private: + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_baro)}; + + sensor_baro_s _report{}; +}; diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 28998f53dbf..a58967f8813 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -73,6 +73,7 @@ if (gz-transport_FOUND) drivers_gyroscope drivers_magnetometer drivers_rangefinder + drivers_barometer mixer_module px4_work_queue ${GZ_TRANSPORT_TARGET} diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2818d6eb0a3..a3878b89b3f 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -400,21 +400,8 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg) void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg) { - const uint64_t timestamp = hrt_absolute_time(); - - device::Device::DeviceId id{}; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM; - id.devid_s.bus = 1; - id.devid_s.address = 1; - - sensor_baro_s report{}; - report.timestamp = timestamp; - report.timestamp_sample = timestamp; - report.device_id = id.devid; - report.pressure = msg.pressure(); - report.temperature = _temperature; // this will be static if no airspeed sensor is on the model. - _sensor_baro_pub.publish(report); + _px4_baro.set_temperature(_temperature); // this will be static if no airspeed sensor is on the model. + _px4_baro.update(hrt_absolute_time(), msg.pressure()); } void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index d03df89f3f9..6a108dbe730 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -56,9 +57,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -149,6 +148,7 @@ private: 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: 1, ADDR: 3, TYPE: SIMULATION PX4Rangefinder _px4_rangefinder{10092812}; // 10092812: DRV_DIST_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{6619404}; // 6619404: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; @@ -157,7 +157,6 @@ private: uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; diff --git a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt b/src/modules/simulation/sensor_baro_sim/CMakeLists.txt index b58c372fd6f..0a4a0394e5e 100644 --- a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt +++ b/src/modules/simulation/sensor_baro_sim/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( MODULE_CONFIG parameters.yaml DEPENDS + drivers_barometer geo px4_work_queue ) diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index c7bc4c303ed..b304035d823 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -164,14 +164,8 @@ void SensorBaroSim::Run() float temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); // 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); - + _px4_baro.set_temperature(temperature); + _px4_baro.update(hrt_absolute_time(), pressure); _last_update_time = gpos.timestamp; } diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index bcde38852ee..c46d7fab1f6 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -38,12 +38,11 @@ #include #include #include +#include #include -#include #include #include #include -#include #include using namespace time_literals; @@ -83,7 +82,7 @@ private: hrt_abstime _last_update_time{0}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt index f5f39d7106b..1b36b769ec1 100644 --- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt +++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt @@ -49,6 +49,7 @@ px4_add_module( conversion geo drivers_accelerometer + drivers_barometer drivers_gyroscope drivers_magnetometer ) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index ffb4d05d0ea..2eb7c577a92 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -306,28 +306,22 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil } // baro - if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { - - if (!_baro_stuck) { - _last_baro_pressure = sensors.abs_pressure * 100.f; // hPa to Pa - _last_baro_temperature = sensors.temperature; + if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + if (sensors.id >= BARO_COUNT_MAX) { + PX4_ERR("Number of simulated barometer %d out of range. Max: %d", sensors.id, BARO_COUNT_MAX); + return; } - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = time; - sensor_baro.pressure = _last_baro_pressure; - sensor_baro.temperature = _last_baro_temperature; + if (_baro_stuck[sensors.id]) { + _px4_baro[sensors.id].set_temperature(_last_baro_temperature[sensors.id]); + _px4_baro[sensors.id].update(time, _last_baro_pressure[sensors.id]); - // 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); + } else if (!_baro_blocked[sensors.id]) { + _last_baro_pressure[sensors.id] = sensors.abs_pressure * 100.f; // hPa to Pa + _last_baro_temperature[sensors.id] = sensors.temperature; + _px4_baro[sensors.id].set_temperature(_last_baro_temperature[sensors.id]); + _px4_baro[sensors.id].update(time, _last_baro_pressure[sensors.id]); + } } // differential pressure @@ -1708,20 +1702,55 @@ void SimulatorMavlink::check_failure_injections() handled = true; if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, baro off"); supported = true; - _baro_blocked = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < BARO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d off", i); + _baro_blocked[i] = true; + _baro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d off", instance - 1); + _baro_blocked[instance - 1] = true; + _baro_stuck[instance - 1] = false; + } } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); supported = true; - _baro_stuck = true; - _baro_blocked = false; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < BARO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d stuck", i); + _baro_blocked[i] = false; + _baro_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d stuck", instance - 1); + _baro_blocked[instance - 1] = false; + _baro_stuck[instance - 1] = true; + } } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, baro ok"); supported = true; - _baro_blocked = false; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < BARO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d ok", i); + _baro_blocked[i] = false; + _baro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, baro %d ok", instance - 1); + _baro_blocked[instance - 1] = false; + _baro_stuck[instance - 1] = false; + } } } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 605b68ffbb2..35964a5a723 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -71,7 +72,6 @@ #endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR #include #include -#include #include #include #include @@ -199,7 +199,11 @@ private: {197644, ROTATION_NONE}, }; - uORB::PublicationMulti _sensor_baro_pubs[2] {{ORB_ID(sensor_baro)}, {ORB_ID(sensor_baro)}}; + static constexpr uint8_t BARO_COUNT_MAX = 2; + PX4Barometer _px4_baro[BARO_COUNT_MAX] { + {6620172}, // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + {6620428}, // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION + }; float _sensors_temperature{0}; @@ -314,8 +318,8 @@ private: sensor_gyro_fifo_s _last_gyro_fifo{}; matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {}; - bool _baro_blocked{false}; - bool _baro_stuck{false}; + bool _baro_blocked[BARO_COUNT_MAX] {}; + bool _baro_stuck[BARO_COUNT_MAX] {}; bool _mag_blocked[MAG_COUNT_MAX] {}; bool _mag_stuck[MAG_COUNT_MAX] {}; @@ -332,8 +336,8 @@ private: float _last_magy[MAG_COUNT_MAX] {}; float _last_magz[MAG_COUNT_MAX] {}; - float _last_baro_pressure{0.0f}; - float _last_baro_temperature{0.0f}; + float _last_baro_pressure[BARO_COUNT_MAX] {}; + float _last_baro_temperature[BARO_COUNT_MAX] {}; int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};