GCS_MAVLINK: moved deferred message handling into common code

libraries/GCS_MAVLink/GCS.h

@@ -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);
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -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++;
+    }
+}