Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 14:38:44 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: tidy send_message function

Browse Source
This commit is contained in:
Peter Barker 2017-07-17 14:35:17 +10:00 committed by Francisco Ferreira
parent 6b429bd372
commit fdac13d3d3
1 changed files with 24 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -764,49 +764,54 @@ void GCS_MAVLINK::send_message(enum ap_message id)
save_signing_timestamp(false); save_signing_timestamp(false);
} }
// see if we can send the deferred messages, if any // see if we can send the deferred messages, if any:
while (num_deferred_messages != 0) { while (num_deferred_messages != 0) {
if (!try_send_message(deferred_messages[next_deferred_message])) { if (!try_send_message(deferred_messages[next_deferred_message])) {
break; break;
} }
next_deferred_message++; next_deferred_message++;
if (next_deferred_message == MSG_RETRY_DEFERRED) { if (next_deferred_message == ARRAY_SIZE(deferred_messages)) {
next_deferred_message = 0; next_deferred_message = 0;
} }
num_deferred_messages--; num_deferred_messages--;
} }
// MSG_RETRY_DEFERRED is a "marker" message only used to push the queue
if (id == MSG_RETRY_DEFERRED) { if (id == MSG_RETRY_DEFERRED) {
return; return;
} }
// this message id might already be deferred // if there are no deferred messages, attempt to send straight away:
if (num_deferred_messages == 0) {
if (try_send_message(id)) {
// yay, we sent it!
return;
}
}
// we failed to send the message this time around, so try to defer:
if (num_deferred_messages == ARRAY_SIZE(deferred_messages)) {
// the defer buffer is full, discard this attempt to send.
// Note that the message *may* already be in the defer buffer
return;
}
// check if this message is deferred:
for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) { for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) {
if (deferred_messages[nextid] == id) { if (deferred_messages[nextid] == id) {
// it's already deferred, discard // it's already deferred
return; return;
} }
nextid++; nextid++;
if (nextid == MSG_RETRY_DEFERRED) { if (nextid == ARRAY_SIZE(deferred_messages)) {
nextid = 0; nextid = 0;
} }
} }
if (num_deferred_messages != 0 || // not already deferred, defer it
!try_send_message(id)) {
// can't send it now, so defer it
if (num_deferred_messages == MSG_RETRY_DEFERRED) {
// the defer buffer is full, discard
return;
}
nextid = next_deferred_message + num_deferred_messages;
if (nextid >= MSG_RETRY_DEFERRED) {
nextid -= MSG_RETRY_DEFERRED;
}
deferred_messages[nextid] = id; deferred_messages[nextid] = id;
num_deferred_messages++; num_deferred_messages++;
} }
}
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) mavlink_message_t &msg)