From d926360e81ea234745c8d01566dee7daf1afbbec Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 13 Dec 2012 23:09:26 +0900
Subject: [PATCH] ArduCopter: move gcs updates to run only when there are 4ms
 of spare time before the next 100hz iteration starts

---
 ArduCopter/ArduCopter.pde  | 13 +++++--------
 ArduCopter/GCS_Mavlink.pde | 30 ++++++++++++++++++++++++++++++
 2 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 846517912b..bb1f82c1de 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1003,6 +1003,11 @@ void loop()
                 compass.accumulate();
             }
         }
+
+        // process communications with the GCS
+        if (timer - fast_loopTimer < 6000) {
+            gcs_check();
+        }
     }
 
 }
@@ -1010,10 +1015,6 @@ void loop()
 // Main loop - 100hz
 static void fast_loop()
 {
-    // try to send any deferred messages if the serial port now has
-    // some space available
-    gcs_send_message(MSG_RETRY_DEFERRED);
-
     // run low level rate controllers that only require IMU data
     run_rate_controllers();
 
@@ -1232,10 +1233,6 @@ static void fifty_hz_loop()
     if (g.log_bitmask & MASK_LOG_RAW && motors.armed())
         Log_Write_Raw();
 #endif
-
-    // kick the GCS to process uplink data
-    gcs_update();
-    gcs_data_stream_send();
 }
 
 
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index a4641b0df4..03e99ae94b 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -18,6 +18,36 @@ static bool mavlink_active;
 // prototype this for use inside the GCS class
 static void gcs_send_text_fmt(const prog_char_t *fmt, ...);
 
+// gcs_check - throttles communication with ground station.
+// should be called regularly
+// returns true if it has sent a message to the ground station
+static bool gcs_check()
+{
+    static uint32_t last_1hz, last_50hz, last_5s;
+    bool sent_message = false;
+
+    uint32_t tnow = millis();
+    if (tnow - last_1hz > 1000) {
+        last_1hz = tnow;
+        gcs_send_message(MSG_HEARTBEAT);
+        gcs_send_message(MSG_EXTENDED_STATUS1);
+        sent_message = true;
+    }
+    if (tnow - last_50hz > 20 && !sent_message) {
+        last_50hz = tnow;
+        gcs_update();
+        gcs_data_stream_send();
+        sent_message = true;
+    }
+    if (tnow - last_5s > 5000 && !sent_message) {
+        last_5s = tnow;
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
+        sent_message = true;
+    }
+
+    return sent_message;
+}
+
 /*
  *  !!NOTE!!
  *