fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 15:33:33 +08: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
src/modules/commander/commander.cpp
+3 -6
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) { } else if (armed->ready_to_arm) {
/* ready to arm, blink at 2.5Hz */ /* ready to arm, blink at 2.5Hz */
if (leds_counter & 8) { if (leds_counter % 8 == 0) {
led_on(LED_AMBER); led_toggle(LED_AMBER);
} else { } else {
led_off(LED_AMBER); led_toggle(LED_AMBER);
} }
} else { } else {
@@ -1358,9 +1358,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
} }
leds_counter++; leds_counter++;
if (leds_counter >= 16)
leds_counter = 0;
} }
void void
src/modules/commander/commander_helper.cpp
+35 -10
View File
@@ -53,6 +53,8 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h> #include <drivers/drv_tone_alarm.h>
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <drivers/drv_rgbled.h>
#include "commander_helper.h" #include "commander_helper.h"
@@ -136,9 +138,11 @@ void tune_stop()
} }
static int leds; static int leds;
static int rgbleds;
int led_init() int led_init()
{ {
/* first open normal LEDs */
leds = open(LED_DEVICE_PATH, 0); leds = open(LED_DEVICE_PATH, 0);
if (leds < 0) { if (leds < 0) {
@@ -146,10 +150,29 @@ int led_init()
return ERROR; return ERROR;
} }
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { /* the blue LED is only available on FMUv1 but not FMUv2 */
warnx("LED: ioctl fail\n"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(leds, LED_ON, LED_BLUE)) {
warnx("Blue LED: ioctl fail\n");
return ERROR; 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; return 0;
} }
@@ -157,18 +180,15 @@ int led_init()
void led_deinit() void led_deinit()
{ {
close(leds); close(leds);
if (rgbleds != -1) {
close(rgbleds);
}
} }
int led_toggle(int led) int led_toggle(int led)
{ {
static int last_blue = LED_ON; return ioctl(leds, LED_TOGGLE, led);
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);
} }
int led_on(int led) int led_on(int led)
@@ -181,6 +201,11 @@ int led_off(int led)
return ioctl(leds, LED_OFF, 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 battery_remaining_estimate_voltage(float voltage)
{ {
float ret = 0; float ret = 0;