From 537e78f9fad532adf693d43c305f1e94c8f5b786 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 19 Mar 2014 06:44:37 +1100
Subject: [PATCH] Plane: much faster mission upload

send next WP request immediately if possible. This speeds up mission
upload on USB by about 10x
---
 ArduPlane/GCS_Mavlink.pde | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 2216bc1f99..23e4ed8a3b 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -993,7 +993,7 @@ GCS_MAVLINK::update(void)
 
     if (waypoint_receiving &&
         waypoint_request_i <= waypoint_request_last &&
-        tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
+        tnow - waypoint_timelast_request > 2000 + (stream_slowdown*20)) {
         waypoint_timelast_request = tnow;
         send_message(MSG_NEXT_WAYPOINT);
     }
@@ -1624,7 +1624,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             // update waypoint receiving state machine
             waypoint_timelast_receive = millis();
-            waypoint_timelast_request = 0;
             waypoint_request_i++;
 
             if (waypoint_request_i >= waypoint_request_last) {
@@ -1638,6 +1637,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 waypoint_receiving = false;
                 // XXX ignores waypoint radius for individual waypoints, can
                 // only set WP_RADIUS parameter
+            } else {
+                waypoint_timelast_request = hal.scheduler->millis();
+                // if we have enough space, then send the next WP immediately
+                if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
+                    queued_waypoint_send();
+                } else {
+                    send_message(MSG_NEXT_WAYPOINT);
+                }
             }
         }
         break;