// 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); }