From 329516373ca9abc5e92a03789aa7aae0d9352899 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.de.marchi@gmail.com>
Date: Fri, 28 Sep 2018 09:11:53 -0700
Subject: [PATCH] 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 | 53 +++++++++++++++++++++
 libraries/AP_Compass/AP_Compass_Backend.h   |  8 ++++
 2 files changed, 61 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index d938ab176b..7d6c8c66f3 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/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
  */
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 24bb60adae..5a20262587 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/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);