diff --git a/src/lib/led/led.cpp b/src/lib/led/led.cpp index 97c0a4ff54..6ddd81b468 100644 --- a/src/lib/led/led.cpp +++ b/src/lib/led/led.cpp @@ -51,18 +51,22 @@ int LedController::update(LedControlData &control_data) orb_check(_led_control_sub, &updated); - while (updated) { + while (updated || _force_update) { // handle new state led_control_s led_control; - orb_copy(ORB_ID(led_control), _led_control_sub, &led_control); - // don't apply the new state just yet to avoid interrupting an ongoing blinking state - for (int i = 0; i < BOARD_MAX_LEDS; ++i) { - if (led_control.led_mask & (1 << i)) { - _states[i].next_state.set(led_control); + if (orb_copy(ORB_ID(led_control), _led_control_sub, &led_control) == 0) { + + // don't apply the new state just yet to avoid interrupting an ongoing blinking state + for (int i = 0; i < BOARD_MAX_LEDS; ++i) { + if (led_control.led_mask & (1 << i)) { + _states[i].next_state.set(led_control); + } } } + _force_update = false; + orb_check(_led_control_sub, &updated); } diff --git a/src/lib/led/led.h b/src/lib/led/led.h index b22e6f747b..39e153c8f0 100644 --- a/src/lib/led/led.h +++ b/src/lib/led/led.h @@ -143,5 +143,6 @@ private: int _led_control_sub = -1; ///< uorb subscription hrt_abstime _last_update_call; + bool _force_update = true; ///< force an orb_copy in the beginning };