From e73dd73ea717156feaf4f5e9075fce21ba5f4a81 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:40:45 -0500 Subject: [PATCH] mpl3115a2 move to px4 work queue --- src/drivers/barometer/mpl3115a2/mpl3115a2.cpp | 92 ++++++------------- 1 file changed, 30 insertions(+), 62 deletions(-) diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp index 6d8236057b..03193872b2 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp @@ -57,7 +57,7 @@ #include #include -#include +#include #include #include @@ -81,10 +81,6 @@ enum MPL3115A2_BUS { MPL3115A2_BUS_I2C_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) @@ -101,7 +97,7 @@ enum MPL3115A2_BUS { #define MPL3115A2_BARO_DEVICE_PATH_EXT "/dev/mpl3115a2_ext" #define MPL3115A2_BARO_DEVICE_PATH_INT "/dev/mpl3115a2_int" -class MPL3115A2 : public cdev::CDev +class MPL3115A2 : public cdev::CDev, public px4::ScheduledWorkItem { public: MPL3115A2(device::Device *interface, const char *path); @@ -120,8 +116,7 @@ public: protected: device::Device *_interface; - struct work_s _work; - unsigned _measure_ticks; + unsigned _measure_interval; ringbuffer::RingBuffer *_reports; bool _collect_phase; @@ -141,17 +136,17 @@ protected: /** * Initialize the automatic measurement state machine and start it. * - * @param delay_ticks the number of queue ticks before executing the next cycle + * @param delay_us the number of microseconds 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(unsigned delay_us = 1); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,15 +161,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. @@ -197,8 +184,9 @@ extern "C" __EXPORT int mpl3115a2_main(int argc, char *argv[]); MPL3115A2::MPL3115A2(device::Device *interface, const char *path) : CDev(path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), - _measure_ticks(0), + _measure_interval(0), _reports(nullptr), _collect_phase(false), _P(0), @@ -210,16 +198,13 @@ MPL3115A2::MPL3115A2(device::Device *interface, const char *path) : _measure_perf(perf_alloc(PC_ELAPSED, "mpl3115a2_measure")), _comms_errors(perf_alloc(PC_COUNT, "mpl3115a2_com_err")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - _interface->set_device_type(DRV_BARO_DEVTYPE_MPL3115A2); } MPL3115A2::~MPL3115A2() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); @@ -332,7 +317,7 @@ MPL3115A2::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. @@ -398,14 +383,14 @@ MPL3115A2::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(MPL3115A2_CONVERSION_INTERVAL); + _measure_interval = MPL3115A2_CONVERSION_INTERVAL; /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -414,22 +399,22 @@ MPL3115A2::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); + unsigned interval = (1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)) { + if (interval < MPL3115A2_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; @@ -457,33 +442,24 @@ MPL3115A2::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MPL3115A2::start_cycle(unsigned delay_ticks) +MPL3115A2::start(unsigned delay_us) { - /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MPL3115A2::cycle_trampoline, this, delay_ticks); + ScheduleDelayed(delay_us); } void -MPL3115A2::stop_cycle() +MPL3115A2::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -MPL3115A2::cycle_trampoline(void *arg) -{ - MPL3115A2 *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -MPL3115A2::cycle() +MPL3115A2::Run() { int ret; unsigned dummy; @@ -501,19 +477,15 @@ MPL3115A2::cycle() * to wait 2.8 ms after issuing the sensor reset command * according to the MPL3115A2 datasheet */ - start_cycle(USEC2TICK(2800)); + start(2800); return; } - if (ret == -EAGAIN) { /* Ready read it on next cycle */ - work_queue(HPWORK, - &_work, - (worker_t)&MPL3115A2::cycle_trampoline, - this, - USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)); + ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); + return; } @@ -529,7 +501,7 @@ MPL3115A2::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; } @@ -537,11 +509,7 @@ MPL3115A2::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MPL3115A2::cycle_trampoline, - this, - USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)); + ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); } int @@ -633,7 +601,7 @@ MPL3115A2::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"); sensor_baro_s brp = {};