From 4b68dd48f7c35a3d461a73afd49844c9fbef6c0a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 8 Oct 2013 17:30:43 +1100
Subject: [PATCH] Copter: use ins.wait_for_sample() for main loop

this takes advantage of the INS specific method to wait for a sample
---
 ArduCopter/ArduCopter.pde | 51 ++++++++++++++++++---------------------
 ArduCopter/defines.h      |  2 ++
 2 files changed, 26 insertions(+), 27 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ab82772562..fe11c74dc8 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -953,40 +953,37 @@ static void perf_update(void)
 
 void loop()
 {
+    // wait for an INS sample
+    if (!ins.wait_for_sample(1000)) {
+        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY);
+        return;
+    }
     uint32_t timer = micros();
 
-    // We want this to execute fast
-    // ----------------------------
-    if (ins.sample_available()) {
-
-        // check loop time
-        perf_info_check_loop_time(timer - fast_loopTimer);
+    // check loop time
+    perf_info_check_loop_time(timer - fast_loopTimer);
 
-        G_Dt                            = (float)(timer - fast_loopTimer) / 1000000.f;                  // used by PI Loops
-        fast_loopTimer          = timer;
+    // used by PI Loops
+    G_Dt                    = (float)(timer - fast_loopTimer) / 1000000.f;
+    fast_loopTimer          = timer;
 
-        // for mainloop failure monitoring
-        mainLoop_count++;
+    // for mainloop failure monitoring
+    mainLoop_count++;
 
-        // Execute the fast loop
-        // ---------------------
-        fast_loop();
+    // Execute the fast loop
+    // ---------------------
+    fast_loop();
 
-        // tell the scheduler one tick has passed
-        scheduler.tick();
+    // tell the scheduler one tick has passed
+    scheduler.tick();
 
-        // run all the tasks that are due to run. Note that we only
-        // have to call this once per loop, as the tasks are scheduled
-        // in multiples of the main loop tick. So if they don't run on
-        // the first call to the scheduler they won't run on a later
-        // call until scheduler.tick() is called again
-        uint32_t time_available = (timer + 10000) - micros();
-        scheduler.run(time_available - 500);
-    }
-    if ((timer - fast_loopTimer) < 8500) {
-        // we have plenty of time - be friendly to multi-tasking OSes
-        hal.scheduler->delay(1);
-    }
+    // run all the tasks that are due to run. Note that we only
+    // have to call this once per loop, as the tasks are scheduled
+    // in multiples of the main loop tick. So if they don't run on
+    // the first call to the scheduler they won't run on a later
+    // call until scheduler.tick() is called again
+    uint32_t time_available = (timer + 10000) - micros();
+    scheduler.run(time_available - 500);
 }
 
 
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 747bdd453b..d13267f896 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -449,6 +449,8 @@ enum ap_message {
 #define ERROR_CODE_COMPASS_FAILED_TO_READ   2
 // subsystem specific error codes -- gps
 #define ERROR_CODE_GPS_GLITCH               2
+// subsystem specific error codes -- main
+#define ERROR_CODE_INS_DELAY                1