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
This commit is contained in:
Beat Küng
2017-02-24 10:04:08 +01:00
committed by Lorenz Meier
parent 5ebbbfc6bf
commit 43b98d9a2f
6 changed files with 480 additions and 0 deletions

View File

@@ -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

32
msg/led_control.msg Normal file
View File

@@ -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)

61
src/drivers/drv_led.h Normal file
View File

@@ -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"

View File

@@ -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 :

199
src/lib/led/led.cpp Normal file
View File

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

147
src/lib/led/led.h Normal file
View File

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