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

---
 libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 41 +--------------------
 libraries/AP_Compass/AP_Compass_LSM9DS1.h   |  4 --
 2 files changed, 2 insertions(+), 43 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
index 431e09d9de..073332f983 100644
--- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
+++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
@@ -155,49 +155,12 @@ void AP_Compass_LSM9DS1::_update(void)
 
     raw_field *= _scaling;
 
-    // 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 (_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 = 5;
-        }
-        _sem->give();
-    }
+    accumulate_sample(raw_field, _compass_instance);
 }
 
 void AP_Compass_LSM9DS1::read()
 {
-    if (!_sem->take_nonblocking()) {
-        return;
-    }
-    if (_accum_count == 0) {
-        /* We're not ready to publish*/
-        _sem->give();
-        return;
-    }
-
-    Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
-    field /= _accum_count;
-    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-    _accum_count = 0;
-
-    _sem->give();
-
-    publish_filtered_field(field, _compass_instance);
+    drain_accumulated_samples(_compass_instance);
 }
 
 bool AP_Compass_LSM9DS1::_check_id(void)
diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h
index f2139eaecd..611051c3cd 100644
--- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h
+++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h
@@ -38,9 +38,5 @@ private:
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
     uint8_t _compass_instance;
     float _scaling;
-    float _mag_x_accum;
-    float _mag_y_accum;
-    float _mag_z_accum;
-    uint32_t _accum_count;
     enum Rotation _rotation;
 };