From 835a38f6dc0a684200753d5fd9417f5af862e49d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:44:41 -0500 Subject: [PATCH] vl53lxx move to px4 work queue --- .../distance_sensor/vl53lxx/vl53lxx.cpp | 89 ++++++------------- 1 file changed, 25 insertions(+), 64 deletions(-) diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index a9fde0422e..57df0c09c5 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include @@ -101,11 +101,7 @@ #define VL53LXX_MAX_RANGING_DISTANCE 2.0f #define VL53LXX_MIN_RANGING_DISTANCE 0.0f -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class VL53LXX : public device::I2C +class VL53LXX : public device::I2C, public px4::ScheduledWorkItem { public: VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, @@ -113,11 +109,11 @@ public: virtual ~VL53LXX(); - virtual int init(); + virtual int init() override; - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. @@ -125,14 +121,13 @@ public: void print_info(); protected: - virtual int probe(); + virtual int probe() override; private: uint8_t _rotation; - work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_interval; bool _collect_phase; bool _new_measurement; bool _measurement_started; @@ -162,7 +157,7 @@ private: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); + void Run() override; int measure(); int collect(); @@ -176,15 +171,6 @@ private: bool spadCalculations(); bool sensorTuning(); bool singleRefCalibration(uint8_t byte); - - /** - * 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); - }; @@ -195,10 +181,11 @@ extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]); VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _rotation(rotation), _reports(nullptr), _sensor_ok(false), - _measure_ticks(0), + _measure_interval(0), _collect_phase(false), _new_measurement(true), _measurement_started(false), @@ -211,12 +198,6 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : { // up the retries since the device misses the first measure attempts I2C::_retries = 3; - - // enable debug() calls - _debug_enabled = false; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); } VL53LXX::~VL53LXX() @@ -351,10 +332,10 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) 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(VL53LXX_SAMPLE_RATE); + _measure_interval = (VL53LXX_SAMPLE_RATE); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -369,13 +350,13 @@ VL53LXX::ioctl(device::file_t *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); /* 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) { @@ -407,7 +388,7 @@ VL53LXX::read(device::file_t *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. @@ -585,8 +566,8 @@ VL53LXX::measure() readRegister(SYSRANGE_START_REG, system_start); if ((system_start & 0x01) == 1) { - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready + ScheduleDelayed(VL53LXX_US); + ret = OK; return ret; @@ -601,8 +582,7 @@ VL53LXX::measure() readRegister(SYSRANGE_START_REG, system_start); if ((system_start & 0x01) == 1) { - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready + ScheduleDelayed(VL53LXX_US); ret = OK; return ret; @@ -614,8 +594,7 @@ VL53LXX::measure() readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); if ((wait_for_measurement & 0x07) == 0) { - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready + ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready ret = OK; return ret; } @@ -699,28 +678,17 @@ VL53LXX::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_US)); + ScheduleDelayed(VL53LXX_US); } - void VL53LXX::stop() { - work_cancel(LPWORK, &_work); + ScheduleClear(); } - void -VL53LXX::cycle_trampoline(void *arg) -{ - VL53LXX *dev = (VL53LXX *)arg; - - dev->cycle(); -} - - -void -VL53LXX::cycle() +VL53LXX::Run() { measure(); @@ -731,26 +699,19 @@ VL53LXX::cycle() collect(); - work_queue(LPWORK, - &_work, - (worker_t)&VL53LXX::cycle_trampoline, - this, - _measure_ticks); + ScheduleDelayed(_measure_interval); } - } - void VL53LXX::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"); } - bool VL53LXX::spadCalculations() {