From 841d2700b67a2325fb3ef96c66f08b04fc088059 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Sep 2012 13:07:16 +1000 Subject: [PATCH] ACM: use spare CPU cycles to read the magnetometer --- ArduCopter/ArduCopter.pde | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9d21a34659..cc3dfb9d74 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -982,10 +982,12 @@ void loop() { uint32_t timer = micros(); static bool run_50hz_loop = false; + uint16_t num_samples; // We want this to execute fast // ---------------------------- - if( imu.num_samples_available() >= NUM_IMU_SAMPLES_FOR_100HZ ) { + num_samples = imu.num_samples_available(); + if (num_samples >= NUM_IMU_SAMPLES_FOR_100HZ) { #if DEBUG_FAST_LOOP == ENABLED Log_Write_Data(50, (int32_t)(timer - fast_loopTimer)); @@ -1055,6 +1057,17 @@ void loop() #ifdef DESKTOP_BUILD usleep(1000); #endif + if (num_samples < NUM_IMU_SAMPLES_FOR_100HZ-1) { + // we have some spare cycles available + // 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(); + } + } } // port manipulation for external timing of main loops