From 38d1f8705fd6e7703e2f493a8ab95a0f0ffc7455 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.de.marchi@gmail.com>
Date: Fri, 28 Sep 2018 11:10:01 -0700
Subject: [PATCH] AP_Compass: LIS3MDL: use common method to accumulate samples

---
 libraries/AP_Compass/AP_Compass_LIS3MDL.cpp | 46 +--------------------
 libraries/AP_Compass/AP_Compass_LIS3MDL.h   |  2 -
 2 files changed, 2 insertions(+), 46 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
index f4b2b6704d..8d9e28c40a 100644
--- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
+++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
@@ -157,20 +157,7 @@ void AP_Compass_LIS3MDL::timer()
 
     field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
 
-    /* rotate raw_field from sensor frame to body frame */
-    rotate_field(field, compass_instance);
-
-    /* publish raw_field (uncorrected point sample) for calibration use */
-    publish_raw_field(field, compass_instance);
-
-    /* correct raw_field for known errors */
-    correct_field(field, compass_instance);
-
-    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        accum += field;
-        accum_count++;
-        _sem->give();
-    }
+    accumulate_sample(field, compass_instance);
 
 check_registers:
     dev->check_next_register();
@@ -178,34 +165,5 @@ check_registers:
 
 void AP_Compass_LIS3MDL::read()
 {
-    if (!_sem->take_nonblocking()) {
-        return;
-    }
-    if (accum_count == 0) {
-        _sem->give();
-        return;
-    }
-
-#if 0
-    // debugging code for sample rate
-    static uint32_t lastt;
-    static uint32_t total;
-    total += accum_count;
-    uint32_t now = AP_HAL::micros();
-    float dt = (now - lastt) * 1.0e-6;
-    if (dt > 1) {
-        printf("%u samples\n", total);
-        lastt = now;
-        total = 0;
-    }
-#endif
-    
-    accum /= accum_count;
-
-    publish_filtered_field(accum, compass_instance);
-
-    accum.zero();
-    accum_count = 0;
-    
-    _sem->give();
+    drain_accumulated_samples(compass_instance);
 }
diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.h b/libraries/AP_Compass/AP_Compass_LIS3MDL.h
index f74a8bcb23..51406d154b 100644
--- a/libraries/AP_Compass/AP_Compass_LIS3MDL.h
+++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.h
@@ -55,8 +55,6 @@ private:
     void timer();
 
     uint8_t compass_instance;
-    Vector3f accum;
-    uint16_t accum_count;
     bool force_external;
     enum Rotation rotation;
 };