From 77926a6965780d72f3cd001c516bc929fcb1aee2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 11 Sep 2012 16:39:36 +1000
Subject: [PATCH] APM: switch to Randys new IMU rate control method

this drives the speed of the 50Hz loop by the number of samples
accumulated in the IMU. This should give much more consistent timing
in DCM.

Thanks to Randy for introducing this scheme in ArduCopter!
---
 ArduPlane/ArduPlane.pde |  8 ++++----
 ArduPlane/config.h      | 12 ++++++++++++
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index ae867cffaa..68a0f80eda 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -709,9 +709,9 @@ void setup() {
 
 void loop()
 {
-    // We want this to execute at 50Hz if possible
-    // -------------------------------------------
-    if (millis() - fast_loopTimer_ms > 19) {
+    // We want this to execute at 50Hz, but synchronised with the gyro/accel
+    uint16_t num_samples = imu.num_samples_available();
+    if (num_samples >= NUM_IMU_SAMPLES_FOR_50HZ) {
         delta_ms_fast_loop      = millis() - fast_loopTimer_ms;
         load                = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop;
         G_Dt                = (float)delta_ms_fast_loop / 1000.f;
@@ -745,7 +745,7 @@ void loop()
         }
 
         fast_loopTimeStamp_ms = millis();
-    } else {
+    } else if (num_samples < NUM_IMU_SAMPLES_FOR_50HZ-1) {
         // less than 20ms has passed. We have at least one millisecond
         // of free time. The most useful thing to do with that time is
         // to accumulate some sensor readings, specifically the
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index 98652489d9..59a9cd6112 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -853,3 +853,15 @@
 #ifndef SERIAL_BUFSIZE
 # define SERIAL_BUFSIZE 256
 #endif
+
+#if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
+ # define NUM_IMU_SAMPLES_FOR_200HZ 5
+ # define NUM_IMU_SAMPLES_FOR_100HZ 10
+ # define NUM_IMU_SAMPLES_FOR_50HZ  20
+#endif
+
+#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
+ # define NUM_IMU_SAMPLES_FOR_200HZ 1
+ # define NUM_IMU_SAMPLES_FOR_100HZ 2
+ # define NUM_IMU_SAMPLES_FOR_50HZ  4
+#endif