Added LED_TOGGLE for normal LEDs

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);
+	}
+}

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);
+	}
+}

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
 

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);
 	}