GCS_MAVLink: tidy send_message function

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -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,