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

43
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -764,48 +764,53 @@ void GCS_MAVLINK::send_message(enum ap_message id)
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) {
if (!try_send_message(deferred_messages[next_deferred_message])) {
break;
}
next_deferred_message++;
if (next_deferred_message == MSG_RETRY_DEFERRED) {
if (next_deferred_message == ARRAY_SIZE(deferred_messages)) {
next_deferred_message = 0;
}
num_deferred_messages--;
}
// MSG_RETRY_DEFERRED is a "marker" message only used to push the queue
if (id == MSG_RETRY_DEFERRED) {
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++) {
if (deferred_messages[nextid] == id) {
// it's already deferred, discard
// it's already deferred
return;
}
nextid++;
if (nextid == MSG_RETRY_DEFERRED) {
if (nextid == ARRAY_SIZE(deferred_messages)) {
nextid = 0;
}
}
if (num_deferred_messages != 0 ||
!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;
num_deferred_messages++;
}
// not already deferred, defer it
deferred_messages[nextid] = id;
num_deferred_messages++;
}
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,