yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Try to open RGBLEDs in commander (WIP)

Browse Source
This commit is contained in:
Julian Oes
2013-08-15 17:48:28 +02:00
parent 901cef922c
commit d75c3c4e73
2 changed files with 38 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/modules/commander/commander_helper.cpp
View File

@@ -53,6 +53,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_led.h>
#include <drivers/drv_rgbled.h>
#include "commander_helper.h"
@@ -136,9 +138,11 @@ void tune_stop()
}
static int leds;
static int rgbleds;
int led_init()
{
/* first open normal LEDs */
leds = open(LED_DEVICE_PATH, 0);
if (leds < 0) {
@@ -146,10 +150,29 @@ int led_init()
return ERROR;
}
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
warnx("LED: ioctl fail\n");
/* the blue LED is only available on FMUv1 but not FMUv2 */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(leds, LED_ON, LED_BLUE)) {
warnx("Blue LED: ioctl fail\n");
return ERROR;
}
#endif
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
}
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0);
if (rgbleds == -1) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
#else
warnx("No RGB LED found");
#endif
}
return 0;
}
@@ -157,18 +180,15 @@ int led_init()
void led_deinit()
{
close(leds);
if (rgbleds != -1) {
close(rgbleds);
}
}
int led_toggle(int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
return ioctl(leds, LED_TOGGLE, led);
}
int led_on(int led)
@@ -181,6 +201,11 @@ int led_off(int led)
return ioctl(leds, LED_OFF, led);
}
int rgbled_set_color(rgbled_color_t color) {
return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color);
}
float battery_remaining_estimate_voltage(float voltage)
{
float ret = 0;