AP_Compass: add common method to accumulate and drain samples

Instead of repeating on each driver the same sequence of steps,
let's move them to the common parent class. This only implements
it, but no driver was ported yet.

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -2,6 +2,8 @@
 
 #include "AP_Compass.h"
 #include "AP_Compass_Backend.h"
+
+#include <AP_Common/Semaphore.h>
 #include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
@@ -95,6 +97,57 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
     mag += state.motor_offset;
 }
 
+void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
+                                           uint32_t max_samples)
+{
+    /* rotate raw_field from sensor frame to body frame */
+    rotate_field(field, instance);
+
+    /* publish raw_field (uncorrected point sample) for calibration use */
+    publish_raw_field(field, instance);
+
+    /* correct raw_field for known errors */
+    correct_field(field, instance);
+
+    if (!field_ok(field)) {
+        return;
+    }
+
+    WITH_SEMAPHORE(_sem);
+
+    _accum += field;
+    _accum_count++;
+    if (max_samples && _accum_count >= max_samples) {
+        _accum_count /= 2;
+        _accum /= 2;
+    }
+}
+
+void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
+                                                   const Vector3f *scaling)
+{
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
+
+    if (_accum_count == 0) {
+        _sem->give();
+        return;
+    }
+
+    if (scaling) {
+        _accum *= *scaling;
+    }
+    _accum /= _accum_count;
+
+    publish_filtered_field(_accum, instance);
+
+    _accum.zero();
+    _accum_count = 0;
+
+    _sem->give();
+}
+
 /*
   copy latest data to the frontend from a backend
  */

libraries/AP_Compass/AP_Compass_Backend.h

@@ -81,6 +81,10 @@ protected:
     void publish_filtered_field(const Vector3f &mag, uint8_t instance);
     void set_last_update_usec(uint32_t last_update, uint8_t instance);
 
+    void accumulate_sample(Vector3f &field, uint8_t instance,
+                           uint32_t max_samples = 10);
+    void drain_accumulated_samples(uint8_t instance, const Vector3f *scale = NULL);
+
     // register a new compass instance with the frontend
     uint8_t register_compass(void) const;
 
@@ -108,6 +112,10 @@ protected:
     // semaphore for access to shared frontend data
     AP_HAL::Semaphore *_sem;
 
+    // accumulated samples, protected by _sem
+    Vector3f    _accum;
+    uint32_t    _accum_count;
+
     // Check that the compass field is valid by using a mean filter on the vector length
     bool field_ok(const Vector3f &field);