From 45f7322af6d562e2dba3da6f25455600408596cd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 24 Aug 2020 13:10:58 +1000 Subject: [PATCH] GCS_MAVLink: avoid catching up on sending messages when sending disrupted The code here was never meant to maintain an "average" streamrate. It was designed so that we would maintain a consistent clock in the face of minor scheduling anomalies (like an EKF fusion step). The way this is written, however, makes us spit out a message for each of the intervals we missed - clearly not intended behaviour. This was tested by inserting the following code: void GCS_MAVLINK::update_send() { + const uint32_t xnow = AP_HAL::millis(); + if (xnow > 10000 && + xnow < 20000) { + return; + } + --- libraries/GCS_MAVLink/GCS_Common.cpp | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 702bcfc570..a698a6b467 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1066,6 +1066,7 @@ void GCS_MAVLINK::update_send() #endif const uint32_t start = AP_HAL::millis(); + const uint16_t start16 = AP_HAL::millis16(); while (AP_HAL::millis() - start < 5) { // spend a max of 5ms sending messages. This should never trigger - out_of_time() should become true if (gcs().out_of_time()) { #if GCS_DEBUG_SEND_MESSAGE_TIMINGS @@ -1085,7 +1086,15 @@ void GCS_MAVLINK::update_send() if (!do_try_send_message(deferred_message[next].id)) { break; } - deferred_message[next].last_sent_ms += deferred_message[next].interval_ms; + // we try to keep output on a regular clock to avoid + // user support questions: + const uint16_t interval_ms = deferred_message[next].interval_ms; + deferred_message[next].last_sent_ms += interval_ms; + // but we do not want to try to catch up too much: + if (uint16_t(start16 - deferred_message[next].last_sent_ms) > interval_ms) { + deferred_message[next].last_sent_ms = start16; + } + next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate #if GCS_DEBUG_SEND_MESSAGE_TIMINGS const uint32_t stop = AP_HAL::micros(); @@ -1126,8 +1135,14 @@ void GCS_MAVLINK::update_send() bucket_message_ids_to_send.clear(next); if (bucket_message_ids_to_send.count() == 0) { // we sent everything in the bucket. Reschedule it. - deferred_message_bucket[sending_bucket_id].last_sent_ms += - get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id]); + // we try to keep output on a regular clock to avoid + // user support questions: + const uint16_t interval_ms = get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id]); + deferred_message_bucket[sending_bucket_id].last_sent_ms += interval_ms; + // but we do not want to try to catch up too much: + if (uint16_t(start16 - deferred_message_bucket[sending_bucket_id].last_sent_ms) > interval_ms) { + deferred_message_bucket[sending_bucket_id].last_sent_ms = start16; + } find_next_bucket_to_send(); } #if GCS_DEBUG_SEND_MESSAGE_TIMINGS