Merge pull request #2876 from NaterGator/ledlimit

Add parameter to limit the maximum brightness of the RGB LED

src/drivers/rgbled/module.mk

@@ -4,6 +4,7 @@
 
 MODULE_COMMAND	 = rgbled
 
-SRCS		 = rgbled.cpp
+SRCS		 = rgbled.cpp \
+			   rgbled_params.c
 
 MAXOPTIMIZATION	 = -Os

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_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
-		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
-		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_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()
 {

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 <nate.weibley@prioria.com>
+ */
+
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * 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);