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

GCS_MAVLINK: moved deferred message handling into common code

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-19 10:56:09 +11:00 committed by Randy Mackay
parent c8eafc4d42
commit 0832aba6bc
2 changed files with 57 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/GCS_MAVLink/GCS.h
View File

@ -280,6 +280,14 @@ private:
// start page of log data
uint16_t _log_data_page;
// deferred message handling
enum ap_message deferred_messages[MSG_RETRY_DEFERRED];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
// vehicle specific message send function
bool try_send_message(enum ap_message id);
void handle_guided_request(AP_Mission::Mission_Command &cmd);
void handle_change_alt_request(AP_Mission::Mission_Command &cmd);

49
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -759,3 +759,52 @@ mission_ack:
msg->compid,
result);
}
// send a message using mavlink, handling message queueing
void GCS_MAVLINK::send_message(enum ap_message id)
{
uint8_t i, nextid;
// 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) {
next_deferred_message = 0;
}
num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) {
if (deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MSG_RETRY_DEFERRED) {
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++;
}
}