Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: avoid catching up on sending messages when sending disrupted

Browse Source 
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;
+    }
+
This commit is contained in:
Peter Barker 2020-08-24 13:10:58 +10:00 committed by Andrew Tridgell
parent 8f1e9ebc7f
commit 45f7322af6
1 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1066,6 +1066,7 @@ void GCS_MAVLINK::update_send()
#endif #endif
const uint32_t start = AP_HAL::millis(); 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 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().out_of_time()) {
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
@ -1085,7 +1086,15 @@ void GCS_MAVLINK::update_send()
if (!do_try_send_message(deferred_message[next].id)) { if (!do_try_send_message(deferred_message[next].id)) {
break; 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 next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
const uint32_t stop = AP_HAL::micros(); const uint32_t stop = AP_HAL::micros();
@ -1126,8 +1135,14 @@ void GCS_MAVLINK::update_send()
bucket_message_ids_to_send.clear(next); bucket_message_ids_to_send.clear(next);
if (bucket_message_ids_to_send.count() == 0) { if (bucket_message_ids_to_send.count() == 0) {
// we sent everything in the bucket. Reschedule it. // 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(); find_next_bucket_to_send();
} }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS #if GCS_DEBUG_SEND_MESSAGE_TIMINGS