From 7343de2893a955303ac82c016c3c862e014d3c85 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 21 Aug 2014 07:31:53 +1000
Subject: [PATCH] AP_Notify: avoid suspend_timer_procs() by using atomic
 updates in ToshibaLED_PX4

---
 libraries/AP_Notify/ToshibaLED_PX4.cpp | 28 ++++++++++++++------------
 libraries/AP_Notify/ToshibaLED_PX4.h   | 13 +++++++++++-
 2 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_Notify/ToshibaLED_PX4.cpp b/libraries/AP_Notify/ToshibaLED_PX4.cpp
index 40a6d8ab88..e9d3a13700 100644
--- a/libraries/AP_Notify/ToshibaLED_PX4.cpp
+++ b/libraries/AP_Notify/ToshibaLED_PX4.cpp
@@ -41,8 +41,8 @@ bool ToshibaLED_PX4::hw_init()
         return false;
     }
     ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
-    last.zero();
-    next.zero();
+    last.v = 0;
+    next.v = 0;
     hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
     return true;
 }
@@ -50,28 +50,30 @@ bool ToshibaLED_PX4::hw_init()
 // 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)
 {
-    hal.scheduler->suspend_timer_procs();
-    next[0] = red;
-    next[1] = green;
-    next[2] = blue;
-    hal.scheduler->resume_timer_procs();
+    union rgb_value v;
+    v.r = red;
+    v.g = green;
+    v.b = blue;
+    // this does an atomic 32 bit update
+    next.v = v.v;
     return true;
 }
 
 void ToshibaLED_PX4::update_timer(void)
 {
-    if (last == next) {
+    if (last.v == next.v) {
         return;
     }
     rgbled_rgbset_t v;
-
-    v.red   = next[0];
-    v.green = next[1];
-    v.blue  = next[2];
+    union rgb_value newv;
+    newv.v = next.v;
+    v.red   = newv.r;
+    v.green = newv.g;
+    v.blue  = newv.b;
 
     ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
 
-    last = next;
+    last.v = next.v;
 }
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_Notify/ToshibaLED_PX4.h b/libraries/AP_Notify/ToshibaLED_PX4.h
index 66b93a9611..01698063b5 100644
--- a/libraries/AP_Notify/ToshibaLED_PX4.h
+++ b/libraries/AP_Notify/ToshibaLED_PX4.h
@@ -30,8 +30,19 @@ public:
 private:
     int _rgbled_fd;
     void update_timer(void);
+
+    // use a union so that updates can be of a single 32 bit value,
+    // making it atomic on PX4
+    union rgb_value {
+        struct {
+            uint8_t r;
+            uint8_t g;
+            uint8_t b;
+        };
+        volatile uint32_t v;
+    };
     
-    VectorN<uint8_t,3> last, next;
+    union rgb_value last, next;
 };
 
 #endif // __TOSHIBA_LED_PX4_H__