AP_Notify: sync oreoleds before a reboot

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];

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