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 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; iset_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; };