Rover: switch to IMU driven timing, same as ArduPlane

APMrover2/APMrover2.pde

@@ -605,9 +605,9 @@ void setup() {
 
 void loop()
 {
-	// We want this to execute at 50Hz if possible
-	// -------------------------------------------
-	if (millis()-fast_loopTimer > 19) {
+    // We want this to execute at 50Hz, but synchronised with the gyro/accel
+    uint16_t num_samples = ins.num_samples_available();
+    if (num_samples >= 1) {
 		delta_ms_fast_loop	= millis() - fast_loopTimer;
 		load                = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
 		G_Dt                = (float)delta_ms_fast_loop / 1000.f;
@@ -642,7 +642,16 @@ void loop()
 		}
 
 		fast_loopTimeStamp = millis();
-	}
+    } else if (millis() - fast_loopTimeStamp < 19) {
+        // less than 19ms 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
+        // compass, which is often very noisy but is not interrupt
+        // driven, so it can't accumulate readings by itself
+        if (g.compass_enabled) {
+            compass.accumulate();
+        }
+    }
 }
 
 // Main loop 50Hz