From 6debbcb20d6deca75783019365b660b7a1a0ba18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Feb 2017 10:04:52 +0100 Subject: [PATCH] navio: switch to new led uorb interface --- src/drivers/boards/rpi/board_config.h | 2 + src/drivers/navio_rgbled/CMakeLists.txt | 1 + src/drivers/navio_rgbled/navio_rgbled.cpp | 175 ++++++++++------------ src/drivers/navio_rgbled/navio_rgbled.h | 36 +---- 4 files changed, 90 insertions(+), 124 deletions(-) diff --git a/src/drivers/boards/rpi/board_config.h b/src/drivers/boards/rpi/board_config.h index 15178c8dbb..2e44158a2e 100644 --- a/src/drivers/boards/rpi/board_config.h +++ b/src/drivers/boards/rpi/board_config.h @@ -45,3 +45,5 @@ #define BOARD_BATTERY1_A_PER_V (15.391030303f) #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER + +#define BOARD_MAX_LEDS 1 // Number external of LED's this board has diff --git a/src/drivers/navio_rgbled/CMakeLists.txt b/src/drivers/navio_rgbled/CMakeLists.txt index 26b3e587f5..a9f70d6837 100644 --- a/src/drivers/navio_rgbled/CMakeLists.txt +++ b/src/drivers/navio_rgbled/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS platforms__common drivers__navio_gpio + lib__led ) #add_subdirectory(test) diff --git a/src/drivers/navio_rgbled/navio_rgbled.cpp b/src/drivers/navio_rgbled/navio_rgbled.cpp index f14c78ae44..038e832063 100644 --- a/src/drivers/navio_rgbled/navio_rgbled.cpp +++ b/src/drivers/navio_rgbled/navio_rgbled.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ #include -#include +#include #include "navio_rgbled.h" @@ -41,9 +41,31 @@ #define GPIO_LED_G (GPIO_PIN27) #define GPIO_LED_B (GPIO_PIN6) +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" + +// inverted +#define LED_ON 0 +#define LED_OFF 1 + using namespace DriverFramework; +RGBLED::RGBLED(const char *name) + : DevObj(name, + RGBLED0_DEVICE_PATH, + RGBLED_BASE_DEVICE_PATH, + DeviceBusType_UNKNOWN, + 0) +{ +}; + +RGBLED::~RGBLED() +{ + if (_led_controller.led_control_subscription() >= 0) { + orb_unsubscribe(_led_controller.led_control_subscription()); + } +}; + int RGBLED::start() { int res; @@ -61,6 +83,9 @@ int RGBLED::start() _gpio.configgpio(GPIO_LED_CNF | GPIO_LED_G); _gpio.configgpio(GPIO_LED_CNF | GPIO_LED_B); + // update at fixed interval + DevObj::setSampleInterval(_led_controller.maximum_update_interval()); + return 0; } @@ -81,100 +106,66 @@ int RGBLED::stop() return 0; } -int RGBLED::devIOCTL(unsigned long request, unsigned long arg) -{ - int ret = ENOTTY; - rgbled_rgbset_t *rgb; - - switch (request) { - case RGBLED_SET_RGB: - ret = 0; - rgb = (rgbled_rgbset_t *)arg; - _rgb.red = (rgb->red != 0) ? LED_ON : LED_OFF; - _rgb.green = (rgb->green != 0) ? LED_ON : LED_OFF; - _rgb.blue = (rgb->blue != 0) ? LED_ON : LED_OFF; - _gpio.gpiowrite(GPIO_LED_R, _rgb.red); - _gpio.gpiowrite(GPIO_LED_G, _rgb.green); - _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); - break; - - case RGBLED_SET_COLOR: - if (arg > _max_color) { - ret = ENOTSUP; - - } else { - _rgb = _rgbsets[arg]; - _gpio.gpiowrite(GPIO_LED_R, _rgb.red); - _gpio.gpiowrite(GPIO_LED_G, _rgb.green); - _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); - ret = 0; - } - - break; - - case RGBLED_SET_MODE: - ret = 0; - - switch (arg) { - case RGBLED_MODE_ON: - DevObj::setSampleInterval(0); - break; - - case RGBLED_MODE_BLINK_SLOW: - DevObj::setSampleInterval(2000 * 1000); - break; - - case RGBLED_MODE_BLINK_NORMAL: - DevObj::setSampleInterval(500 * 1000); - break; - - case RGBLED_MODE_BLINK_FAST: - DevObj::setSampleInterval(100 * 1000); - break; - - case RGBLED_MODE_BREATHE: - DevObj::setSampleInterval(1500 * 1000); - break; - - default: - ret = ENOTSUP; - } - - if (!m_work_handle.isValid()) { - // this can fail - DevObj::start(); - } - - break; - - case RGBLED_PLAY_SCRIPT_NAMED: - case RGBLED_PLAY_SCRIPT: - case RGBLED_SET_USER_SCRIPT: - case RGBLED_SET_PATTERN: - ret = ENOTSUP; - break; - - default: - ret = DevObj::devIOCTL(request, arg); - break; - } - - return ret; -} - void RGBLED::_measure() { - if (_turn) { - _gpio.gpiowrite(GPIO_LED_R, LED_OFF); - _gpio.gpiowrite(GPIO_LED_G, LED_OFF); - _gpio.gpiowrite(GPIO_LED_B, LED_OFF); - _turn = false; + if (!_led_controller.is_init()) { + int led_control_sub = orb_subscribe(ORB_ID(led_control)); + _led_controller.init(led_control_sub); + } - } else { - _gpio.gpiowrite(GPIO_LED_R, _rgb.red); - _gpio.gpiowrite(GPIO_LED_G, _rgb.green); - _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); - _turn = true; + 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: + _gpio.gpiowrite(GPIO_LED_R, LED_ON); + _gpio.gpiowrite(GPIO_LED_G, LED_OFF); + _gpio.gpiowrite(GPIO_LED_B, LED_OFF); + break; + + case led_control_s::COLOR_GREEN: + _gpio.gpiowrite(GPIO_LED_R, LED_OFF); + _gpio.gpiowrite(GPIO_LED_G, LED_ON); + _gpio.gpiowrite(GPIO_LED_B, LED_OFF); + break; + + case led_control_s::COLOR_BLUE: + _gpio.gpiowrite(GPIO_LED_R, LED_OFF); + _gpio.gpiowrite(GPIO_LED_G, LED_OFF); + _gpio.gpiowrite(GPIO_LED_B, LED_ON); + break; + + case led_control_s::COLOR_AMBER: //make it the same as yellow + case led_control_s::COLOR_YELLOW: + _gpio.gpiowrite(GPIO_LED_R, LED_ON); + _gpio.gpiowrite(GPIO_LED_G, LED_ON); + _gpio.gpiowrite(GPIO_LED_B, LED_OFF); + break; + + case led_control_s::COLOR_PURPLE: + _gpio.gpiowrite(GPIO_LED_R, LED_ON); + _gpio.gpiowrite(GPIO_LED_G, LED_OFF); + _gpio.gpiowrite(GPIO_LED_B, LED_ON); + break; + + case led_control_s::COLOR_CYAN: + _gpio.gpiowrite(GPIO_LED_R, LED_OFF); + _gpio.gpiowrite(GPIO_LED_G, LED_ON); + _gpio.gpiowrite(GPIO_LED_B, LED_ON); + break; + + case led_control_s::COLOR_WHITE: + _gpio.gpiowrite(GPIO_LED_R, LED_ON); + _gpio.gpiowrite(GPIO_LED_G, LED_ON); + _gpio.gpiowrite(GPIO_LED_B, LED_ON); + break; + + default: // led_control_s::COLOR_OFF + _gpio.gpiowrite(GPIO_LED_R, LED_OFF); + _gpio.gpiowrite(GPIO_LED_G, LED_OFF); + _gpio.gpiowrite(GPIO_LED_B, LED_OFF); + break; + } } } diff --git a/src/drivers/navio_rgbled/navio_rgbled.h b/src/drivers/navio_rgbled/navio_rgbled.h index 1f817a0634..db5874ad96 100644 --- a/src/drivers/navio_rgbled/navio_rgbled.h +++ b/src/drivers/navio_rgbled/navio_rgbled.h @@ -36,12 +36,7 @@ #include "navio_gpio.h" -#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" -#define RGBLED_DEVICE_PATH "/dev/rgbled0" - -// inverted -#define LED_ON 0 -#define LED_OFF 1 +#include using namespace navio_gpio; @@ -49,39 +44,16 @@ using namespace navio_gpio; class RGBLED : public DevObj { public: - RGBLED(const char *name) : - DevObj(name, - RGBLED_DEVICE_PATH, - RGBLED_BASE_DEVICE_PATH, - DeviceBusType_UNKNOWN, - 0), - _rgbsets{ - {LED_OFF, LED_OFF, LED_OFF}, /* OFF */ - {LED_ON, LED_OFF, LED_OFF}, /* red */ - {LED_ON, LED_ON, LED_OFF}, /* yellow */ - {LED_ON, LED_OFF, LED_ON}, /* purple */ - {LED_OFF, LED_ON, LED_OFF}, /* green */ - {LED_OFF, LED_OFF, LED_ON}, /* blue */ - {LED_ON, LED_ON, LED_ON}, /* white */ - }, - _max_color(7), - _rgb{LED_OFF, LED_OFF, LED_OFF}, - _turn(true) - { }; - virtual ~RGBLED() - { }; + RGBLED(const char *name); + virtual ~RGBLED(); int start(); int stop(); - int devIOCTL(unsigned long request, unsigned long arg); protected: void _measure(); private: + LedController _led_controller; Gpio _gpio; - const rgbled_rgbset_t _rgbsets[7]; - const int _max_color; - rgbled_rgbset_t _rgb; - bool _turn; };