From 329516373ca9abc5e92a03789aa7aae0d9352899 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi 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 #include 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);