lib/led: add led class with led_control uorb topic

This is the new interface to control external LED's (user-facing). Features: - Supports maximum N Leds (where the board can define N) - on/off/blink M times - Different priorities - Allows setting a single led or multiple at once
2026-05-21 19:07:35 +08:00 · 2017-02-24 10:04:08 +01:00
parent 5ebbbfc6bf
commit 43b98d9a2f
6 changed files with 480 additions and 0 deletions
@@ -67,6 +67,7 @@ set(msg_file_names
 	hil_sensor.msg
 	home_position.msg
 	input_rc.msg
+	led_control.msg
 	log_message.msg
 	manual_control_setpoint.msg
 	mavlink_log.msg
@@ -0,0 +1,32 @@
+# LED control: control a single or multiple LED's.
+# These are the externally visible LED's, not the board LED's
+
+# colors
+uint8 COLOR_OFF = 0 # this is only used in the drivers
+uint8 COLOR_RED = 1
+uint8 COLOR_GREEN = 2
+uint8 COLOR_BLUE = 3
+uint8 COLOR_YELLOW = 4
+uint8 COLOR_PURPLE = 5
+uint8 COLOR_AMBER = 6
+uint8 COLOR_CYAN = 7
+uint8 COLOR_WHITE = 8
+
+# LED modes definitions
+uint8 MODE_OFF = 0 # turn LED off
+uint8 MODE_ON = 1  # turn LED on
+uint8 MODE_DISABLED = 2  # disable this priority (switch to lower priority setting)
+uint8 MODE_BLINK_SLOW = 3
+uint8 MODE_BLINK_NORMAL = 4
+uint8 MODE_BLINK_FAST = 5
+# uint8 MODE_BREATHE = 6 # not implemented (yet)
+
+uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0)
+
+
+uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all
+uint8 color # see COLOR_*
+uint8 mode # see MODE_*
+uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*). Set to 0 for infinite
+uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY)
+
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_led.h
+ *
+ * Led device API to control the external LED(s) via uORB interface
+ */
+
+#pragma once
+
+
+#include <uORB/topics/led_control.h>
+
+#include <board_config.h>
+
+// allow the board to override the number (or maxiumum number) of LED's it has
+#ifndef BOARD_MAX_LEDS
+#define BOARD_MAX_LEDS 4
+#endif
+
+#if BOARD_MAX_LEDS > 8 // because led_mask is uint8_t
+#error "BOARD_MAX_LEDS too large. You need to change the led_mask type in the led_control uorb topic (and where it's used)"
+#endif
+
+
+// set the queue size to the number of LED's, so that each led can be controlled individually
+static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS;
+
+
+#define RGBLED0_DEVICE_PATH "/dev/rgbled0"
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__led
+	SRCS
+		led.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file led.cpp
+ */
+
+
+#include "led.h"
+
+int LedController::init(int led_control_sub)
+{
+	_led_control_sub = led_control_sub;
+	_last_update_call = hrt_absolute_time();
+	return 0;
+}
+
+int LedController::update(LedControlData &control_data)
+{
+	bool updated = false;
+
+	orb_check(_led_control_sub, &updated);
+
+	while (updated) {
+		// 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);
+			}
+		}
+
+		orb_check(_led_control_sub, &updated);
+	}
+
+	bool had_changes = false; // did one of the outputs change?
+
+	// handle state updates
+	hrt_abstime now = hrt_absolute_time();
+	uint16_t blink_delta_t = (uint16_t)((now - _last_update_call) / 100);
+
+	for (int i = 0; i < BOARD_MAX_LEDS; ++i) {
+		bool do_not_change_state = false;
+		int priority = led_control_s::MAX_PRIORITY;
+
+		for (; priority >= 0; --priority) {
+
+			PerPriorityData &cur_data = _states[i].priority[priority];
+
+			if (cur_data.mode == led_control_s::MODE_DISABLED) {
+				continue; // handle next priority
+			}
+
+			// handle state updates
+			uint16_t current_blink_duration = 0;
+
+			switch (cur_data.mode) {
+			case led_control_s::MODE_BLINK_FAST:
+				current_blink_duration = BLINK_FAST_DURATION / 100;
+				break;
+
+			case led_control_s::MODE_BLINK_NORMAL:
+				current_blink_duration = BLINK_NORMAL_DURATION / 100;
+				break;
+
+			case led_control_s::MODE_BLINK_SLOW:
+				current_blink_duration = BLINK_SLOW_DURATION / 100;
+				break;
+			}
+
+			if (current_blink_duration > 0) {
+				if ((_states[i].current_blinking_time += blink_delta_t) > current_blink_duration) {
+					_states[i].current_blinking_time -= current_blink_duration;
+
+					if (cur_data.blink_times_left == 254) {
+						// handle toggling for infinite case
+						cur_data.blink_times_left = 255;
+						do_not_change_state = true;
+
+					} else if (cur_data.blink_times_left == 255) {
+						cur_data.blink_times_left = 254;
+
+					} else if (--cur_data.blink_times_left == 0) { //TODO: 1 to ignore last off duration?
+						cur_data.mode = led_control_s::MODE_DISABLED;
+						_states[i].current_blinking_time = 0;
+
+					} else if (cur_data.blink_times_left % 2 == 1) {
+						do_not_change_state = true;
+					}
+
+					had_changes = true;
+
+				} else {
+					do_not_change_state = true;
+				}
+			}
+
+			break; // handle next led
+		}
+
+		// handle next state
+		if (!do_not_change_state && _states[i].next_state.is_valid()) {
+			uint8_t next_priority = _states[i].next_state.priority;
+
+			if ((int)next_priority >= priority) {
+				_states[i].current_blinking_time = 0;
+				had_changes = true;
+			}
+
+			_states[i].priority[next_priority].color = _states[i].next_state.color;
+			_states[i].priority[next_priority].mode = _states[i].next_state.mode;
+			_states[i].priority[next_priority].blink_times_left = _states[i].next_state.num_blinks * 2;
+
+			if (_states[i].priority[next_priority].blink_times_left == 0) {
+				// handle infinite case
+				_states[i].priority[next_priority].blink_times_left = 254;
+			}
+
+			_states[i].next_state.reset();
+		}
+	}
+
+	_last_update_call = now;
+
+	if (!had_changes) {
+		return 0;
+	}
+
+	// create output
+	get_control_data(control_data);
+
+	return 1;
+}
+
+void LedController::get_control_data(LedControlData &control_data)
+{
+	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
+
+		for (int priority = led_control_s::MAX_PRIORITY; priority >= 0; --priority) {
+			const PerPriorityData &cur_data = _states[i].priority[priority];
+
+			if (cur_data.mode == led_control_s::MODE_DISABLED) {
+				continue; // handle next priority
+			}
+
+			switch (cur_data.mode) {
+			case led_control_s::MODE_ON:
+				control_data.leds[i].color = cur_data.color;
+				break;
+
+			case led_control_s::MODE_BLINK_FAST:
+			case led_control_s::MODE_BLINK_NORMAL:
+			case led_control_s::MODE_BLINK_SLOW:
+				if (cur_data.blink_times_left % 2 == 0) {
+					control_data.leds[i].color = cur_data.color;
+				}
+
+				break;
+				// MODE_OFF does not need to be handled, it's already set above
+			}
+
+			break; // handle next led
+		}
+	}
+}
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file led.h
+ *
+ * Led controller helper class, used by Led drivers
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+
+struct LedControlDataSingle {
+	uint8_t color;
+};
+struct LedControlData {
+	LedControlDataSingle leds[BOARD_MAX_LEDS];
+};
+
+
+/**
+ ** class LedController
+ * Handles the led_control topic: blinking, priorities and state updates.
+ */
+class LedController
+{
+public:
+	LedController() = default;
+	~LedController() = default;
+
+	/**
+	 * initialize. Call this once before using the object
+	 * @param led_control_sub uorb subscription for led_control
+	 * @return 0 on success, <0 on error otherwise
+	 */
+	int init(int led_control_sub);
+
+	/**
+	 * check if already initialized
+	 */
+	bool is_init() const { return _led_control_sub >= 0; }
+
+	/**
+	 * get maxium time between two consecutive calls to update() in us.
+	 */
+	int maximum_update_interval() const
+	{
+		return BLINK_FAST_DURATION;
+	}
+
+	/**
+	 * Update and retrieve the Led state. It will do the orb_copy() and needs to be called at least every
+	 * maximum_update_interval(). In addition a caller might poll on the led_control_sub
+	 * @param control_data output structure (will always be set)
+	 * @return 1 if control_data set (state changed), 0 if control_data not changed (state did not change), <0 error otherwise
+	 */
+	int update(LedControlData &control_data);
+
+	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 (on-to-off and off-to-on) in us
+	static const int BLINK_SLOW_DURATION = 2000 *
+					       1000; ///< duration of half a blinking cycle (on-to-off and off-to-on) in us
+
+	int led_control_subscription() const { return _led_control_sub; }
+
+private:
+
+	/** set control_data based on current Led states */
+	inline void get_control_data(LedControlData &control_data);
+
+	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)
+	};
+
+	struct NextState {
+		uint8_t color;
+		uint8_t mode;
+		uint8_t num_blinks;
+		uint8_t priority = led_control_s::MAX_PRIORITY + 1;
+
+		void set(const led_control_s &led_control)
+		{
+			color = led_control.color;
+			mode = led_control.mode;
+			num_blinks = led_control.num_blinks;
+			priority = led_control.priority;
+
+			if (priority > led_control_s::MAX_PRIORITY) {
+				priority = led_control_s::MAX_PRIORITY;
+			}
+		}
+		void reset() { priority = led_control_s::MAX_PRIORITY + 1; }
+		bool is_valid() const { return priority != led_control_s::MAX_PRIORITY + 1; }
+	};
+
+	struct PerLedData {
+		PerPriorityData priority[led_control_s::MAX_PRIORITY + 1];
+		uint16_t current_blinking_time = 0; ///< how long the Led was in current state (in 0.1 ms)
+		NextState next_state;
+	};
+
+	PerLedData _states[BOARD_MAX_LEDS]; ///< keep current LED states
+
+	int _led_control_sub = -1; ///< uorb subscription
+	hrt_abstime _last_update_call;
+};
+