forked from Archive/PX4-Autopilot
navio: switch to new led uorb interface
This commit is contained in:
Beat Küng
Lorenz Meier
parent
43b98d9a2f
commit
6debbcb20d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@ px4_add_module(
|
|||
DEPENDS
|
||||
platforms__common
|
||||
drivers__navio_gpio
|
||||
lib__led
|
||||
)
|
||||
|
||||
#add_subdirectory(test)
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@
|
|||
****************************************************************************/
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <drivers/drv_rgbled.h>
|
||||
#include <drivers/drv_led.h>
|
||||
|
||||
#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) {
|
||||
if (!_led_controller.is_init()) {
|
||||
int led_control_sub = orb_subscribe(ORB_ID(led_control));
|
||||
_led_controller.init(led_control_sub);
|
||||
}
|
||||
|
||||
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);
|
||||
_turn = false;
|
||||
|
||||
} else {
|
||||
_gpio.gpiowrite(GPIO_LED_R, _rgb.red);
|
||||
_gpio.gpiowrite(GPIO_LED_G, _rgb.green);
|
||||
_gpio.gpiowrite(GPIO_LED_B, _rgb.blue);
|
||||
_turn = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <lib/led/led.h>
|
||||
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue