mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 01:03:33 +08:00
rgbled_pwm: switch to new led uorb interface
This commit is contained in:
Beat Küng
@@ -38,5 +38,6 @@ px4_add_module(
|
||||
rgbled_pwm.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
lib__led
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
||||
@@ -62,7 +62,8 @@
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include <drivers/drv_rgbled.h>
|
||||
#include <drivers/drv_led.h>
|
||||
#include <lib/led/led.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
@@ -78,44 +79,24 @@ public:
|
||||
|
||||
virtual int init();
|
||||
virtual int probe();
|
||||
virtual int info();
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
int status();
|
||||
|
||||
private:
|
||||
struct hrt_call _call;
|
||||
struct hrt_call _call_r;
|
||||
struct hrt_call _call_g;
|
||||
struct hrt_call _call_b;
|
||||
|
||||
work_s _work;
|
||||
|
||||
rgbled_mode_t _mode;
|
||||
rgbled_pattern_t _pattern;
|
||||
|
||||
uint8_t _r;
|
||||
uint8_t _g;
|
||||
uint8_t _b;
|
||||
bool _enable;
|
||||
float _brightness;
|
||||
|
||||
bool _running;
|
||||
int _led_interval;
|
||||
bool _should_run;
|
||||
int _counter;
|
||||
volatile bool _running;
|
||||
volatile bool _should_run;
|
||||
|
||||
static void pwm_begin(void *arg);
|
||||
static void pwm_end_r(void *arg);
|
||||
static void pwm_end_g(void *arg);
|
||||
static void pwm_end_b(void *arg);
|
||||
|
||||
void set_color(rgbled_color_t ledcolor);
|
||||
void set_mode(rgbled_mode_t mode);
|
||||
void set_pattern(rgbled_pattern_t *pattern);
|
||||
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);
|
||||
};
|
||||
@@ -137,32 +118,24 @@ void rgbled_usage();
|
||||
|
||||
RGBLED_PWM::RGBLED_PWM() :
|
||||
CDev("rgbled", RGBLED0_DEVICE_PATH),
|
||||
_call{},
|
||||
_call_r{},
|
||||
_call_g{},
|
||||
_call_b{},
|
||||
_mode(RGBLED_MODE_OFF),
|
||||
_work{},
|
||||
_r(0),
|
||||
_g(0),
|
||||
_b(0),
|
||||
_enable(false),
|
||||
_brightness(1.0f),
|
||||
_running(false),
|
||||
_led_interval(0),
|
||||
_should_run(false),
|
||||
_counter(0)
|
||||
_should_run(true)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
memset(&_pattern, 0, sizeof(_pattern));
|
||||
|
||||
memset(&_call, 0, sizeof(_call));
|
||||
memset(&_call_r, 0, sizeof(_call_r));
|
||||
memset(&_call_g, 0, sizeof(_call_g));
|
||||
memset(&_call_b, 0, sizeof(_call_b));
|
||||
}
|
||||
|
||||
RGBLED_PWM::~RGBLED_PWM()
|
||||
{
|
||||
_should_run = false;
|
||||
int counter = 0;
|
||||
|
||||
while (_running && ++counter < 10) {
|
||||
usleep(100000);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
@@ -171,32 +144,17 @@ RGBLED_PWM::init()
|
||||
/* switch off LED on start */
|
||||
CDev::init();
|
||||
led_pwm_servo_init();
|
||||
send_led_enable(false);
|
||||
send_led_rgb();
|
||||
|
||||
_running = true;
|
||||
// kick off work queue
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 0);
|
||||
|
||||
return OK;
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_begin(void *arg)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RGBLED_PWM::pwm_end_r(void *arg)
|
||||
{
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_end_g(void *arg)
|
||||
{
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_end_b(void *arg)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
RGBLED_PWM::info()
|
||||
RGBLED_PWM::status()
|
||||
{
|
||||
int ret;
|
||||
bool on, powersave;
|
||||
@@ -220,45 +178,6 @@ RGBLED_PWM::probe()
|
||||
{
|
||||
return (OK);
|
||||
}
|
||||
int
|
||||
RGBLED_PWM::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = ENOTTY;
|
||||
|
||||
switch (cmd) {
|
||||
case RGBLED_SET_RGB:
|
||||
/* 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 mode */
|
||||
set_mode((rgbled_mode_t)arg);
|
||||
return OK;
|
||||
|
||||
case RGBLED_SET_PATTERN:
|
||||
/* set a special pattern */
|
||||
set_pattern((rgbled_pattern_t *)arg);
|
||||
return OK;
|
||||
|
||||
default:
|
||||
/* see if the parent class can make any use of it */
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RGBLED_PWM::led_trampoline(void *arg)
|
||||
@@ -275,257 +194,65 @@ 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;
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case RGBLED_MODE_BLINK_SLOW:
|
||||
case RGBLED_MODE_BLINK_NORMAL:
|
||||
case RGBLED_MODE_BLINK_FAST:
|
||||
if (_counter >= 2) {
|
||||
_counter = 0;
|
||||
}
|
||||
|
||||
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;
|
||||
if (!_led_controller.is_init()) {
|
||||
int led_control_sub = orb_subscribe(ORB_ID(led_control));
|
||||
_led_controller.init(led_control_sub);
|
||||
}
|
||||
|
||||
_counter++;
|
||||
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;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_GREEN:
|
||||
_r = 0; _g = 255; _b = 0;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_BLUE:
|
||||
_r = 0; _g = 0; _b = 255;
|
||||
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;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_PURPLE:
|
||||
_r = 255; _g = 0; _b = 255;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_CYAN:
|
||||
_r = 0; _g = 255; _b = 255;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_WHITE:
|
||||
_r = 255; _g = 255; _b = 255;
|
||||
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, _led_interval);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse color constant and set _r _g _b values
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_color(rgbled_color_t color)
|
||||
{
|
||||
switch (color) {
|
||||
case RGBLED_COLOR_OFF:
|
||||
_r = 0;
|
||||
_g = 0;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_RED:
|
||||
_r = 255;
|
||||
_g = 0;
|
||||
_b = 0;
|
||||
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:
|
||||
PX4_WARN("color unknown");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_mode(rgbled_mode_t mode)
|
||||
{
|
||||
if (mode != _mode) {
|
||||
_mode = mode;
|
||||
|
||||
switch (mode) {
|
||||
case RGBLED_MODE_OFF:
|
||||
_should_run = false;
|
||||
send_led_enable(false);
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_ON:
|
||||
_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;
|
||||
_counter = 0;
|
||||
_led_interval = 25;
|
||||
send_led_enable(true);
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_PATTERN:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_brightness = 1.0f;
|
||||
send_led_enable(true);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("mode unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
/* if it should run now, start the workq */
|
||||
if (_should_run && !_running) {
|
||||
_running = true;
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 1);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set pattern for PATTERN mode, but don't change current mode
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_pattern(rgbled_pattern_t *pattern)
|
||||
{
|
||||
memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sent ENABLE flag to LED driver
|
||||
*/
|
||||
int
|
||||
RGBLED_PWM::send_led_enable(bool enable)
|
||||
{
|
||||
_enable = enable;
|
||||
send_led_rgb();
|
||||
return (OK);
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this,
|
||||
USEC2TICK(_led_controller.maximum_update_interval()));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -535,16 +262,9 @@ int
|
||||
RGBLED_PWM::send_led_rgb()
|
||||
{
|
||||
|
||||
if (_enable) {
|
||||
led_pwm_servo_set(0, _r);
|
||||
led_pwm_servo_set(1, _g);
|
||||
led_pwm_servo_set(2, _b);
|
||||
|
||||
} else {
|
||||
led_pwm_servo_set(0, 0);
|
||||
led_pwm_servo_set(1, 0);
|
||||
led_pwm_servo_set(2, 0);
|
||||
}
|
||||
led_pwm_servo_set(0, _r);
|
||||
led_pwm_servo_set(1, _g);
|
||||
led_pwm_servo_set(2, _b);
|
||||
|
||||
return (OK);
|
||||
}
|
||||
@@ -552,7 +272,8 @@ RGBLED_PWM::send_led_rgb()
|
||||
int
|
||||
RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
|
||||
{
|
||||
powersave = OK; on = _enable;
|
||||
powersave = OK;
|
||||
on = _r > 0 || _g > 0 || _b > 0;
|
||||
r = _r;
|
||||
g = _g;
|
||||
b = _b;
|
||||
@@ -562,7 +283,7 @@ RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
|
||||
void
|
||||
rgbled_usage()
|
||||
{
|
||||
PX4_WARN("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
|
||||
PX4_INFO("missing command: try 'start', 'status', 'stop'");
|
||||
}
|
||||
|
||||
int
|
||||
@@ -592,9 +313,6 @@ rgbled_main(int argc, char *argv[])
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (g_rgbled != nullptr) {
|
||||
errx(1, "already started");
|
||||
@@ -624,68 +342,15 @@ rgbled_main(int argc, char *argv[])
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test")) {
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "info")) {
|
||||
g_rgbled->info();
|
||||
if (!strcmp(verb, "status")) {
|
||||
g_rgbled->status();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
|
||||
close(fd);
|
||||
|
||||
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "rgb")) {
|
||||
if (argc < 5) {
|
||||
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
|
||||
}
|
||||
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
rgbled_rgbset_t v;
|
||||
v.red = strtol(argv[2], nullptr, 0);
|
||||
v.green = strtol(argv[3], nullptr, 0);
|
||||
v.blue = strtol(argv[4], nullptr, 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);
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
rgbled_usage();
|
||||
|
||||
Reference in New Issue
Block a user