diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 23c97ce5ed..e0cf34352c 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -325,6 +325,8 @@ RGBLED::led() break; } + _brightness = (float)led_control_data.leds[0].brightness / 255.f; + send_led_rgb(); } diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp index 5abba42da9..9800c9280b 100644 --- a/src/drivers/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp @@ -247,6 +247,8 @@ RGBLED_PWM::led() break; } + _brightness = (float)led_control_data.leds[0].brightness / 255.f; + send_led_rgb(); } diff --git a/src/lib/led/led.cpp b/src/lib/led/led.cpp index db177a6ef2..9daca56cea 100644 --- a/src/lib/led/led.cpp +++ b/src/lib/led/led.cpp @@ -75,6 +75,7 @@ int LedController::update(LedControlData &control_data) // handle state updates hrt_abstime now = hrt_absolute_time(); uint16_t blink_delta_t = (uint16_t)((now - _last_update_call) / 100); // Note: this is in 0.1ms + constexpr uint16_t breathe_duration = BREATHE_INTERVAL * BREATHE_STEPS / 100; int num_blinking_leds = 0; int num_blinking_do_not_change_state = 0; @@ -106,6 +107,16 @@ int LedController::update(LedControlData &control_data) case led_control_s::MODE_BLINK_SLOW: current_blink_duration = BLINK_SLOW_DURATION / 100; break; + + case led_control_s::MODE_BREATHE: + _states[i].current_blinking_time += blink_delta_t; + + while (_states[i].current_blinking_time > breathe_duration) { + _states[i].current_blinking_time -= breathe_duration; + } + + had_changes = true; + break; } if (current_blink_duration > 0) { @@ -186,8 +197,11 @@ int LedController::update(LedControlData &control_data) void LedController::get_control_data(LedControlData &control_data) { + _breathe_enabled = false; + for (int i = 0; i < BOARD_MAX_LEDS; ++i) { control_data.leds[i].color = led_control_s::COLOR_OFF; // set output to a defined state + control_data.leds[i].brightness = 255; for (int priority = led_control_s::MAX_PRIORITY; priority >= 0; --priority) { const PerPriorityData &cur_data = _states[i].priority[priority]; @@ -198,10 +212,19 @@ void LedController::get_control_data(LedControlData &control_data) switch (cur_data.mode) { case led_control_s::MODE_ON: - case led_control_s::MODE_BREATHE: // TODO: handle this properly control_data.leds[i].color = cur_data.color; break; + case led_control_s::MODE_BREATHE: { + // fade on and off + int counter = _states[i].current_blinking_time / (BREATHE_INTERVAL / 100); + int n = counter >= (BREATHE_STEPS / 2) ? BREATHE_STEPS - counter : counter; + control_data.leds[i].brightness = (n * n) * 255 / (BREATHE_STEPS * BREATHE_STEPS / 4); // (n/(steps/2))^2 + control_data.leds[i].color = cur_data.color; + _breathe_enabled = true; + break; + } + case led_control_s::MODE_BLINK_FAST: case led_control_s::MODE_BLINK_NORMAL: case led_control_s::MODE_BLINK_SLOW: diff --git a/src/lib/led/led.h b/src/lib/led/led.h index 39e153c8f0..1de903c8ef 100644 --- a/src/lib/led/led.h +++ b/src/lib/led/led.h @@ -46,7 +46,8 @@ struct LedControlDataSingle { - uint8_t color; + uint8_t color; ///< one of led_control_s::COLOR_* + uint8_t brightness; ///< brightness in [0, 255] }; struct LedControlData { LedControlDataSingle leds[BOARD_MAX_LEDS]; @@ -80,7 +81,7 @@ public: */ int maximum_update_interval() const { - return BLINK_FAST_DURATION; + return _breathe_enabled ? BREATHE_INTERVAL : BLINK_FAST_DURATION; } /** @@ -91,6 +92,9 @@ public: */ int update(LedControlData &control_data); + static const int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */ + static const int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */ + static const int BLINK_FAST_DURATION = 100 * 1000; /**< duration of half a blinking cycle (on-to-off and off-to-on) in us */ static const int BLINK_NORMAL_DURATION = 500 * 1000; /**< duration of half a blinking cycle @@ -108,8 +112,8 @@ private: struct PerPriorityData { uint8_t color = 0; ///< one of led_control_s::COLOR_* uint8_t mode = led_control_s::MODE_DISABLED; ///< one of led_control_s::MODE_* - uint8_t blink_times_left = 0; ///< how many times left to blink (MSB bit is used for infinite case). - /// This limits the number of complete blink cycles to 64 (if not infinite) + uint8_t blink_times_left = 0; /**< how many times left to blink (MSB bit is used for infinite case). + This limits the number of complete blink cycles to 64 (if not infinite) */ }; struct NextState { @@ -144,5 +148,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 + bool _breathe_enabled = false; ///< true if at least one of the led's is currently in breathe mode }; diff --git a/src/systemcmds/led_control/led_control.cpp b/src/systemcmds/led_control/led_control.cpp index f436757810..4e9254a801 100644 --- a/src/systemcmds/led_control/led_control.cpp +++ b/src/systemcmds/led_control/led_control.cpp @@ -60,7 +60,7 @@ usage() PX4_INFO( "External Led control for testing. Usage:\n" "led_control test\t\tRun test pattern\n" - "led_control {on, off, reset, blink} [-c ] [-l ] [-n ] [-s ] [-p ]\n" + "led_control {on, off, reset, blink, breathe} [-c ] [-l ] [-n ] [-s ] [-p ]\n" "\n" "\t-c \t\tColor (red,blue,green,yellow,purple,amber,cyan,white) (default=white)\n" "\t-l \t\tWhich led to control (0,1,...) (default=all)\n" @@ -237,6 +237,9 @@ led_control_main(int argc, char *argv[]) } else if (!strcmp(argv[myoptind], "blink")) { led_control.mode = blink_speed; + } else if (!strcmp(argv[myoptind], "breathe")) { + led_control.mode = led_control_s::MODE_BREATHE; + } else { usage(); return 1;