diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h
index 5f999f6ca2..7172eeaf52 100644
--- a/ArduCopter/GCS.h
+++ b/ArduCopter/GCS.h
@@ -152,6 +152,7 @@ private:
 	uint16_t waypoint_count;
 	uint32_t waypoint_timelast_send; // milliseconds
 	uint32_t waypoint_timelast_receive; // milliseconds
+	uint32_t waypoint_timelast_request; // milliseconds
 	uint16_t waypoint_send_timeout; // milliseconds
 	uint16_t waypoint_receive_timeout; // milliseconds
 
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index ce351f2987..1f4a5c10bd 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -572,18 +572,26 @@ GCS_MAVLINK::update(void)
         send_message(MSG_NEXT_PARAM);
     }
 
+    if (!waypoint_receiving && !waypoint_sending) {
+        return;
+    }
+
+    uint32_t tnow = millis();
+
     if (waypoint_receiving &&
-        waypoint_request_i <= (unsigned)g.waypoint_total) {
+        waypoint_request_i <= (unsigned)g.waypoint_total &&
+        tnow > waypoint_timelast_request + 500) {
+        waypoint_timelast_request = tnow;
         send_message(MSG_NEXT_WAYPOINT);
     }
 
     // stop waypoint sending if timeout
-    if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
+    if (waypoint_sending && (tnow - waypoint_timelast_send) > waypoint_send_timeout){
         waypoint_sending = false;
     }
 
     // stop waypoint receiving if timeout
-    if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
+    if (waypoint_receiving && (tnow - waypoint_timelast_receive) > waypoint_receive_timeout){
         waypoint_receiving = false;
     }
 }
@@ -1151,6 +1159,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			waypoint_receiving   = true;
 			waypoint_sending	 = false;
 			waypoint_request_i   = 0;
+            waypoint_timelast_request = 0;
 			break;
 		}
 
@@ -1296,6 +1305,7 @@ 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 > (uint16_t)g.waypoint_total){