From 95c66a794e1e1382d1d29bcf0f7802a20e3312d2 Mon Sep 17 00:00:00 2001
From: Angus Peart <angusp@gmail.com>
Date: Tue, 12 May 2015 18:30:15 +0800
Subject: [PATCH] AP_Notify: sync oreoleds before a reboot

---
 libraries/AP_Notify/OreoLED_PX4.cpp | 29 +++++++++++++++++++++++++++++
 libraries/AP_Notify/OreoLED_PX4.h   |  8 ++++++--
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Notify/OreoLED_PX4.cpp b/libraries/AP_Notify/OreoLED_PX4.cpp
index b394e589f7..33db5976a0 100644
--- a/libraries/AP_Notify/OreoLED_PX4.cpp
+++ b/libraries/AP_Notify/OreoLED_PX4.cpp
@@ -89,6 +89,8 @@ void OreoLED_PX4::update()
 
     // handle firmware update event
     if (AP_Notify::flags.firmware_update) {
+        // Force a syncronisation before setting the free-running colour cycle macro
+        send_sync();
         set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_COLOUR_CYCLE);
         return;
     }
@@ -290,6 +292,28 @@ void OreoLED_PX4::set_macro(uint8_t instance, oreoled_macro macro)
     _state_desired_semaphore = false;
 }
 
+// set_macro - set macro for one or all LEDs
+void OreoLED_PX4::send_sync(void)
+{
+    // return immediately if no healthy leds
+    if (!_overall_health) {
+        return;
+    }
+
+    // set semaphore
+    _state_desired_semaphore = true;
+
+    for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
+        if (_state_desired[i].mode != OREOLED_MODE_SYNC) {
+            _state_desired[i].mode = OREOLED_MODE_SYNC;
+            _send_required = true;
+        }
+    }  
+
+    // release semaphore
+    _state_desired_semaphore = false;
+}
+
 // update_timer - called by scheduler and updates PX4 driver with commands
 void OreoLED_PX4::update_timer(void)
 {
@@ -324,6 +348,11 @@ void OreoLED_PX4::update_timer(void)
                     ioctl(_oreoled_fd, OREOLED_SET_RGB, (unsigned long)&rgb_set);
                     }
                     break;
+                case OREOLED_MODE_SYNC:
+                    {
+                    ioctl(_oreoled_fd, OREOLED_FORCE_SYNC, 0);
+                    }
+                    break;
             }
             // save state change
             _state_sent[i] = _state_desired[i];
diff --git a/libraries/AP_Notify/OreoLED_PX4.h b/libraries/AP_Notify/OreoLED_PX4.h
index dca312c352..1a430fbf40 100644
--- a/libraries/AP_Notify/OreoLED_PX4.h
+++ b/libraries/AP_Notify/OreoLED_PX4.h
@@ -53,14 +53,18 @@ private:
     // set_rgb - set color as a combination of red, green and blue values for one or all LEDs
     void set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue);
 
-    // set_macrxo - set macro for one or all LEDs
+    // set_macro - set macro for one or all LEDs
     void set_macro(uint8_t instance, enum oreoled_macro macro);
 
+    // send_sync - force a syncronisation of the LEDs
+    void send_sync(void);
+
     // oreo led modes (pattern, macro or rgb)
     enum oreoled_mode {
         OREOLED_MODE_PATTERN,
         OREOLED_MODE_MACRO,
-        OREOLED_MODE_RGB
+        OREOLED_MODE_RGB,
+        OREOLED_MODE_SYNC
     };
 
     // oreo_state structure holds possible state of an led