diff --git a/src/drivers/barometer/lps22hb/CMakeLists.txt b/src/drivers/barometer/lps22hb/CMakeLists.txt index 782adac43e..41652d1c51 100644 --- a/src/drivers/barometer/lps22hb/CMakeLists.txt +++ b/src/drivers/barometer/lps22hb/CMakeLists.txt @@ -38,4 +38,7 @@ px4_add_module( LPS22HB.cpp 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 667122a189..e43155a080 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.cpp @@ -43,20 +43,16 @@ #define LPS22HB_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */ LPS22HB::LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface) : - CDev(nullptr), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), + _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")) + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) { } LPS22HB::~LPS22HB() { - if (_class_instance != -1) { - unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance); - } - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -64,102 +60,18 @@ LPS22HB::~LPS22HB() delete _interface; } -int -LPS22HB::init() +int LPS22HB::init() { - int ret = CDev::init(); - - if (ret != OK) { - PX4_DEBUG("CDev init failed"); - goto out; - } - if (reset() != OK) { - goto out; + return PX4_ERROR; } - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - ret = OK; - - _measure_interval = LPS22HB_CONVERSION_INTERVAL; start(); -out: - return ret; -} - -int -LPS22HB::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - unsigned dummy = arg; - - switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (LPS22HB_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - _measure_interval = (LPS22HB_CONVERSION_INTERVAL); - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < (LPS22HB_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - return reset(); - - case DEVIOCGDEVICEID: - return _interface->ioctl(cmd, dummy); - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } + return PX4_OK; } -void -LPS22HB::start() +void LPS22HB::start() { /* reset the report ring and state machine */ _collect_phase = false; @@ -168,18 +80,12 @@ LPS22HB::start() ScheduleNow(); } -int -LPS22HB::reset() +int LPS22HB::reset() { - int ret = PX4_ERROR; - - ret = write_reg(CTRL_REG2, BOOT | SWRESET); - - return ret; + return write_reg(CTRL_REG2, BOOT | SWRESET); } -void -LPS22HB::RunImpl() +void LPS22HB::RunImpl() { /* collection phase? */ if (_collect_phase) { @@ -194,17 +100,6 @@ LPS22HB::RunImpl() /* next phase is measurement */ _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > LPS22HB_CONVERSION_INTERVAL) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - LPS22HB_CONVERSION_INTERVAL); - - return; - } } /* measurement phase */ @@ -219,8 +114,7 @@ LPS22HB::RunImpl() ScheduleDelayed(LPS22HB_CONVERSION_INTERVAL); } -int -LPS22HB::measure() +int LPS22HB::measure() { // Send the command to begin a 16-bit measurement. int ret = write_reg(CTRL_REG2, IF_ADD_INC | ONE_SHOT); @@ -232,14 +126,11 @@ LPS22HB::measure() return ret; } -int -LPS22HB::collect() +int LPS22HB::collect() { perf_begin(_sample_perf); - sensor_baro_s new_report; /* get measurements from the device : MSB enables register address auto-increment */ -#pragma pack(push, 1) struct { uint8_t STATUS; uint8_t PRESS_OUT_XL; @@ -247,11 +138,10 @@ LPS22HB::collect() uint8_t PRESS_OUT_H; uint8_t TEMP_OUT_L; uint8_t TEMP_OUT_H; - } report; -#pragma pack(pop) + } report{}; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - new_report.timestamp = hrt_absolute_time(); + const hrt_abstime timestamp_sample = hrt_absolute_time(); int ret = _interface->read(STATUS, (uint8_t *)&report, sizeof(report)); if (ret != OK) { @@ -260,50 +150,28 @@ LPS22HB::collect() return ret; } + uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8); + float temperature = 42.5f + (TEMP_OUT / 480.0f); + _px4_baro.set_temperature(temperature); + // To obtain the pressure in hPa, take the two’s complement of the complete word and then divide by 4096 LSB/hPa. uint32_t P = report.PRESS_OUT_XL + (report.PRESS_OUT_L << 8) + (report.PRESS_OUT_H << 16); - uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8); - /* Pressure and MSL in mBar */ - new_report.pressure = P / 4096.0f; - new_report.temperature = 42.5f + (TEMP_OUT / 480.0f); - - /* get device ID */ - new_report.device_id = _interface->get_device_id(); - new_report.error_count = perf_event_count(_comms_errors); - - if (_baro_topic != nullptr) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report); - - } else { - bool sensor_is_onboard = !_interface->external(); - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report, &_orb_class_instance, - (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - - if (_baro_topic == nullptr) { - PX4_ERR("advertise failed"); - } - } - - _last_report = new_report; - - ret = OK; + float pressure = P / 4096.0f; + _px4_baro.update(timestamp_sample, pressure); perf_end(_sample_perf); - return ret; + return PX4_OK; } -int -LPS22HB::write_reg(uint8_t reg, uint8_t val) +int LPS22HB::write_reg(uint8_t reg, uint8_t val) { uint8_t buf = val; return _interface->write(reg, &buf, 1); } -int -LPS22HB::read_reg(uint8_t reg, uint8_t &val) +int LPS22HB::read_reg(uint8_t reg, uint8_t &val) { uint8_t buf = val; int ret = _interface->read(reg, &buf, 1); @@ -311,14 +179,10 @@ LPS22HB::read_reg(uint8_t reg, uint8_t &val) return ret; } -void -LPS22HB::print_status() +void LPS22HB::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - - PX4_INFO("poll interval: %u", _measure_interval); - - print_message(_last_report); + _px4_baro.print_status(); } diff --git a/src/drivers/barometer/lps22hb/LPS22HB.hpp b/src/drivers/barometer/lps22hb/LPS22HB.hpp index 8d0d6fc5bc..b95c254149 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.hpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.hpp @@ -36,17 +36,10 @@ #include #include -#include -#include -#include -#include -#include -#include +#include #include #include -#include -#include - +#include static constexpr uint8_t WHO_AM_I = 0x0F; static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1; @@ -81,7 +74,7 @@ static constexpr uint8_t TEMP_OUT_H = 0x2C; extern device::Device *LPS22HB_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); extern device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency); -class LPS22HB : public cdev::CDev, public I2CSPIDriver +class LPS22HB : public I2CSPIDriver { public: LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface); @@ -91,32 +84,21 @@ public: int runtime_instance); static void print_usage(); - virtual int init(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + int init(); void print_status(); void RunImpl(); -protected: - device::Device *_interface; - private: - unsigned _measure_interval{0}; + PX4Barometer _px4_baro; + device::Device *_interface; bool _collect_phase{false}; - orb_advert_t _baro_topic{nullptr}; - - int _orb_class_instance{-1}; - int _class_instance{-1}; - perf_counter_t _sample_perf; perf_counter_t _comms_errors; - sensor_baro_s _last_report{}; /**< used for info() */ - /** * Initialise the automatic measurement state machine and start it. * @@ -130,7 +112,6 @@ private: */ int reset(); - /** * Write a register. * @@ -160,8 +141,4 @@ private: * Collect the result of the most recent measurement. */ int collect(); - - /* this class has pointer data members, do not allow copying it */ - LPS22HB(const LPS22HB &); - LPS22HB operator=(const LPS22HB &); }; diff --git a/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp index f929e52452..1b4a3dee59 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp @@ -39,6 +39,8 @@ #include "LPS22HB.hpp" +#include + #define LPS22HB_ADDRESS 0x5D device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency); @@ -47,13 +49,13 @@ class LPS22HB_I2C : public device::I2C { public: LPS22HB_I2C(int bus, int bus_frequency); - virtual ~LPS22HB_I2C() = default; + ~LPS22HB_I2C() override = default; - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + int probe() override; }; @@ -68,10 +70,9 @@ LPS22HB_I2C::LPS22HB_I2C(int bus, int bus_frequency) : { } -int -LPS22HB_I2C::probe() +int LPS22HB_I2C::probe() { - uint8_t id; + uint8_t id = 0; _retries = 10; @@ -87,11 +88,10 @@ LPS22HB_I2C::probe() return -EIO; } - return OK; + return PX4_OK; } -int -LPS22HB_I2C::write(unsigned address, void *data, unsigned count) +int LPS22HB_I2C::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; diff --git a/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp b/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp index 5211b014c9..0fd4832dfc 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp @@ -39,6 +39,8 @@ #include "LPS22HB.hpp" +#include + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -49,12 +51,13 @@ class LPS22HB_SPI : public device::SPI { public: LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); - virtual ~LPS22HB_SPI() = default; + ~LPS22HB_SPI() override = default; - virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; +protected: + int probe() override; }; device::Device * @@ -63,21 +66,13 @@ LPS22HB_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi return new LPS22HB_SPI(bus, devid, bus_frequency, spi_mode); } -LPS22HB_SPI::LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) - : SPI(DRV_BARO_DEVTYPE_LPS22HB, MODULE_NAME, bus, device, spi_mode, bus_frequency) +LPS22HB_SPI::LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_LPS22HB, MODULE_NAME, bus, device, spi_mode, bus_frequency) { } -int -LPS22HB_SPI::init() +int LPS22HB_SPI::probe() { - int ret = SPI::init(); - - if (ret != OK) { - DEVICE_DEBUG("SPI init failed"); - return -EIO; - } - // read WHO_AM_I value uint8_t id = 0; @@ -91,11 +86,10 @@ LPS22HB_SPI::init() return -EIO; } - return OK; + return PX4_OK; } -int -LPS22HB_SPI::write(unsigned address, void *data, unsigned count) +int LPS22HB_SPI::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -109,8 +103,7 @@ LPS22HB_SPI::write(unsigned address, void *data, unsigned count) return transfer(&buf[0], &buf[0], count + 1); } -int -LPS22HB_SPI::read(unsigned address, void *data, unsigned count) +int LPS22HB_SPI::read(unsigned address, void *data, unsigned count) { uint8_t buf[32]; diff --git a/src/drivers/barometer/lps22hb/lps22hb_main.cpp b/src/drivers/barometer/lps22hb/lps22hb_main.cpp index 0b83dce5e4..1c4bf5bb4e 100644 --- a/src/drivers/barometer/lps22hb/lps22hb_main.cpp +++ b/src/drivers/barometer/lps22hb/lps22hb_main.cpp @@ -34,11 +34,7 @@ #include "LPS22HB.hpp" #include -extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]); - - -void -LPS22HB::print_usage() +void LPS22HB::print_usage() { PRINT_MODULE_USAGE_NAME("lps22hb", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("baro"); @@ -85,7 +81,7 @@ I2CSPIDriverBase *LPS22HB::instantiate(const BusCLIArguments &cli, const BusInst return dev; } -extern "C" int lps22hb_main(int argc, char *argv[]) +extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]) { using ThisDriver = LPS22HB; BusCLIArguments cli{true, true};