diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp index 29a6000377..78fb2e55a3 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp @@ -43,12 +43,15 @@ void AP_EFI_Serial_MS::update() const uint32_t expected_bytes = 2 + (RT_LAST_OFFSET - RT_FIRST_OFFSET) + 4; if (port->available() >= expected_bytes && read_incoming_realtime_data()) { - last_response_ms = now; copy_to_frontend(); } - if (now - last_response_ms > 100) { + const uint32_t last_request_delta = (now - last_request_ms); + const uint32_t available = port->available(); + if (((last_request_delta > 150) && (available > 0)) || // nothing in our input buffer 150 ms after request + ((last_request_delta > 90) && (available == 0))) { // we requested something over 90 ms ago, but didn't get any data port->discard_input(); + last_request_ms = now; // Request an update from the realtime table (7). // The data we need start at offset 6 and ends at 129 send_request(7, RT_FIRST_OFFSET, RT_LAST_OFFSET); diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.h b/libraries/AP_EFI/AP_EFI_Serial_MS.h index 533d2dab93..89dd018bf0 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_MS.h +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.h @@ -46,7 +46,7 @@ private: uint8_t step; uint8_t response_flag; uint16_t message_counter; - uint32_t last_response_ms; + uint32_t last_request_ms; // confirmed that last command was ok bool last_command_confirmed;