diff --git a/src/drivers/barometer/bmp280/CMakeLists.txt b/src/drivers/barometer/bmp280/CMakeLists.txt index 6bddf53313..9d4544eb57 100644 --- a/src/drivers/barometer/bmp280/CMakeLists.txt +++ b/src/drivers/barometer/bmp280/CMakeLists.txt @@ -42,4 +42,6 @@ px4_add_module( bmp280_spi.cpp bmp280_i2c.cpp bmp280.cpp + DEPENDS + px4_work_queue ) diff --git a/src/drivers/barometer/bmp280/bmp280.cpp b/src/drivers/barometer/bmp280/bmp280.cpp index f90bfbc430..a4ec020415 100644 --- a/src/drivers/barometer/bmp280/bmp280.cpp +++ b/src/drivers/barometer/bmp280/bmp280.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include @@ -68,6 +67,7 @@ #include #include +#include enum BMP280_BUS { @@ -78,15 +78,11 @@ enum BMP280_BUS { BMP280_BUS_SPI_EXTERNAL }; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - /* * BMP280 internal constants and data structures. */ -class BMP280 : public cdev::CDev +class BMP280 : public cdev::CDev, public px4::ScheduledWorkItem { public: BMP280(bmp280::IBMP280 *interface, const char *path); @@ -109,9 +105,8 @@ private: uint8_t _curr_ctrl; - struct work_s _work; - unsigned _report_ticks; // 0 - no cycling, otherwise period of sending a report - unsigned _max_mesure_ticks; //ticks needed to measure + unsigned _report_interval; // 0 - no cycling, otherwise period of sending a report + unsigned _max_measure_interval; // interval in microseconds needed to measure ringbuffer::RingBuffer *_reports; @@ -135,8 +130,8 @@ private: /* periodic execution helpers */ void start_cycle(); void stop_cycle(); - void cycle(); //main execution - static void cycle_trampoline(void *arg); + + void Run() override; int measure(); //start measure int collect(); //get results and publish @@ -149,9 +144,10 @@ extern "C" __EXPORT int bmp280_main(int argc, char *argv[]); BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) : CDev(path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _running(false), - _report_ticks(0), + _report_interval(0), _reports(nullptr), _collect_phase(false), _baro_topic(nullptr), @@ -161,8 +157,6 @@ BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) : _measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")), _comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); } BMP280::~BMP280() @@ -226,7 +220,7 @@ BMP280::init() /* set config, recommended settings */ _curr_ctrl = BMP280_CTRL_P16 | BMP280_CTRL_T2; _interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL); - _max_mesure_ticks = USEC2TICK(BMP280_MT_INIT + BMP280_MT * (16 - 1 + 2 - 1)); + _max_measure_interval = (BMP280_MT_INIT + BMP280_MT * (16 - 1 + 2 - 1)); _interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG); /* get calibration and pre process them*/ @@ -256,7 +250,7 @@ BMP280::init() return -EIO; } - usleep(TICK2USEC(_max_mesure_ticks)); + usleep(_max_measure_interval); if (collect()) { return -EIO; @@ -289,7 +283,7 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_report_ticks > 0) { + if (_report_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -315,7 +309,7 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen) return -EIO; } - usleep(TICK2USEC(_max_mesure_ticks)); + usleep(_max_measure_interval); if (collect()) { return -EIO; @@ -335,7 +329,7 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { - unsigned ticks = 0; + unsigned interval = 0; switch (arg) { @@ -343,23 +337,23 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case SENSOR_POLLRATE_DEFAULT: - ticks = _max_mesure_ticks; + interval = _max_measure_interval; /* FALLTHROUGH */ default: { - if (ticks == 0) { - ticks = USEC2TICK(USEC_PER_SEC / arg); + if (interval == 0) { + interval = (USEC_PER_SEC / arg); } /* do we need to start internal polling? */ - bool want_start = (_report_ticks == 0); + bool want_start = (_report_interval == 0); /* check against maximum rate */ - if (ticks < _max_mesure_ticks) { + if (interval < _max_measure_interval) { return -EINVAL; } - _report_ticks = ticks; + _report_interval = interval; if (want_start) { start_cycle(); @@ -389,41 +383,33 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg) void BMP280::start_cycle() { - /* reset the report ring and state machine */ _collect_phase = false; _running = true; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, 1); + ScheduleNow(); } void BMP280::stop_cycle() { _running = false; - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -BMP280::cycle_trampoline(void *arg) -{ - BMP280 *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -BMP280::cycle() +BMP280::Run() { if (_collect_phase) { collect(); - unsigned wait_gap = _report_ticks - _max_mesure_ticks; + unsigned wait_gap = _report_interval - _max_measure_interval; if ((wait_gap != 0) && (_running)) { - work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, - wait_gap); //need to wait some time before new measurement + //need to wait some time before new measurement + ScheduleDelayed(wait_gap); + return; } @@ -432,9 +418,8 @@ BMP280::cycle() measure(); if (_running) { - work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks); + ScheduleDelayed(_max_measure_interval); } - } int @@ -517,7 +502,7 @@ BMP280::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %u us \n", _report_ticks * USEC_PER_TICK); + printf("poll interval: %u us \n", _report_interval); _reports->print_info("report queue"); sensor_baro_s brp = {}; diff --git a/src/drivers/barometer/bmp280/bmp280.h b/src/drivers/barometer/bmp280/bmp280.h index 143fbfbb6b..528e639f84 100644 --- a/src/drivers/barometer/bmp280/bmp280.h +++ b/src/drivers/barometer/bmp280/bmp280.h @@ -148,6 +148,8 @@ public: // bulk read of calibration data into buffer, return same pointer virtual bmp280::calibration_s *get_calibration(uint8_t addr) = 0; + virtual uint32_t get_device_id() const = 0; + }; } /* namespace */ diff --git a/src/drivers/barometer/bmp280/bmp280_i2c.cpp b/src/drivers/barometer/bmp280/bmp280_i2c.cpp index 79ac6863fd..e873927d74 100644 --- a/src/drivers/barometer/bmp280/bmp280_i2c.cpp +++ b/src/drivers/barometer/bmp280/bmp280_i2c.cpp @@ -60,6 +60,8 @@ public: bmp280::data_s *get_data(uint8_t addr); bmp280::calibration_s *get_calibration(uint8_t addr); + uint32_t get_device_id() const override { return device::I2C::get_device_id(); } + private: struct bmp280::calibration_s _cal; struct bmp280::data_s _data; diff --git a/src/drivers/barometer/bmp280/bmp280_spi.cpp b/src/drivers/barometer/bmp280/bmp280_spi.cpp index 1b2947e681..f7e01ce59b 100644 --- a/src/drivers/barometer/bmp280/bmp280_spi.cpp +++ b/src/drivers/barometer/bmp280/bmp280_spi.cpp @@ -76,6 +76,8 @@ public: bmp280::data_s *get_data(uint8_t addr); bmp280::calibration_s *get_calibration(uint8_t addr); + uint32_t get_device_id() const override { return device::SPI::get_device_id(); } + private: spi_calibration_s _cal; spi_data_s _data;