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

@@ -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 +=
+                // we try to keep output on a regular clock to avoid
-                    get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id]);
+                // 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