diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index 1f5f873457..5e19963a8b 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/src/drivers/lights/rgbled/rgbled.cpp @@ -55,7 +55,7 @@ #include #include -#include +#include #include #include @@ -81,7 +81,7 @@ #define SETTING_ENABLE 0x02 /**< on */ -class RGBLED : public device::I2C +class RGBLED : public device::I2C, public px4::ScheduledWorkItem { public: RGBLED(int bus, int rgbled); @@ -93,8 +93,6 @@ public: int status(); private: - work_s _work; - float _brightness; float _max_brightness; @@ -108,8 +106,7 @@ private: LedController _led_controller; - static void led_trampoline(void *arg); - void led(); + void Run() override; int send_led_enable(bool enable); int send_led_rgb(); @@ -128,12 +125,8 @@ void rgbled_usage(); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : - I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled -#ifdef __PX4_NUTTX - , 100000 /* maximum speed supported */ -#endif - ), - _work{}, + I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _brightness(1.0f), _max_brightness(1.0f), _r(0), @@ -173,8 +166,9 @@ RGBLED::init() update_params(); _running = true; + // kick off work queue - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 0); + ScheduleNow(); return OK; } @@ -230,19 +224,11 @@ RGBLED::status() return ret; } -void -RGBLED::led_trampoline(void *arg) -{ - RGBLED *rgbl = reinterpret_cast(arg); - - rgbl->led(); -} - /** * Main loop function */ void -RGBLED::led() +RGBLED::Run() { if (!_should_run) { if (_param_sub >= 0) { @@ -332,10 +318,8 @@ RGBLED::led() send_led_rgb(); } - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, - USEC2TICK(_led_controller.maximum_update_interval())); + ScheduleDelayed(_led_controller.maximum_update_interval()); } /**