AP_Notify: avoid suspend_timer_procs() by using atomic updates in ToshibaLED_PX4

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();
+    last.v = 0;
-    next.zero();
+    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();
+    union rgb_value v;
-    next[0] = red;
+    v.r = red;
-    next[1] = green;
+    v.g = green;
-    next[2] = blue;
+    v.b = blue;
-    hal.scheduler->resume_timer_procs();
+    // 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;
-
+    union rgb_value newv;
-    v.red   = next[0];
+    newv.v = next.v;
-    v.green = next[1];
+    v.red   = newv.r;
-    v.blue  = next[2];
+    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

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__