aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/rgbled/rgbled.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/rgbled/rgbled.cpp')
-rw-r--r--src/drivers/rgbled/rgbled.cpp481
1 files changed, 293 insertions, 188 deletions
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index feb8f1c6c..727c86e02 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -1,6 +1,8 @@
/****************************************************************************
*
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Julian Oes <joes@student.ethz.ch>
+ * Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,7 +38,6 @@
*
* Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
*
- *
*/
#include <nuttx/config.h>
@@ -92,53 +93,51 @@ public:
private:
work_s _work;
- rgbled_color_t _color;
rgbled_mode_t _mode;
rgbled_pattern_t _pattern;
- float _brightness;
uint8_t _r;
uint8_t _g;
uint8_t _b;
+ float _brightness;
- bool _should_run;
bool _running;
int _led_interval;
+ bool _should_run;
int _counter;
void set_color(rgbled_color_t ledcolor);
void set_mode(rgbled_mode_t mode);
void set_pattern(rgbled_pattern_t *pattern);
- void set_brightness(float brightness);
static void led_trampoline(void *arg);
void led();
- int set(bool on, uint8_t r, uint8_t g, uint8_t b);
- int set_on(bool on);
- int set_rgb(uint8_t r, uint8_t g, uint8_t b);
- int get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+ 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);
};
/* for now, we only support one RGBLED */
namespace
{
- RGBLED *g_rgbled;
+RGBLED *g_rgbled;
}
+void rgbled_usage();
extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
RGBLED::RGBLED(int bus, int rgbled) :
I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
- _color(RGBLED_COLOR_OFF),
_mode(RGBLED_MODE_OFF),
- _running(false),
- _brightness(1.0f),
_r(0),
_g(0),
_b(0),
+ _brightness(1.0f),
+ _running(false),
_led_interval(0),
+ _should_run(false),
_counter(0)
{
memset(&_work, 0, sizeof(_work));
@@ -159,8 +158,9 @@ RGBLED::init()
return ret;
}
- /* start off */
- set(false, 0, 0, 0);
+ /* switch off LED on start */
+ send_led_enable(false);
+ send_led_rgb();
return OK;
}
@@ -169,10 +169,23 @@ int
RGBLED::probe()
{
int ret;
- bool on, not_powersave;
+ bool on, powersave;
uint8_t r, g, b;
- ret = get(on, not_powersave, r, g, b);
+ /**
+ this may look strange, but is needed. There is a serial
+ EEPROM (Microchip-24aa01) on the PX4FMU-v1 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.
+ */
+ if ((ret=get(on, powersave, r, g, b)) != OK ||
+ (ret=send_led_enable(false) != OK) ||
+ (ret=send_led_enable(false) != OK)) {
+ return ret;
+ }
return ret;
}
@@ -181,15 +194,16 @@ int
RGBLED::info()
{
int ret;
- bool on, not_powersave;
+ bool on, powersave;
uint8_t r, g, b;
- ret = get(on, not_powersave, r, g, b);
+ ret = get(on, powersave, r, g, b);
if (ret == OK) {
/* we don't care about power-save mode */
log("state: %s", on ? "ON" : "OFF");
log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
} else {
warnx("failed to read led");
}
@@ -201,28 +215,30 @@ int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
+
switch (cmd) {
case RGBLED_SET_RGB:
- /* set the specified RGB values */
- rgbled_rgbset_t rgbset;
- memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
- set_rgb(rgbset.red, rgbset.green, rgbset.blue);
- set_mode(RGBLED_MODE_ON);
+ /* set the specified color */
+ _r = ((rgbled_rgbset_t *) arg)->red;
+ _g = ((rgbled_rgbset_t *) arg)->green;
+ _b = ((rgbled_rgbset_t *) arg)->blue;
+ send_led_rgb();
return OK;
case RGBLED_SET_COLOR:
/* set the specified color name */
set_color((rgbled_color_t)arg);
+ send_led_rgb();
return OK;
case RGBLED_SET_MODE:
- /* set the specified blink speed */
+ /* set the specified mode */
set_mode((rgbled_mode_t)arg);
return OK;
case RGBLED_SET_PATTERN:
/* set a special pattern */
- set_pattern((rgbled_pattern_t*)arg);
+ set_pattern((rgbled_pattern_t *)arg);
return OK;
default:
@@ -241,39 +257,59 @@ RGBLED::led_trampoline(void *arg)
rgbl->led();
}
-
-
+/**
+ * Main loop function
+ */
void
RGBLED::led()
{
+ if (!_should_run) {
+ _running = false;
+ return;
+ }
+
switch (_mode) {
- case RGBLED_MODE_BLINK_SLOW:
- case RGBLED_MODE_BLINK_NORMAL:
- case RGBLED_MODE_BLINK_FAST:
- if(_counter % 2 == 0)
- set_on(true);
- else
- set_on(false);
- break;
- case RGBLED_MODE_BREATHE:
- if (_counter >= 30)
- _counter = 0;
- if (_counter <= 15) {
- set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
- } else {
- set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
- }
- break;
- case RGBLED_MODE_PATTERN:
- /* don't run out of the pattern array and stop if the next frame is 0 */
- if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
- _counter = 0;
+ case RGBLED_MODE_BLINK_SLOW:
+ case RGBLED_MODE_BLINK_NORMAL:
+ case RGBLED_MODE_BLINK_FAST:
+ if (_counter >= 2)
+ _counter = 0;
- set_color(_pattern.color[_counter]);
- _led_interval = _pattern.duration[_counter];
- break;
- default:
- break;
+ send_led_enable(_counter == 0);
+
+ break;
+
+ case RGBLED_MODE_BREATHE:
+
+ if (_counter >= 62)
+ _counter = 0;
+
+ int n;
+
+ if (_counter < 32) {
+ n = _counter;
+
+ } else {
+ n = 62 - _counter;
+ }
+
+ _brightness = n * n / (31.0f * 31.0f);
+ send_led_rgb();
+ break;
+
+ case RGBLED_MODE_PATTERN:
+
+ /* don't run out of the pattern array and stop if the next frame is 0 */
+ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+ _counter = 0;
+
+ set_color(_pattern.color[_counter]);
+ send_led_rgb();
+ _led_interval = _pattern.duration[_counter];
+ break;
+
+ default:
+ break;
}
_counter++;
@@ -282,181 +318,226 @@ RGBLED::led()
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
}
+/**
+ * Parse color constant and set _r _g _b values
+ */
void
-RGBLED::set_color(rgbled_color_t color) {
+RGBLED::set_color(rgbled_color_t color)
+{
+ switch (color) {
+ case RGBLED_COLOR_OFF:
+ _r = 0;
+ _g = 0;
+ _b = 0;
+ break;
- _color = color;
+ case RGBLED_COLOR_RED:
+ _r = 255;
+ _g = 0;
+ _b = 0;
+ break;
- switch (color) {
- case RGBLED_COLOR_OFF: // off
- set_rgb(0,0,0);
- break;
- case RGBLED_COLOR_RED: // red
- set_rgb(255,0,0);
- break;
- case RGBLED_COLOR_YELLOW: // yellow
- set_rgb(255,70,0);
- break;
- case RGBLED_COLOR_PURPLE: // purple
- set_rgb(255,0,255);
- break;
- case RGBLED_COLOR_GREEN: // green
- set_rgb(0,255,0);
- break;
- case RGBLED_COLOR_BLUE: // blue
- set_rgb(0,0,255);
- break;
- case RGBLED_COLOR_WHITE: // white
- set_rgb(255,255,255);
- break;
- case RGBLED_COLOR_AMBER: // amber
- set_rgb(255,20,0);
- break;
- case RGBLED_COLOR_DIM_RED: // red
- set_rgb(90,0,0);
- break;
- case RGBLED_COLOR_DIM_YELLOW: // yellow
- set_rgb(80,30,0);
- break;
- case RGBLED_COLOR_DIM_PURPLE: // purple
- set_rgb(45,0,45);
- break;
- case RGBLED_COLOR_DIM_GREEN: // green
- set_rgb(0,90,0);
- break;
- case RGBLED_COLOR_DIM_BLUE: // blue
- set_rgb(0,0,90);
- break;
- case RGBLED_COLOR_DIM_WHITE: // white
- set_rgb(30,30,30);
- break;
- case RGBLED_COLOR_DIM_AMBER: // amber
- set_rgb(80,20,0);
- break;
- default:
- warnx("color unknown");
- break;
+ case RGBLED_COLOR_YELLOW:
+ _r = 255;
+ _g = 200;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_PURPLE:
+ _r = 255;
+ _g = 0;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_GREEN:
+ _r = 0;
+ _g = 255;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_BLUE:
+ _r = 0;
+ _g = 0;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_WHITE:
+ _r = 255;
+ _g = 255;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_AMBER:
+ _r = 255;
+ _g = 80;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_RED:
+ _r = 90;
+ _g = 0;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_YELLOW:
+ _r = 80;
+ _g = 30;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_PURPLE:
+ _r = 45;
+ _g = 0;
+ _b = 45;
+ break;
+
+ case RGBLED_COLOR_DIM_GREEN:
+ _r = 0;
+ _g = 90;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_BLUE:
+ _r = 0;
+ _g = 0;
+ _b = 90;
+ break;
+
+ case RGBLED_COLOR_DIM_WHITE:
+ _r = 30;
+ _g = 30;
+ _b = 30;
+ break;
+
+ case RGBLED_COLOR_DIM_AMBER:
+ _r = 80;
+ _g = 20;
+ _b = 0;
+ break;
+
+ default:
+ warnx("color unknown");
+ break;
}
}
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
void
RGBLED::set_mode(rgbled_mode_t mode)
{
- _mode = mode;
+ if (mode != _mode) {
+ _mode = mode;
- switch (mode) {
+ switch (mode) {
case RGBLED_MODE_OFF:
_should_run = false;
- set_on(false);
+ send_led_enable(false);
break;
+
case RGBLED_MODE_ON:
- _should_run = false;
- set_on(true);
+ _brightness = 1.0f;
+ send_led_rgb();
+ send_led_enable(true);
break;
+
case RGBLED_MODE_BLINK_SLOW:
_should_run = true;
+ _counter = 0;
_led_interval = 2000;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BLINK_NORMAL:
_should_run = true;
+ _counter = 0;
_led_interval = 500;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BLINK_FAST:
_should_run = true;
+ _counter = 0;
_led_interval = 100;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BREATHE:
_should_run = true;
- set_on(true);
_counter = 0;
- _led_interval = 1000/15;
+ _led_interval = 25;
+ send_led_enable(true);
break;
+
case RGBLED_MODE_PATTERN:
_should_run = true;
- set_on(true);
_counter = 0;
+ _brightness = 1.0f;
+ send_led_enable(true);
break;
+
default:
warnx("mode unknown");
break;
- }
+ }
+
+ /* if it should run now, start the workq */
+ if (_should_run && !_running) {
+ _running = true;
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+ }
- /* if it should run now, start the workq */
- if (_should_run && !_running) {
- _running = true;
- work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
- }
- /* if it should stop, then cancel the workq */
- if (!_should_run && _running) {
- _running = false;
- work_cancel(LPWORK, &_work);
}
}
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
void
RGBLED::set_pattern(rgbled_pattern_t *pattern)
{
memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-
- set_mode(RGBLED_MODE_PATTERN);
-}
-
-void
-RGBLED::set_brightness(float brightness) {
-
- _brightness = brightness;
- set_rgb(_r, _g, _b);
-}
-
-int
-RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
-{
- uint8_t settings_byte = 0;
-
- if (on)
- settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-// if (not_powersave)
- settings_byte |= SETTING_NOT_POWERSAVE;
-
- const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
-
- return transfer(msg, sizeof(msg), nullptr, 0);
}
+/**
+ * Sent ENABLE flag to LED driver
+ */
int
-RGBLED::set_on(bool on)
+RGBLED::send_led_enable(bool enable)
{
uint8_t settings_byte = 0;
- if (on)
+ if (enable)
settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-// if (not_powersave)
- settings_byte |= SETTING_NOT_POWERSAVE;
+ 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::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+RGBLED::send_led_rgb()
{
- /* save the RGB values in case we want to change the brightness later */
- _r = r;
- _g = g;
- _b = b;
-
- const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
-
+ /* 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)
+ };
return transfer(msg, sizeof(msg), nullptr, 0);
}
-
int
-RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
{
uint8_t result[2];
int ret;
@@ -465,24 +546,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
if (ret == OK) {
on = result[0] & SETTING_ENABLE;
- not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+ powersave = !(result[0] & SETTING_NOT_POWERSAVE);
/* XXX check, looks wrong */
- r = (result[0] & 0x0f)*255/15;
- g = (result[1] & 0xf0)*255/15;
- b = (result[1] & 0x0f)*255/15;
+ r = (result[0] & 0x0f) << 4;
+ g = (result[1] & 0xf0);
+ b = (result[1] & 0x0f) << 4;
}
return ret;
}
-void rgbled_usage();
-
-
-void rgbled_usage() {
- warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+void
+rgbled_usage()
+{
+ warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
- errx(0, " -a addr (0x%x)", ADDR);
+ warnx(" -a addr (0x%x)", ADDR);
}
int
@@ -492,21 +572,30 @@ rgbled_main(int argc, char *argv[])
int rgbledadr = ADDR; /* 7bit */
int ch;
+
/* jump over start/off/etc and look at options first */
- while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+ while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
break;
+
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
+
default:
rgbled_usage();
+ exit(0);
}
}
- const char *verb = argv[1];
+ if (optind >= argc) {
+ rgbled_usage();
+ exit(1);
+ }
+
+ const char *verb = argv[optind];
int fd;
int ret;
@@ -519,17 +608,24 @@ rgbled_main(int argc, char *argv[])
// 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) {
+ errx(1, "init failed");
+ }
i2cdevice = PX4_I2C_BUS_LED;
}
}
+
if (g_rgbled == nullptr) {
g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
if (g_rgbled == nullptr)
errx(1, "new failed");
@@ -545,21 +641,24 @@ rgbled_main(int argc, char *argv[])
/* need the driver past this point */
if (g_rgbled == nullptr) {
- warnx("not started");
- rgbled_usage();
- exit(0);
+ warnx("not started");
+ rgbled_usage();
+ exit(0);
}
if (!strcmp(verb, "test")) {
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
- rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
- {200, 200, 200, 400 } };
+ rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+ {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern
+ };
ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+ ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
close(fd);
exit(ret);
@@ -570,33 +669,39 @@ rgbled_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
- /* although technically it doesn't stop, this is the excepted syntax */
+ if (!strcmp(verb, "off")) {
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
+
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
exit(ret);
}
if (!strcmp(verb, "rgb")) {
+ if (argc < 5) {
+ errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+ }
+
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
- if (argc < 5) {
- errx(1, "Usage: rgbled rgb <red> <green> <blue>");
- }
+
rgbled_rgbset_t v;
- v.red = strtol(argv[1], NULL, 0);
- v.green = strtol(argv[2], NULL, 0);
- v.blue = strtol(argv[3], NULL, 0);
+ v.red = strtol(argv[2], NULL, 0);
+ v.green = strtol(argv[3], NULL, 0);
+ v.blue = strtol(argv[4], NULL, 0);
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+ ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
close(fd);
exit(ret);
}
rgbled_usage();
+ exit(0);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 00:32:44 +0000