drivers create lights category (directory)

src/drivers/lights/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2018 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.
+#
+############################################################################
+
+add_subdirectory(blinkm)
+add_subdirectory(rgbled)
+add_subdirectory(rgbled_ncp5623c)
+add_subdirectory(rgbled_pwm)

src/drivers/lights/blinkm/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__blinkm
+	MAIN blinkm
+	COMPILE_FLAGS
+	SRCS
+		blinkm.cpp
+	DEPENDS
+	)
+

src/drivers/lights/oreoled/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__oreoled
+	MAIN oreoled
+	STACK_MAIN 1024
+	COMPILE_FLAGS
+	SRCS
+		oreoled.cpp
+	DEPENDS
+	)
+

src/drivers/lights/rgbled/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__rgbled
+	MAIN rgbled
+	STACK_MAIN
+		1500
+	SRCS
+		rgbled.cpp
+	DEPENDS
+		drivers__device
+		led
+	)

src/drivers/lights/rgbled/rgbled.cpp

@@ -0,0 +1,527 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ * @author Julian Oes <julian@px4.io>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <px4_workqueue.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_led.h>
+#include <lib/led/led.h>
+
+#include "uORB/topics/parameter_update.h"
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR			0x55	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+class RGBLED : public device::I2C
+{
+public:
+	RGBLED(int bus, int rgbled);
+	virtual ~RGBLED();
+
+
+	virtual int		init();
+	virtual int		probe();
+	int		status();
+
+private:
+	work_s			_work;
+
+	float			_brightness;
+	float			_max_brightness;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+	volatile bool		_running;
+	volatile bool		_should_run;
+	bool			_leds_enabled;
+	int			_param_sub;
+
+	LedController		_led_controller;
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	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 */
+namespace
+{
+RGBLED *g_rgbled = nullptr;
+}
+
+void rgbled_usage();
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+	I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled
+#ifdef __PX4_NUTTX
+	    , 100000 /* maximum speed supported */
+#endif
+	   ),
+	_work{},
+	_brightness(1.0f),
+	_max_brightness(1.0f),
+	_r(0),
+	_g(0),
+	_b(0),
+	_running(false),
+	_should_run(true),
+	_leds_enabled(true),
+	_param_sub(-1)
+{
+}
+
+RGBLED::~RGBLED()
+{
+	_should_run = false;
+	int counter = 0;
+
+	while (_running && ++counter < 10) {
+		usleep(100000);
+	}
+}
+
+int
+RGBLED::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
+
+	update_params();
+
+	_running = true;
+	// kick off work queue
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 0);
+
+	return OK;
+}
+
+int
+RGBLED::probe()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) that responds to a bunch of I2C
+	   addresses, including the 0x55 used by this LED device. So
+	   we need to do enough operations to be sure we are talking
+	   to the right device. These 3 operations seem to be enough,
+	   as the 3rd one consistently fails if no RGBLED is on the bus.
+	 */
+
+	unsigned prevretries = _retries;
+	_retries = 4;
+
+	if ((ret = get(on, powersave, r, g, b)) != OK ||
+	    (ret = send_led_enable(false) != OK) ||
+	    (ret = send_led_enable(false) != OK)) {
+		return ret;
+	}
+
+	_retries = prevretries;
+
+	return ret;
+}
+
+int
+RGBLED::status()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		DEVICE_LOG("state: %s", on ? "ON" : "OFF");
+		DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
+	} else {
+		PX4_WARN("failed to read led");
+	}
+
+	return ret;
+}
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+	RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLED::led()
+{
+	if (!_should_run) {
+		if (_param_sub >= 0) {
+			orb_unsubscribe(_param_sub);
+		}
+
+		int led_control_sub = _led_controller.led_control_subscription();
+
+		if (led_control_sub >= 0) {
+			orb_unsubscribe(led_control_sub);
+		}
+
+		_running = false;
+		return;
+	}
+
+	if (_param_sub < 0) {
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
+	}
+
+	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();
+		}
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_r = 255; _g = 0; _b = 0;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			_r = 0; _g = 255; _b = 0;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			_r = 0; _g = 0; _b = 255;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_r = 255; _g = 255; _b = 0;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			_r = 255; _g = 0; _b = 255;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			_r = 0; _g = 255; _b = 255;
+			send_led_enable(true);
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			_r = 255; _g = 255; _b = 255;
+			send_led_enable(true);
+			break;
+
+		default: // led_control_s::COLOR_OFF
+			_r = 0; _g = 0; _b = 0;
+			send_led_enable(false);
+			break;
+		}
+
+		_brightness = (float)led_control_data.leds[0].brightness / 255.f;
+
+		send_led_rgb();
+	}
+
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this,
+		   USEC2TICK(_led_controller.maximum_update_interval()));
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+RGBLED::send_led_enable(bool enable)
+{
+	if (_leds_enabled && enable) {
+		// already enabled
+		return 0;
+	}
+
+	_leds_enabled = enable;
+	uint8_t settings_byte = 0;
+
+	if (enable) {
+		settings_byte |= SETTING_ENABLE;
+	}
+
+	settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
+int
+RGBLED::send_led_rgb()
+{
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
+	};
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2] = {0, 0};
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = ((result[0] >> 4) & SETTING_ENABLE);
+		powersave = !((result[0] >> 4) & SETTING_NOT_POWERSAVE);
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
+	}
+
+	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()
+{
+	PX4_INFO("missing command: try 'start', 'status', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	PX4_INFO("    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'a':
+			rgbledadr = strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(myoptarg, nullptr, 0);
+			break;
+
+		default:
+			rgbled_usage();
+			return 1;
+		}
+	}
+
+	if (myoptind >= argc) {
+		rgbled_usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr) {
+			PX4_WARN("already started");
+			return 1;
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
+			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+			}
+
+			if (g_rgbled == nullptr) {
+				// fall back to default bus
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					PX4_WARN("no RGB led on bus #%d", i2cdevice);
+					return 1;
+				}
+
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
+			if (g_rgbled == nullptr) {
+				PX4_WARN("alloc failed");
+				return 1;
+			}
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				PX4_WARN("no RGB led on bus #%d", i2cdevice);
+				return 1;
+			}
+		}
+
+		return 0;
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		PX4_WARN("not started");
+		rgbled_usage();
+		return 1;
+	}
+
+	if (!strcmp(verb, "status")) {
+		g_rgbled->status();
+		return 0;
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		return 0;
+	}
+
+	rgbled_usage();
+	return 1;
+}

src/drivers/lights/rgbled/rgbled_params.c

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   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 <parameters/param.h>
+
+/**
+ * RGB Led brightness limit
+ *
+ * Set to 0 to disable, 1 for minimum brightness up to 15 (max)
+ *
+ * @min 0
+ * @max 15
+ * @group System
+ */
+PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15);

src/drivers/lights/rgbled_ncp5623c/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2018 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 drivers__rgbled_ncp5623c
+	MAIN rgbled_ncp5623c
+	COMPILE_FLAGS
+	SRCS
+		rgbled_ncp5623c.cpp
+	DEPENDS
+		drivers__device
+		led
+	)

src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp

@@ -0,0 +1,427 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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_ncp5623c.cpp
+ *
+ * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C.
+ *
+ * @author CUAVcaijie <caijie@cuav.net>
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <px4_workqueue.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_led.h>
+#include <lib/led/led.h>
+
+#include "uORB/topics/parameter_update.h"
+
+#define ADDR			0x39	/**< I2C adress of NCP5623C */
+
+#define NCP5623_LED_CURRENT	0x20	/**< Current register */
+#define NCP5623_LED_PWM0	0x40	/**< pwm0 register */
+#define NCP5623_LED_PWM1	0x60	/**< pwm1 register */
+#define NCP5623_LED_PWM2	0x80	/**< pwm2 register */
+
+#define NCP5623_LED_BRIGHT	0x1f	/**< full brightness */
+#define NCP5623_LED_OFF		0x00	/**< off */
+
+
+class RGBLED_NPC5623C : public device::I2C
+{
+public:
+	RGBLED_NPC5623C(int bus, int rgbled);
+	virtual ~RGBLED_NPC5623C();
+
+
+	virtual int		init();
+	virtual int		probe();
+private:
+	work_s			_work;
+
+	float			_brightness;
+	float			_max_brightness;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+	volatile bool		_running;
+	volatile bool		_should_run;
+	bool			_leds_enabled;
+	int			_param_sub;
+
+	LedController		_led_controller;
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			send_led_rgb();
+	void			update_params();
+
+	int			write(uint8_t reg, uint8_t data);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+RGBLED_NPC5623C *g_rgbled = nullptr;
+}
+
+void rgbled_ncp5623c_usage();
+
+extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]);
+
+RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) :
+	I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled
+#ifdef __PX4_NUTTX
+	    , 100000 /* maximum speed supported */
+#endif
+	   ),
+	_work{},
+	_brightness(1.0f),
+	_max_brightness(1.0f),
+	_r(0),
+	_g(0),
+	_b(0),
+	_running(false),
+	_should_run(true),
+	_leds_enabled(true),
+	_param_sub(-1)
+{
+}
+
+RGBLED_NPC5623C::~RGBLED_NPC5623C()
+{
+	_should_run = false;
+	int counter = 0;
+
+	while (_running && ++counter < 10) {
+		usleep(100000);
+	}
+}
+
+int
+RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
+{
+	uint8_t msg[1] = { 0x00 };
+	msg[0] = ((reg & 0xe0) | (data & 0x1f));
+
+	int ret = transfer(&msg[0], 1, nullptr, 0);
+
+	return ret;
+}
+
+int
+RGBLED_NPC5623C::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	update_params();
+
+	_running = true;
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this, 0);
+
+	return OK;
+}
+
+int
+RGBLED_NPC5623C::probe()
+{
+	_retries = 4;
+
+	return write(NCP5623_LED_CURRENT, 0x00);
+}
+
+void
+RGBLED_NPC5623C::led_trampoline(void *arg)
+{
+	RGBLED_NPC5623C *rgbl = reinterpret_cast<RGBLED_NPC5623C *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLED_NPC5623C::led()
+{
+	if (!_should_run) {
+		if (_param_sub >= 0) {
+			orb_unsubscribe(_param_sub);
+		}
+
+		int led_control_sub = _led_controller.led_control_subscription();
+
+		if (led_control_sub >= 0) {
+			orb_unsubscribe(led_control_sub);
+		}
+
+		_running = false;
+		return;
+	}
+
+	if (_param_sub < 0) {
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
+	}
+
+	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();
+		}
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_r = NCP5623_LED_BRIGHT; _g = 0; _b = 0;
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			_r = 0; _g = NCP5623_LED_BRIGHT; _b = 0;
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			_r = 0; _g = 0; _b = NCP5623_LED_BRIGHT;
+			break;
+
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = 0;
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			_r = NCP5623_LED_BRIGHT; _g = 0; _b = NCP5623_LED_BRIGHT;
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			_r = 0; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT;
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			_r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT;
+			break;
+
+		default: // led_control_s::COLOR_OFF
+			_r = 0; _g = 0; _b = 0;
+			break;
+		}
+
+		_brightness = (float)led_control_data.leds[0].brightness / 255.f;
+		send_led_rgb();
+
+	}
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this,
+		   USEC2TICK(_led_controller.maximum_update_interval()));
+}
+
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
+int
+RGBLED_NPC5623C::send_led_rgb()
+{
+
+	uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
+	uint8_t brightness = 0x1f * _max_brightness;
+
+	msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
+	msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
+	msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
+	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
+
+	return transfer(&msg[0], 7, nullptr, 0);
+}
+
+void
+RGBLED_NPC5623C::update_params()
+{
+	int32_t maxbrt = 31;
+	param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt);
+	maxbrt = maxbrt > 31 ? 31 : maxbrt;
+	maxbrt = maxbrt <  0 ?  0 : maxbrt;
+
+	if (maxbrt == 0) {
+		maxbrt = 1;
+	}
+
+	_max_brightness = maxbrt / 31.0f;
+}
+
+void
+rgbled_ncp5623c_usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	PX4_INFO("    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_ncp5623c_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'a':
+			rgbledadr = strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(myoptarg, nullptr, 0);
+			break;
+
+		default:
+			rgbled_ncp5623c_usage();
+			return 1;
+		}
+	}
+
+	if (myoptind >= argc) {
+		rgbled_ncp5623c_usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr) {
+			PX4_WARN("already started");
+			return 1;
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLED_NPC5623C(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
+			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+			}
+
+			if (g_rgbled == nullptr) {
+				// fall back to default bus
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					PX4_WARN("no RGB led on bus #%d", i2cdevice);
+					return 1;
+				}
+
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLED_NPC5623C(i2cdevice, rgbledadr);
+
+			if (g_rgbled == nullptr) {
+				PX4_WARN("alloc failed");
+				return 1;
+			}
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				PX4_WARN("no RGB led on bus #%d", i2cdevice);
+				return 1;
+			}
+		}
+
+		return 0;
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		PX4_WARN("not started");
+		rgbled_ncp5623c_usage();
+		return 1;
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		return 0;
+	}
+
+	rgbled_ncp5623c_usage();
+	return 1;
+}

src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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_ncp5623c_params.c
+ *
+ * Parameters defined by the RBG led driver
+ *
+ * @author CUAVcaijie <caijie@cuav.net>
+ */
+
+
+#include <px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * RGB Led brightness limit
+ *
+ * Set to 0 to disable, 1 for minimum brightness up to 31 (max)
+ *
+ * @min 0
+ * @max 31
+ * @group System
+ */
+PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31);
+

src/drivers/lights/rgbled_pwm/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__rgbled_pwm
+	MAIN rgbled_pwm
+	STACK_MAIN
+		1500
+	SRCS
+		rgbled_pwm.cpp
+	DEPENDS
+		led
+	)

src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp

@@ -0,0 +1,362 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015, 2016 Airmind 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 Airmind 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_pwm.cpp
+ *
+ * Driver for the onboard RGB LED controller by PWM.
+ * this driver is based the PX4 led driver
+ *
+ */
+
+#include <nuttx/config.h>
+
+//#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+#include <drivers/drv_hrt.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_led.h>
+#include <lib/led/led.h>
+#include <drivers/device/device.h>
+#include <systemlib/err.h>
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+class RGBLED_PWM : public device::CDev
+{
+public:
+	RGBLED_PWM();
+	virtual ~RGBLED_PWM();
+
+
+	virtual int		init();
+	virtual int		probe();
+	int		status();
+
+private:
+	work_s			_work;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+
+	volatile bool		_running;
+	volatile bool		_should_run;
+
+	LedController		_led_controller;
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+extern "C" __EXPORT int rgbled_pwm_main(int argc, char *argv[]);
+extern int led_pwm_servo_set(unsigned channel, uint8_t  value);
+extern unsigned led_pwm_servo_get(unsigned channel);
+extern int led_pwm_servo_init(void);
+extern void led_pwm_servo_deinit(void);
+
+
+/* for now, we only support one RGBLED */
+namespace
+{
+RGBLED_PWM *g_rgbled = nullptr;
+}
+
+RGBLED_PWM::RGBLED_PWM() :
+	CDev("rgbled_pwm", RGBLED_PWM0_DEVICE_PATH),
+	_work{},
+	_r(0),
+	_g(0),
+	_b(0),
+	_running(false),
+	_should_run(true)
+{
+}
+
+RGBLED_PWM::~RGBLED_PWM()
+{
+	_should_run = false;
+	int counter = 0;
+
+	while (_running && ++counter < 10) {
+		usleep(100000);
+	}
+}
+
+int
+RGBLED_PWM::init()
+{
+	/* switch off LED on start */
+	CDev::init();
+	led_pwm_servo_init();
+	send_led_rgb();
+
+	_running = true;
+	// kick off work queue
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 0);
+
+	return OK;
+}
+
+int
+RGBLED_PWM::status()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		DEVICE_LOG("state: %s", on ? "ON" : "OFF");
+		DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
+	} else {
+		PX4_WARN("failed to read led");
+	}
+
+	return ret;
+}
+int
+RGBLED_PWM::probe()
+{
+	return (OK);
+}
+
+void
+RGBLED_PWM::led_trampoline(void *arg)
+{
+	RGBLED_PWM *rgbl = reinterpret_cast<RGBLED_PWM *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLED_PWM::led()
+{
+	if (!_should_run) {
+		int led_control_sub = _led_controller.led_control_subscription();
+
+		if (led_control_sub >= 0) {
+			orb_unsubscribe(led_control_sub);
+		}
+
+		_running = false;
+		return;
+	}
+
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+		uint8_t brightness = led_control_data.leds[0].brightness;
+
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_r = brightness; _g = 0; _b = 0;
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			_r = 0; _g = brightness; _b = 0;
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			_r = 0; _g = 0; _b = brightness;
+			break;
+
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_r = brightness; _g = brightness; _b = 0;
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			_r = brightness; _g = 0; _b = brightness;
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			_r = 0; _g = brightness; _b = brightness;
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			_r = brightness; _g = brightness; _b = brightness;
+			break;
+
+		default: // led_control_s::COLOR_OFF
+			_r = 0; _g = 0; _b = 0;
+			break;
+		}
+
+		send_led_rgb();
+	}
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this,
+		   USEC2TICK(_led_controller.maximum_update_interval()));
+}
+
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
+int
+RGBLED_PWM::send_led_rgb()
+{
+#if defined(BOARD_HAS_LED_PWM)
+	led_pwm_servo_set(0, _r);
+	led_pwm_servo_set(1, _g);
+	led_pwm_servo_set(2, _b);
+#endif
+
+#if defined(BOARD_HAS_UI_LED_PWM)
+	led_pwm_servo_set(3, _r);
+	led_pwm_servo_set(4, _g);
+	led_pwm_servo_set(5, _b);
+#endif
+
+	return (OK);
+}
+
+int
+RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	powersave = OK;
+	on = _r > 0 || _g > 0 || _b > 0;
+	r = _r;
+	g = _g;
+	b = _b;
+	return OK;
+}
+
+static void
+rgbled_usage()
+{
+	PX4_INFO("missing command: try 'start', 'status', 'stop'");
+}
+
+int
+rgbled_pwm_main(int argc, char *argv[])
+{
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			break;
+
+		case 'b':
+			break;
+
+		default:
+			rgbled_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		rgbled_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr) {
+			errx(1, "already started");
+		}
+
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLED_PWM();
+
+			if (g_rgbled == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		PX4_WARN("not started");
+		rgbled_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "status")) {
+		g_rgbled->status();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		exit(0);
+	}
+
+	rgbled_usage();
+	exit(0);
+}