From d75c3c4e7369510db1d91721c2793c23dcd873fa Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 15 Aug 2013 17:48:28 +0200
Subject: [PATCH] Try to open RGBLEDs in commander (WIP)

---
 src/modules/commander/commander.cpp        |  9 ++---
 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 71f33d81be..4e6ecd1e4b 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 681a11568f..d9b255f4f8 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;