// 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++;
}
}
void loop(void) {
try_send_message(MAVLINK_COMM_0, MAVLINK_MSG_ID_HEARTBEAT);
simplegcs_update(MAVLINK_COMM_0);
// flush_console_to_statustext();
gcs_console_send(MAVLINK_COMM_0);
console_loopback();
hal.scheduler->delay(100);
}
