From cdb207804dc23df7618720f940833b72970badff Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:48:05 -0500 Subject: [PATCH] irlock move to px4 work queue --- src/drivers/irlock/irlock.cpp | 34 ++++++++-------------------------- 1 file changed, 8 insertions(+), 26 deletions(-) diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index d5393a9d2e..4c61ea5a9d 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include #include #include @@ -86,10 +86,6 @@ #define IRLOCK_TAN_ANG_PER_PIXEL_X (2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X) #define IRLOCK_TAN_ANG_PER_PIXEL_Y (2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y) -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - #define IRLOCK_BASE_DEVICE_PATH "/dev/irlock" #define IRLOCK0_DEVICE_PATH "/dev/irlock0" @@ -110,7 +106,7 @@ struct irlock_s { struct irlock_target_s targets[IRLOCK_OBJECTS_MAX]; }; -class IRLOCK : public device::I2C +class IRLOCK : public device::I2C, public px4::ScheduledWorkItem { public: IRLOCK(int bus = IRLOCK_I2C_BUS, int address = IRLOCK_I2C_ADDRESS); @@ -131,11 +127,8 @@ private: /** stop periodic reads from sensor **/ void stop(); - /** static function that is called by worker queue, arg will be pointer to instance of this class **/ - static void cycle_trampoline(void *arg); - /** read from device and schedule next read **/ - void cycle(); + void Run() override; /** low level communication with sensor **/ int read_device(); @@ -146,7 +139,6 @@ private: /** internal variables **/ ringbuffer::RingBuffer *_reports; bool _sensor_ok; - work_s _work; uint32_t _read_failures; int _orb_class_instance; @@ -166,13 +158,13 @@ extern "C" __EXPORT int irlock_main(int argc, char *argv[]); /** constructor **/ IRLOCK::IRLOCK(int bus, int address) : I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _reports(nullptr), _sensor_ok(false), _read_failures(0), _orb_class_instance(-1), _irlock_report_topic(nullptr) { - memset(&_work, 0, sizeof(_work)); } /** destructor **/ @@ -293,32 +285,22 @@ void IRLOCK::start() _reports->flush(); /** start work queue cycle **/ - work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); + ScheduleNow(); } /** stop periodic reads from sensor **/ void IRLOCK::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } -void IRLOCK::cycle_trampoline(void *arg) -{ - IRLOCK *device = (IRLOCK *)arg; - - /** check global irlock reference and cycle **/ - if (g_irlock != nullptr) { - device->cycle(); - } -} - -void IRLOCK::cycle() +void IRLOCK::Run() { /** ignoring failure, if we do, we will be back again right away... **/ read_device(); /** schedule the next cycle **/ - work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL_US)); + ScheduleDelayed(IRLOCK_CONVERSION_INTERVAL_US); } ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen)