dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Try to open RGBLEDs in commander (WIP)

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/commander/commander.cpp
View File

@ -1331,11 +1331,11 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
} else if (armed->ready_to_arm) {
/* ready to arm, blink at 2.5Hz */
if (leds_counter & 8) {
led_on(LED_AMBER);
if (leds_counter % 8 == 0) {
led_toggle(LED_AMBER);
} else {
led_off(LED_AMBER);
led_toggle(LED_AMBER);
}
} else {
@ -1358,9 +1358,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
}
leds_counter++;
if (leds_counter >= 16)
leds_counter = 0;
}
void

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;