From 23d8bfa976f7fef07f3704cecced47a8ac39cac8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 24 Jan 2014 10:40:39 +1100
Subject: [PATCH] AP_Notify: reduce the latency in the main task of LED updates

the ioctl now gets called in a IO timer callback
---
 libraries/AP_Notify/ToshibaLED_PX4.cpp | 32 +++++++++++++++++++-------
 libraries/AP_Notify/ToshibaLED_PX4.h   |  5 ++++
 2 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Notify/ToshibaLED_PX4.cpp b/libraries/AP_Notify/ToshibaLED_PX4.cpp
index 4abb0036b8..40a6d8ab88 100644
--- a/libraries/AP_Notify/ToshibaLED_PX4.cpp
+++ b/libraries/AP_Notify/ToshibaLED_PX4.cpp
@@ -41,21 +41,37 @@ bool ToshibaLED_PX4::hw_init()
         return false;
     }
     ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
+    last.zero();
+    next.zero();
+    hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
     return true;
 }
 
 // set_rgb - set color as a combination of red, green and blue values
 bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    rgbled_rgbset_t v;
-
-    v.red   = red;
-    v.green = green;
-    v.blue  = blue;
-
-    int ret = ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
-    return (ret == 0);
+    hal.scheduler->suspend_timer_procs();
+    next[0] = red;
+    next[1] = green;
+    next[2] = blue;
+    hal.scheduler->resume_timer_procs();
+    return true;
 }
 
+void ToshibaLED_PX4::update_timer(void)
+{
+    if (last == next) {
+        return;
+    }
+    rgbled_rgbset_t v;
+
+    v.red   = next[0];
+    v.green = next[1];
+    v.blue  = next[2];
+
+    ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
+
+    last = next;
+}
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_Notify/ToshibaLED_PX4.h b/libraries/AP_Notify/ToshibaLED_PX4.h
index 21ecb1b4f5..66b93a9611 100644
--- a/libraries/AP_Notify/ToshibaLED_PX4.h
+++ b/libraries/AP_Notify/ToshibaLED_PX4.h
@@ -19,6 +19,8 @@
 #define __TOSHIBA_LED_PX4_H__
 
 #include "ToshibaLED.h"
+#include "AP_Math.h"
+#include "vectorN.h"
 
 class ToshibaLED_PX4 : public ToshibaLED
 {
@@ -27,6 +29,9 @@ public:
     bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
 private:
     int _rgbled_fd;
+    void update_timer(void);
+    
+    VectorN<uint8_t,3> last, next;
 };
 
 #endif // __TOSHIBA_LED_PX4_H__