From 7fd6748f34012b877b28196910604bc28b4078cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Feb 2017 10:05:12 +0100 Subject: [PATCH] rgbled_pwm: switch to new led uorb interface --- src/drivers/rgbled_pwm/CMakeLists.txt | 1 + src/drivers/rgbled_pwm/rgbled_pwm.cpp | 501 +++++--------------------- 2 files changed, 84 insertions(+), 418 deletions(-) diff --git a/src/drivers/rgbled_pwm/CMakeLists.txt b/src/drivers/rgbled_pwm/CMakeLists.txt index 035c468cc8..fd86ec4aa2 100644 --- a/src/drivers/rgbled_pwm/CMakeLists.txt +++ b/src/drivers/rgbled_pwm/CMakeLists.txt @@ -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 : diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp index 8dcfb32c89..5abba42da9 100644 --- a/src/drivers/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp @@ -62,7 +62,8 @@ #include -#include +#include +#include #include #include @@ -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 "); - } - - 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();