diff --git a/libraries/AP_Notify/OreoLED_I2C.cpp b/libraries/AP_Notify/OreoLED_I2C.cpp
index 40c716f1fa..29451075d1 100644
--- a/libraries/AP_Notify/OreoLED_I2C.cpp
+++ b/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();
diff --git a/libraries/AP_Notify/OreoLED_I2C.h b/libraries/AP_Notify/OreoLED_I2C.h
index 4d72ac41e6..98cdfd6a03 100644
--- a/libraries/AP_Notify/OreoLED_I2C.h
+++ b/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;
};