Merge pull request #90 from PX4/#89-BlinkM-driver

BlinkM driver

apps/drivers/blinkm/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# BlinkM I2C LED driver
+#
+
+APPNAME		 = blinkm
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/drivers/blinkm/blinkm.cpp

@@ -0,0 +1,465 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 blinkm.cpp
+ *
+ * Driver for the BlinkM LED controller connected via I2C.
+ */
+
+
+#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 <drivers/drv_blinkm.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+class BlinkM : public device::I2C
+{
+public:
+	BlinkM(int bus);
+	~BlinkM();
+
+	virtual int		init();
+	virtual int		probe();
+
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	static const char	*script_names[];
+
+private:
+	enum ScriptID {
+		USER		= 0,
+		RGB,
+		WHITE_FLASH,
+		RED_FLASH,
+		GREEN_FLASH,
+		BLUE_FLASH,
+		CYAN_FLASH,
+		MAGENTA_FLASH,
+		YELLOW_FLASH,
+		BLACK,
+		HUE_CYCLE,
+		MOOD_LIGHT,
+		VIRTUAL_CANDLE,
+		WATER_REFLECTIONS,
+		OLD_NEON,
+		THE_SEASONS,
+		THUNDERSTORM,
+		STOP_LIGHT,
+		MORSE_CODE
+	};
+
+	work_s			_work;
+	static const unsigned	_monitor_interval = 250;
+
+	static void		monitor_trampoline(void *arg);
+	void			monitor();
+
+	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
+
+	int			fade_rgb(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb(uint8_t h, uint8_t s, uint8_t b);
+
+	int			fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
+
+	int			set_fade_speed(uint8_t s);
+
+	int			play_script(uint8_t script_id);
+	int			play_script(const char *script_name);
+	int			stop_script();
+
+	int			write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
+	int			read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
+	int			set_script(uint8_t length, uint8_t repeats);
+
+	int			get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
+
+	int			get_firmware_version(uint8_t version[2]);
+};
+
+/* for now, we only support one BlinkM */
+namespace
+{
+BlinkM *g_blinkm;
+
+}
+
+/* list of script names, must match script ID numbers */
+const char *BlinkM::script_names[] = {
+	"USER",
+	"RGB",
+	"WHITE_FLASH",
+	"RED_FLASH",
+	"GREEN_FLASH",
+	"BLUE_FLASH",
+	"CYAN_FLASH",
+	"MAGENTA_FLASH",
+	"YELLOW_FLASH",
+	"BLACK",
+	"HUE_CYCLE",
+	"MOOD_LIGHT",
+	"VIRTUAL_CANDLE",
+	"WATER_REFLECTIONS",
+	"OLD_NEON",
+	"THE_SEASONS",
+	"THUNDERSTORM",
+	"STOP_LIGHT",
+	"MORSE_CODE",
+	nullptr
+};
+
+extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
+
+BlinkM::BlinkM(int bus) :
+	I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
+{
+}
+
+BlinkM::~BlinkM()
+{
+}
+
+int
+BlinkM::init()
+{
+	int ret;
+
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	/* set some sensible defaults */
+	set_fade_speed(25);
+
+	/* turn off by default */
+	play_script(BLACK);
+
+	/* start the system monitor as a low-priority workqueue entry */
+	work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
+
+	return OK;
+}
+
+int
+BlinkM::probe()
+{
+	uint8_t version[2];
+	int ret;
+
+	ret = get_firmware_version(version);
+
+	if (ret == OK)
+		log("found BlinkM firmware version %c%c", version[1], version[0]);
+
+	return ret;
+}
+
+int
+BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case BLINKM_PLAY_SCRIPT_NAMED:
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+		ret = play_script((const char *)arg);
+		break;
+
+	case BLINKM_PLAY_SCRIPT:
+		ret = play_script(arg);
+		break;
+
+	case BLINKM_SET_USER_SCRIPT: {
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+
+		unsigned lines = 0;
+		const uint8_t *script = (const uint8_t *)arg;
+
+		while ((lines < 50) && (script[1] != 0)) {
+			ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
+			if (ret != OK)
+				break;
+			script += 5;
+		}
+		if (ret == OK)
+			ret = set_script(lines, 0);
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+void
+BlinkM::monitor_trampoline(void *arg)
+{
+	BlinkM *bm = (BlinkM *)arg;
+
+	bm->monitor();
+}
+
+void
+BlinkM::monitor()
+{
+	/* check system state, possibly update LED to suit */
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval);
+}
+
+int
+BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'n', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'c', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'h', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'C', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'H', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::set_fade_speed(uint8_t s)
+{
+	const uint8_t msg[2] = { 'f', s };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(uint8_t script_id)
+{
+	const uint8_t msg[4] = { 'p', script_id, 0, 0 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(const char *script_name)
+{
+	/* handle HTML colour encoding */
+	if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
+		char code[3];
+		uint8_t r, g, b;
+
+		code[2] = '\0';
+
+		code[0] = script_name[1];
+		code[1] = script_name[2];
+		r = strtol(code, 0, 16);
+		code[0] = script_name[3];
+		code[1] = script_name[4];
+		g = strtol(code, 0, 16);
+		code[0] = script_name[5];
+		code[1] = script_name[6];
+		b = strtol(code, 0, 16);
+
+		stop_script();
+		return set_rgb(r, g, b);
+	}
+
+	for (unsigned i = 0; script_names[i] != nullptr; i++)
+		if (!strcasecmp(script_name, script_names[i]))
+			return play_script(i);
+
+	return -1;
+}
+
+int
+BlinkM::stop_script()
+{
+	const uint8_t msg[1] = { 'o' };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
+{
+	const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
+{
+	const uint8_t msg[3] = { 'R', 0, line };
+	uint8_t result[5];
+
+	int ret = transfer(msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		ticks = result[0];
+		cmd[0] = result[1];
+		cmd[1] = result[2];
+		cmd[2] = result[3];
+		cmd[3] = result[4];
+	}
+
+	return ret;
+}
+
+int
+BlinkM::set_script(uint8_t len, uint8_t repeats)
+{
+	const uint8_t msg[4] = { 'L', 0, len, repeats };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	const uint8_t msg = 'g';
+	uint8_t result[3];
+
+	int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		r = result[0];
+		g = result[1];
+		b = result[2];
+	}
+
+	return ret;
+}
+
+
+int
+BlinkM::get_firmware_version(uint8_t version[2])
+{
+	const uint8_t msg = 'Z';
+
+	return transfer(&msg, sizeof(msg), version, sizeof(version));
+}
+
+int
+blinkm_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "start")) {
+		if (g_blinkm != nullptr)
+			errx(1, "already started");
+
+		g_blinkm = new BlinkM(3);
+
+		if (g_blinkm == nullptr)
+			errx(1, "new failed");
+
+		if (OK != g_blinkm->init()) {
+			delete g_blinkm;
+			g_blinkm = nullptr;
+			errx(1, "init failed");
+		}
+
+		exit(0);
+	}
+
+	if (g_blinkm == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "list")) {
+		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
+			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
+		fprintf(stderr, "    <html color number>\n");
+		exit(0);
+	}
+
+	/* things that require access to the device */
+	int fd = open(BLINKM_DEVICE_PATH, 0);
+	if (fd < 0)
+		err(1, "can't open BlinkM device");
+
+	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
+		exit(0);
+
+	errx(1, "missing command, try 'start', 'list' or a script name");
+}

apps/drivers/device/i2c.cpp

@@ -115,7 +115,7 @@ I2C::probe()
 }
 
 int
-I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
 {
 	struct i2c_msg_s msgv[2];
 	unsigned msgs;
@@ -130,7 +130,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 		if (send_len > 0) {
 			msgv[msgs].addr = _address;
 			msgv[msgs].flags = 0;
-			msgv[msgs].buffer = send;
+			msgv[msgs].buffer = const_cast<uint8_t *>(send);
 			msgv[msgs].length = send_len;
 			msgs++;
 		}

apps/drivers/device/i2c.h

@@ -97,7 +97,7 @@ protected:
 	 * @return		OK if the transfer was successful, -errno
 	 *			otherwise.
 	 */
-	int		transfer(uint8_t *send, unsigned send_len,
+	int		transfer(const uint8_t *send, unsigned send_len,
 				 uint8_t *recv, unsigned recv_len);
 
 	/**

apps/drivers/drv_blinkm.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_blinkm.h
+ *
+ * BlinkM driver API
+ *
+ * This could probably become a more generalised API for multi-colour LED
+ * driver systems, or be merged with the generic LED driver.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define BLINKM_DEVICE_PATH	"/dev/blinkm"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _BLINKMIOCBASE		(0x2900)
+#define _BLINKMIOC(_n)		(_IOC(_BLINKMIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define BLINKM_PLAY_SCRIPT_NAMED	_BLINKMIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define BLINKM_PLAY_SCRIPT		_BLINKMIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define BLINKM_SET_USER_SCRIPT		_BLINKMIOC(3)

nuttx/configs/px4fmu/nsh/appconfig

@@ -98,6 +98,7 @@ CONFIGURED_APPS += drivers/l3gd20
 CONFIGURED_APPS	+= drivers/px4io
 CONFIGURED_APPS += drivers/stm32
 CONFIGURED_APPS += drivers/led
+CONFIGURED_APPS += drivers/blinkm
 CONFIGURED_APPS += drivers/stm32/tone_alarm
 CONFIGURED_APPS += drivers/px4fmu
 CONFIGURED_APPS += drivers/hil