From 68705fe7e6f3cab4cabc9e08126a2d10115c2508 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 26 Aug 2012 13:44:53 +1000 Subject: [PATCH] APM: accumulate mag readings during spare cycles in ArduPlane With this change we average over 100 mag readings per compass.read() call, which means we are reading the compass at over 1kHz instead of 10Hz. The noise reduction is huge. --- ArduPlane/ArduPlane.pde | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 76adb28372..204235bca4 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -747,6 +747,15 @@ void loop() } fast_loopTimeStamp_ms = millis(); + } else { + // 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 + // 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(); + } } }