From d952ccf968cd8e3fd62dd001138bd552333b6033 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 7 Jan 2013 11:02:50 +1100
Subject: [PATCH] Copter: added main_loop_ready() function

this tells us when the main loop is ready to run. MAVLink won't send a
message if the main loop could run.
---
 ArduCopter/ArduCopter.pde  | 15 ++++++++++++---
 ArduCopter/GCS_Mavlink.pde |  7 +++++++
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 45950c070d..96e85aeb26 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -927,15 +927,24 @@ void setup() {
     init_ardupilot();
 }
 
+/*
+  return true if the main loop is ready to run. This is used by
+  potentially expensive functions that are not timing critical, to
+  defer the expensive processing until after the main loop has run.
+ */
+static bool main_loop_ready(void)
+{
+    return ins.num_samples_available() >= 2;
+}
+
+
 void loop()
 {
     uint32_t timer = micros();
-    uint16_t num_samples;
 
     // We want this to execute fast
     // ----------------------------
-    num_samples = ins.num_samples_available();
-    if (num_samples >= 2) {
+    if (main_loop_ready()) {
 
         #if DEBUG_FAST_LOOP == ENABLED
         Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer));
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index c421be7c85..570a465015 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -543,6 +543,13 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         return false;
     }
 
+    // if the ins has new data for the main loop then don't send a
+    // mavlink message. We want to prioritise the main flight control
+    // loop over communications
+    if (main_loop_ready()) {
+        return false;
+    }
+
     switch(id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);