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

---
 libraries/AP_Compass/AP_Compass_MAG3110.cpp | 47 +--------------------
 libraries/AP_Compass/AP_Compass_MAG3110.h   |  5 ---
 2 files changed, 2 insertions(+), 50 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_MAG3110.cpp b/libraries/AP_Compass/AP_Compass_MAG3110.cpp
index 455c712bc0..780de585ff 100644
--- a/libraries/AP_Compass/AP_Compass_MAG3110.cpp
+++ b/libraries/AP_Compass/AP_Compass_MAG3110.cpp
@@ -207,32 +207,7 @@ void AP_Compass_MAG3110::_update()
 
     Vector3f raw_field = Vector3f((float)_mag_x, (float)_mag_y, (float)_mag_z) * MAG_SCALE;
 
-    // rotate raw_field from sensor frame to body frame
-    rotate_field(raw_field, _compass_instance);
-    
-    // publish raw_field (uncorrected point sample) for calibration use
-    publish_raw_field(raw_field, _compass_instance);
-    
-    // correct raw_field for known errors
-    correct_field(raw_field, _compass_instance);
-
-    if (!field_ok(raw_field)) {
-        return;
-    }
-    
-    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mag_x_accum += raw_field.x;
-        _mag_y_accum += raw_field.y;
-        _mag_z_accum += raw_field.z;
-        _accum_count++;
-        if (_accum_count == 10) {
-            _mag_x_accum /= 2;
-            _mag_y_accum /= 2;
-            _mag_z_accum /= 2;
-            _accum_count /= 2;
-        }
-        _sem->give();
-    }
+    accumulate_sample(raw_field, _compass_instance);
 }
 
 
@@ -243,23 +218,5 @@ void AP_Compass_MAG3110::read()
         return;
     }
 
-    if (!_sem->take_nonblocking()) {
-        return;
-    }
-    
-    if (_accum_count == 0) {
-        /* We're not ready to publish*/
-        _sem->give();
-        return;
-    }
-
-    Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum);
-    field /= _accum_count;
-
-    _accum_count = 0;
-    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-
-    _sem->give();    
-
-    publish_filtered_field(field, _compass_instance);
+    drain_accumulated_samples(_compass_instance);
 }
diff --git a/libraries/AP_Compass/AP_Compass_MAG3110.h b/libraries/AP_Compass/AP_Compass_MAG3110.h
index dba9f5ff96..b1a5feeaa7 100644
--- a/libraries/AP_Compass/AP_Compass_MAG3110.h
+++ b/libraries/AP_Compass/AP_Compass_MAG3110.h
@@ -41,11 +41,6 @@ private:
     int32_t _mag_y;
     int32_t _mag_z;
 
-    float _mag_x_accum;
-    float _mag_y_accum;
-    float _mag_z_accum;
-    uint8_t _accum_count;
-
     uint8_t _compass_instance;
     bool _initialised;
 };