AP_Notify: fixed LED sync for OreoLED

the NuttX driver tries to send a sync every 4s, but actually ends up
sending at around 4.1s due to poor scheduling. Rather strangely, the
oreoled firmware seems to rely on this inaccuracy, and doesn't work
with exactly 4s under ChibiOS

libraries/AP_Notify/OreoLED_I2C.cpp

@@ -76,8 +76,6 @@ void OreoLED_I2C::update()
         return;    // slow rate from 50hz to 10hz
     }
 
-    sync_counter();                         // syncronizes LEDs every 10 seconds
-
     if (mode_firmware_update()) {
         return;    // don't go any further if the Pixhawk is in firmware update
     }
@@ -122,18 +120,6 @@ bool OreoLED_I2C::slow_counter()
 }
 
 
-// Calls resyncing the LEDs every 10 seconds
-// Always returns false, no action needed.
-void OreoLED_I2C::sync_counter()
-{
-    _sync_count++;
-    if (_sync_count > 100) {
-        _sync_count = 0;
-        send_sync();
-    }
-}
-
-
 // Procedure for when Pixhawk is in FW update / bootloader
 // Makes all LEDs go into color cycle mode
 // Returns true if firmware update in progress. False if not
@@ -389,21 +375,6 @@ void OreoLED_I2C::set_macro(uint8_t instance, oreoled_macro macro)
     }
 }
 
-// send_sync - force a syncronisation of the all LED's
-void OreoLED_I2C::send_sync()
-{
-    WITH_SEMAPHORE(_sem);
-
-    // store desired macro for all LEDs
-    for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
-        _state_desired[i].send_sync();
-        if (!(_state_desired[i] == _state_sent[i])) {
-            _send_required = true;
-        }
-    }
-}
-
-
 // Clear the desired state
 void OreoLED_I2C::clear_state(void)
 {
@@ -457,11 +428,6 @@ void OreoLED_I2C::boot_leds(void)
 // update_timer - called by scheduler and updates PX4 driver with commands
 void OreoLED_I2C::update_timer(void)
 {
-    // exit immediately if send not required, or state is being updated
-    if (!_send_required) {
-        return;
-    }
-
     WITH_SEMAPHORE(_sem);
 
     uint32_t now = AP_HAL::millis();
@@ -474,6 +440,21 @@ void OreoLED_I2C::update_timer(void)
         boot_leds();
     }
 
+    // send a sync every 4.1s. The PX4 driver uses 4ms, but using
+    // exactly 4ms does not work. It seems that the oreoled firmware
+    // relies on the inaccuracy of the NuttX scheduling for this to
+    // work, and exactly 4ms from ChibiOS causes syncronisation to
+    // fail
+    if (now - _last_sync_ms > 4100) {
+        _last_sync_ms = now;
+        send_sync();
+    }
+
+    // exit immediately if send not required, or state is being updated
+    if (!_send_required) {
+        return;
+    }
+
     // for each LED
     for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
 
@@ -535,18 +516,6 @@ void OreoLED_I2C::update_timer(void)
                 break;
             }
 
-            case OREOLED_MODE_SYNC: {
-                /* set I2C address to zero */
-                _dev->set_address(0);
-
-                /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
-                uint8_t msg[] = {0x01, 0x00};
-
-                /* send I2C command */
-                _dev->transfer(msg, sizeof(msg), nullptr, 0);
-                break;
-            }
-
             default:
                 break;
             };
@@ -559,6 +528,21 @@ void OreoLED_I2C::update_timer(void)
     _send_required = false;
 }
 
+void OreoLED_I2C::send_sync(void)
+{
+    /* set I2C address to zero */
+    _dev->set_address(0);
+
+    /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
+    uint8_t msg[] = {0x01, 0x00};
+
+    /* send I2C command */
+    _dev->set_retries(0);
+    _dev->transfer(msg, sizeof(msg), nullptr, 0);
+    _dev->set_retries(2);
+}
+
+
 
 // Handle an LED_CONTROL mavlink message
 void OreoLED_I2C::handle_led_control(mavlink_message_t *msg)
@@ -635,12 +619,6 @@ void OreoLED_I2C::handle_led_control(mavlink_message_t *msg)
             set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2],
                     packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 4], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 5],
                     packet.custom_bytes[CUSTOM_HEADER_LENGTH + 6], period, phase_offset);
-        } else if (memcmp(packet.custom_bytes, "SYNC", CUSTOM_HEADER_LENGTH) == 0) { // check for the SYNC sub-command
-            // check to make sure the total length matches the length of the SYN0 command + data values
-            if (packet.custom_len != CUSTOM_HEADER_LENGTH + 0) {
-                return;
-            }
-            send_sync();
         } else { // unrecognized command
             return;
         }
@@ -672,12 +650,6 @@ void OreoLED_I2C::oreo_state::clear_state()
     phase_offset = 0;
 }
 
-void OreoLED_I2C::oreo_state::send_sync()
-{
-    clear_state();
-    mode = OREOLED_MODE_SYNC;
-}
-
 void OreoLED_I2C::oreo_state::set_macro(oreoled_macro new_macro)
 {
     clear_state();

libraries/AP_Notify/OreoLED_I2C.h

@@ -110,7 +110,6 @@ private:
 
     // functions to set LEDs to specific patterns.  These functions return true if no further updates should be made to LEDs this iteration
     bool slow_counter(void);
-    void sync_counter(void);
     bool mode_firmware_update(void);
     bool mode_init(void);
     bool mode_failsafe_radio(void);
@@ -128,7 +127,6 @@ private:
         OREOLED_MODE_MACRO,
         OREOLED_MODE_RGB,
         OREOLED_MODE_RGB_EXTENDED,
-        OREOLED_MODE_SYNC
     };
 
     // Oreo LED modes
@@ -157,8 +155,6 @@ private:
 
         void clear_state();
 
-        void send_sync();
-
         void set_macro(oreoled_macro new_macro);
 
         void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue);
@@ -195,8 +191,8 @@ private:
     uint8_t _rear_color_b = 255;                    // the rear LED blue value
 
     uint8_t _slow_count;
-    uint8_t _sync_count = 80;
     uint8_t _boot_count;
     uint32_t _last_boot_ms;
+    uint32_t _last_sync_ms;
 };