diff options
-rw-r--r-- | src/modules/commander/commander.cpp | 9 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 45 |
2 files changed, 38 insertions, 16 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71f33d81b..4e6ecd1e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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 diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 681a11568..d9b255f4f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -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; |