diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index 7f44534ae8..8da539b0e1 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -41,4 +41,6 @@ px4_add_module( ms5611_spi.cpp ms5611_i2c.cpp ms5611.cpp + DEPENDS + px4_work_queue ) diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 8d03cf947d..978e1e6185 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -33,7 +33,7 @@ /** * @file ms5611.cpp - * Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI. + * Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI. */ #include @@ -51,21 +51,16 @@ #include #include -#include -#include -#include - -#include #include #include #include #include #include - -#include +#include #include #include +#include #include "ms5611.h" @@ -83,10 +78,6 @@ enum MS5611_BUS { MS5611_BUS_SPI_EXTERNAL }; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) @@ -126,7 +117,7 @@ enum MS5611_BUS { #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" -class MS5611 : public cdev::CDev +class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem { public: MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); @@ -147,8 +138,7 @@ protected: ms5611::prom_s _prom; - struct work_s _work {}; - unsigned _measure_ticks; + unsigned _measure_interval{0}; ringbuffer::RingBuffer *_reports; enum MS56XX_DEVICE_TYPES _device_type; @@ -173,17 +163,15 @@ protected: /** * Initialize the automatic measurement state machine and start it. * - * @param delay_ticks the number of queue ticks before executing the next cycle - * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(unsigned delay_ticks = 1); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -198,15 +186,7 @@ protected: * and measurement to provide the most recent measurement possible * at the next interval. */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); + void Run() override; /** * Issue a measurement command for the current state. @@ -229,9 +209,9 @@ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type) : CDev(path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), _reports(nullptr), _device_type(device_type), _collect_phase(false), @@ -251,7 +231,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char * MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); @@ -400,7 +380,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -474,14 +454,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default polling rate */ case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_measure_interval == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + _measure_interval = MS5611_CONVERSION_INTERVAL; /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -490,22 +470,22 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_measure_interval == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to interval via microseconds */ + unsigned interval = (1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) { + if (_measure_interval < MS5611_CONVERSION_INTERVAL) { return -EINVAL; } /* update interval for next measurement */ - _measure_ticks = ticks; + _measure_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -530,34 +510,25 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle(unsigned delay_ticks) +MS5611::start() { - /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks); + ScheduleNow(); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -MS5611::cycle_trampoline(void *arg) -{ - MS5611 *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -MS5611::cycle() +MS5611::Run() { int ret; unsigned dummy; @@ -585,7 +556,7 @@ MS5611::cycle() * to wait 2.8 ms after issuing the sensor reset command * according to the MS5611 datasheet */ - start_cycle(USEC2TICK(2800)); + ScheduleDelayed(2800); return; } @@ -598,14 +569,10 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_measure_interval > MS5611_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - MS5611_CONVERSION_INTERVAL); return; } @@ -618,7 +585,7 @@ MS5611::cycle() /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -626,11 +593,7 @@ MS5611::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + ScheduleDelayed(MS5611_CONVERSION_INTERVAL); } int @@ -788,7 +751,7 @@ MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u\n", _measure_interval); _reports->print_info("report queue"); printf("device: %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");