From 891447d798c10009f709c257ac7f81e0f0c7baec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 24 Nov 2018 10:57:45 +1100 Subject: [PATCH] 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 | 88 ++++++++++------------------- libraries/AP_Notify/OreoLED_I2C.h | 6 +- 2 files changed, 31 insertions(+), 63 deletions(-) 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; };