diff --git a/src/drivers/lights/blinkm/blinkm.cpp b/src/drivers/lights/blinkm/blinkm.cpp index 12876b19bc..a53f10651b 100644 --- a/src/drivers/lights/blinkm/blinkm.cpp +++ b/src/drivers/lights/blinkm/blinkm.cpp @@ -108,7 +108,7 @@ #include #include -#include +#include #include #include @@ -131,7 +131,7 @@ static const int LED_OFFTIME = 120; static const int LED_BLINK = 1; static const int LED_NOBLINK = 0; -class BlinkM : public device::I2C +class BlinkM : public device::I2C, public px4::ScheduledWorkItem { public: BlinkM(int bus, int blinkm); @@ -181,8 +181,6 @@ private: LED_AMBER }; - work_s _work; - int led_color_1; int led_color_2; int led_color_3; @@ -216,8 +214,8 @@ private: int num_of_used_sats; void setLEDColor(int ledcolor); - static void led_trampoline(void *arg); - void led(); + + void Run() override; int set_rgb(uint8_t r, uint8_t g, uint8_t b); @@ -276,11 +274,8 @@ const char *const BlinkM::script_names[] = { extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : - I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm -#ifdef __PX4_NUTTX - , 100000 -#endif - ), + I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), @@ -308,14 +303,12 @@ BlinkM::BlinkM(int bus, int blinkm) : led_thread_ready(true), num_of_used_sats(0) { - memset(&_work, 0, sizeof(_work)); } int BlinkM::init() { - int ret; - ret = I2C::init(); + int ret = I2C::init(); if (ret != OK) { warnx("I2C init failed"); @@ -336,7 +329,7 @@ BlinkM::setMode(int mode) stop_script(); set_rgb(0, 0, 0); systemstate_run = true; - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); + ScheduleNow(); } } else { @@ -415,17 +408,7 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) void -BlinkM::led_trampoline(void *arg) -{ - BlinkM *bm = (BlinkM *)arg; - - bm->led(); -} - - - -void -BlinkM::led() +BlinkM::Run() { if (!topic_initialized) { @@ -790,7 +773,7 @@ BlinkM::led() if (systemstate_run == true) { /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + ScheduleDelayed(led_interval); } else { stop_script();