led: add breathe mode

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();
 	}
 

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();
 	}
 

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:

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
 };
 

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 <color>] [-l <led>] [-n <num_blink>] [-s <speed>] [-p <prio>]\n"
+		"led_control {on, off, reset, blink, breathe} [-c <color>] [-l <led>] [-n <num_blink>] [-s <speed>] [-p <prio>]\n"
 		"\n"
 		"\t-c <color>\t\tColor (red,blue,green,yellow,purple,amber,cyan,white) (default=white)\n"
 		"\t-l <led>\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;