AP_Compass: LSM9DS1: use common method to accumulate samples

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)

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;
 };