From 22b89034d996a73d55188b60bf9a79ad4705018e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:42:46 -0500 Subject: [PATCH] ll40ls move to px4 work queue --- .../distance_sensor/ll40ls/LidarLite.cpp | 44 +++---------------- .../distance_sensor/ll40ls/LidarLite.h | 19 ++++---- .../distance_sensor/ll40ls/LidarLiteI2C.cpp | 36 ++++----------- .../distance_sensor/ll40ls/LidarLiteI2C.h | 15 ++----- .../distance_sensor/ll40ls/LidarLitePWM.cpp | 25 +++-------- .../distance_sensor/ll40ls/LidarLitePWM.h | 16 ++----- src/drivers/distance_sensor/ll40ls/ll40ls.cpp | 4 -- 7 files changed, 37 insertions(+), 122 deletions(-) diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp index d9e0022832..68869c9e68 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp @@ -41,38 +41,6 @@ #include "LidarLite.h" -LidarLite::LidarLite() : - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE_V3), - _measure_ticks(0) -{ -} - -void LidarLite::set_minimum_distance(const float min) -{ - _min_distance = min; -} - -void LidarLite::set_maximum_distance(const float max) -{ - _max_distance = max; -} - -float LidarLite::get_minimum_distance() const -{ - return _min_distance; -} - -float LidarLite::get_maximum_distance() const -{ - return _max_distance; -} - -uint32_t LidarLite::getMeasureTicks() const -{ - return _measure_ticks; -} - int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -87,10 +55,10 @@ int LidarLite::ioctl(device::file_t *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(LL40LS_CONVERSION_INTERVAL); + _measure_interval = (LL40LS_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -103,18 +71,18 @@ int LidarLite::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); /* check against maximum rate */ - if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + if (interval < (LL40LS_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) { diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h index 1355bba527..08b1ae3f39 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h @@ -57,8 +57,7 @@ class LidarLite { public: - LidarLite(); - + LidarLite() = default; virtual ~LidarLite() = default; virtual int init() = 0; @@ -89,12 +88,12 @@ protected: * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE * and LL40LS_MAX_DISTANCE_V3 */ - void set_minimum_distance(const float min); - void set_maximum_distance(const float max); - float get_minimum_distance() const; - float get_maximum_distance() const; + void set_minimum_distance(const float min) { _min_distance = min; } + void set_maximum_distance(const float max) { _max_distance = max; } + float get_minimum_distance() const { return _min_distance; } + float get_maximum_distance() const { return _max_distance; } - uint32_t getMeasureTicks() const; + uint32_t getMeasureInterval() const { return _measure_interval; } virtual int measure() = 0; virtual int collect() = 0; @@ -102,7 +101,7 @@ protected: virtual int reset_sensor() = 0; private: - float _min_distance; - float _max_distance; - uint32_t _measure_ticks; + float _min_distance{LL40LS_MIN_DISTANCE}; + float _max_distance{LL40LS_MAX_DISTANCE_V3}; + uint32_t _measure_interval{0}; }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index e16effa511..d4932de393 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -52,8 +52,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) : I2C("LL40LS", path, bus, address, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _rotation(rotation), - _work{}, _reports(nullptr), _sensor_ok(false), _collect_phase(false), @@ -74,9 +74,6 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr { // up the retries since the device misses the first measure attempts _retries = 3; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); } LidarLiteI2C::~LidarLiteI2C() @@ -254,7 +251,7 @@ ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (getMeasureTicks() > 0) { + if (getMeasureInterval() > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -570,22 +567,15 @@ void LidarLiteI2C::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); + ScheduleNow(); } void LidarLiteI2C::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } -void LidarLiteI2C::cycle_trampoline(void *arg) -{ - LidarLiteI2C *dev = (LidarLiteI2C *)arg; - - dev->cycle(); -} - -void LidarLiteI2C::cycle() +void LidarLiteI2C::Run() { /* collection phase? */ if (_collect_phase) { @@ -607,14 +597,10 @@ void LidarLiteI2C::cycle() /* * Is there a collect->measure gap? */ - if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + if (getMeasureInterval() > (LL40LS_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + ScheduleDelayed(getMeasureInterval() - LL40LS_CONVERSION_INTERVAL); return; } @@ -635,11 +621,7 @@ void LidarLiteI2C::cycle() } /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + ScheduleDelayed(LL40LS_CONVERSION_INTERVAL); } void LidarLiteI2C::print_info() @@ -648,7 +630,7 @@ void LidarLiteI2C::print_info() perf_print_counter(_comms_errors); perf_print_counter(_sensor_resets); perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", getMeasureTicks()); + printf("poll interval: %u \n", getMeasureInterval()); _reports->print_info("report queue"); printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 2f7dffb2c9..343a778845 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -42,7 +42,7 @@ #include "LidarLite.h" -#include +#include #include @@ -78,7 +78,7 @@ #define LL40LS_PEAK_STRENGTH_LOW 135 // Minimum peak strength raw value for accepting a measurement #define LL40LS_PEAK_STRENGTH_HIGH 234 // Max peak strength raw value -class LidarLiteI2C : public LidarLite, public device::I2C +class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem { public: LidarLiteI2C(int bus, const char *path, @@ -113,7 +113,6 @@ protected: private: uint8_t _rotation; - work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; bool _collect_phase; @@ -173,17 +172,9 @@ private: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); + void Run() override; int collect() override; - /** - * 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); - private: LidarLiteI2C(const LidarLiteI2C ©) = delete; LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp index bd0ee07634..a1340a9d2d 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp @@ -51,8 +51,8 @@ LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) : CDev(path), + ScheduledWorkItem(px4::wq_configurations::hp_default), _rotation(rotation), - _work{}, _reports(nullptr), _class_instance(-1), _orb_class_instance(-1), @@ -122,7 +122,7 @@ void LidarLitePWM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_read_errors); perf_print_counter(_sensor_zero_resets); - PX4_INFO("poll interval: %u ticks", getMeasureTicks()); + PX4_INFO("poll interval: %u ", getMeasureInterval()); print_message(_range); } @@ -135,31 +135,20 @@ void LidarLitePWM::print_registers() void LidarLitePWM::start() { /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1); + ScheduleNow(); } void LidarLitePWM::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } -void LidarLitePWM::cycle_trampoline(void *arg) -{ - LidarLitePWM *dev = (LidarLitePWM *)arg; - - dev->cycle(); -} - -void LidarLitePWM::cycle() +void LidarLitePWM::Run() { measure(); /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLitePWM::cycle_trampoline, - this, - getMeasureTicks()); + ScheduleDelayed(getMeasureInterval()); } int LidarLitePWM::measure() @@ -213,7 +202,7 @@ ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (getMeasureTicks() > 0) { + if (getMeasureInterval() > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h index a2249b818a..e7809271ef 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h @@ -45,8 +45,7 @@ #include "LidarLite.h" -#include - +#include #include #include @@ -57,7 +56,7 @@ -class LidarLitePWM : public LidarLite, public cdev::CDev +class LidarLitePWM : public LidarLite, public cdev::CDev, public px4::ScheduledWorkItem { public: LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); @@ -72,7 +71,7 @@ public: void stop() override; - void cycle(); + void Run() override; /** * @brief @@ -86,14 +85,6 @@ public: */ void print_registers() override; - /** - * 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); - const char *get_dev_name() override; protected: @@ -108,7 +99,6 @@ protected: private: uint8_t _rotation; - work_s _work; ringbuffer::RingBuffer *_reports; int _class_instance; int _orb_class_instance; diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 515d4558a3..41e7aff1fb 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -51,10 +51,6 @@ #include #include -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - #define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" enum LL40LS_BUS {