From 0c4e3dce0ef82ea3a7dd2b7bed8b23108f34205d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 15 Aug 2013 17:34:49 +0200
Subject: [PATCH] Added LED_TOGGLE for normal LEDs

---
 src/drivers/boards/px4fmu-v1/px4fmu_led.c  | 19 +++++++++++++++++++
 src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 12 ++++++++++++
 src/drivers/drv_led.h                      |  1 +
 src/drivers/led/led.cpp                    |  6 ++++++
 4 files changed, 38 insertions(+)

diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index aa83ec130d..ea91f34ad6 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -57,6 +57,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 __EXPORT void led_init()
@@ -94,3 +95,21 @@ __EXPORT void led_off(int led)
 		stm32_gpiowrite(GPIO_LED2, true);
 	}
 }
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 0)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED2))
+			stm32_gpiowrite(GPIO_LED2, false);
+		else
+			stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
index 5ded4294e6..a856ccb027 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -57,6 +57,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 __EXPORT void led_init()
@@ -83,3 +84,14 @@ __EXPORT void led_off(int led)
 		stm32_gpiowrite(GPIO_LED1, true);
 	}
 }
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index 97f2db107a..4ce04696e0 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -54,6 +54,7 @@
 
 #define LED_ON			_IOC(_LED_BASE, 0)
 #define LED_OFF			_IOC(_LED_BASE, 1)
+#define LED_TOGGLE		_IOC(_LED_BASE, 2)
 
 __BEGIN_DECLS
 
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index fe307223f2..a37eaca533 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -52,6 +52,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 class LED : device::CDev
@@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		led_off(arg);
 		break;
 
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+
 	default:
 		result = CDev::ioctl(filp, cmd, arg);
 	}