From 2a8ef50df4747f0242459bdc22f11a46410f43bb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Dec 2012 19:29:36 -0800 Subject: [PATCH 1/2] A driver and shell command for the BlinkM I2C LED controller. --- apps/drivers/blinkm/Makefile | 42 +++ apps/drivers/blinkm/blinkm.cpp | 461 +++++++++++++++++++++++++++++ apps/drivers/device/i2c.cpp | 4 +- apps/drivers/device/i2c.h | 2 +- apps/drivers/drv_blinkm.h | 69 +++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 6 files changed, 576 insertions(+), 3 deletions(-) create mode 100644 apps/drivers/blinkm/Makefile create mode 100644 apps/drivers/blinkm/blinkm.cpp create mode 100644 apps/drivers/drv_blinkm.h diff --git a/apps/drivers/blinkm/Makefile b/apps/drivers/blinkm/Makefile new file mode 100644 index 0000000000..5a623693df --- /dev/null +++ b/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 diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp new file mode 100644 index 0000000000..96c6ae33f8 --- /dev/null +++ b/apps/drivers/blinkm/blinkm.cpp @@ -0,0 +1,461 @@ +/**************************************************************************** + * + * 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 + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +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; + } + + /* 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; + 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, " \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"); +} \ No newline at end of file diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 56112d7677..474190d839 100644 --- a/apps/drivers/device/i2c.cpp +++ b/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(send); msgv[msgs].length = send_len; msgs++; } diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 3112e8e376..4d630b8a81 100644 --- a/apps/drivers/device/i2c.h +++ b/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); /** diff --git a/apps/drivers/drv_blinkm.h b/apps/drivers/drv_blinkm.h new file mode 100644 index 0000000000..9c278f6c50 --- /dev/null +++ b/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 +#include + +#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 , , arg[0-2] + * + * The script is terminated by a zero command. + */ +#define BLINKM_SET_USER_SCRIPT _BLINKMIOC(3) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5833e35755..58eda35a09 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/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 From 269bd9f4038189716bd630031c90e98b421e5a79 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Dec 2012 19:36:02 -0800 Subject: [PATCH 2/2] Force the fade speed to something sensible. Deal better with failed probes. --- apps/drivers/blinkm/blinkm.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 96c6ae33f8..d589025cce 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -178,6 +178,9 @@ BlinkM::init() return ret; } + /* set some sensible defaults */ + set_fade_speed(25); + /* turn off by default */ play_script(BLACK); @@ -433,6 +436,7 @@ blinkm_main(int argc, char *argv[]) if (OK != g_blinkm->init()) { delete g_blinkm; + g_blinkm = nullptr; errx(1, "init failed"); }