From cd5ad49417fb401be6f4b925e628d718a63c9d95 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 30 Nov 2012 06:39:58 +1100 Subject: [PATCH] Rover: switch to IMU driven timing, same as ArduPlane --- APMrover2/APMrover2.pde | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 2ff060760d..23a979ad52 100644 --- a/APMrover2/APMrover2.pde +++ b/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