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