diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 924624cebb..f4f4e41427 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,6 +4,7 @@ MODULE_COMMAND = rgbled -SRCS = rgbled.cpp +SRCS = rgbled.cpp \ + rgbled_params.c MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 8b5be151de..d379b20b60 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -101,11 +101,13 @@ private: uint8_t _g; uint8_t _b; float _brightness; + float _max_brightness; bool _running; int _led_interval; bool _should_run; int _counter; + int _param_sub; void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); @@ -117,6 +119,7 @@ private: int send_led_enable(bool enable); int send_led_rgb(); int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); + void update_params(); }; /* for now, we only support one RGBLED */ @@ -140,10 +143,12 @@ RGBLED::RGBLED(int bus, int rgbled) : _g(0), _b(0), _brightness(1.0f), + _max_brightness(1.0f), _running(false), _led_interval(0), _should_run(false), - _counter(0) + _counter(0), + _param_sub(-1) { memset(&_work, 0, sizeof(_work)); memset(&_pattern, 0, sizeof(_pattern)); @@ -285,6 +290,22 @@ RGBLED::led() return; } + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -546,9 +567,9 @@ RGBLED::send_led_rgb() { /* To scale from 0..255 -> 0..15 shift right by 4 bits */ const uint8_t msg[6] = { - SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), - SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), - SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) }; return transfer(msg, sizeof(msg), nullptr, 0); } @@ -573,6 +594,21 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } +void +RGBLED::update_params() +{ + int32_t maxbrt = 15; + param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 15 ? 15 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + // A minimum of 2 "on" steps is required for breathe effect + if (maxbrt == 1) { + maxbrt = 2; + } + _max_brightness = maxbrt / 15.0f; +} + void rgbled_usage() { diff --git a/src/drivers/rgbled/rgbled_params.c b/src/drivers/rgbled/rgbled_params.c new file mode 100644 index 0000000000..c43aaba8a5 --- /dev/null +++ b/src/drivers/rgbled/rgbled_params.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 rgbled_params.c + * + * Parameters defined by the RBG led driver + * + * @author Nate Weibley + */ + + +#include +#include + +/** + * RGB Led brightness limit + * + * Set to 0 to disable, 1 for minimum brightness up to 15 (max) + * + * @min 0 + * @max 15 + */ +PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15);