diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 40d204eef7..693ff374c1 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -454,6 +454,10 @@ private: // board specific orientation enum Rotation rotation; + + // accumulated samples, protected by _sem, used by AP_Compass_Backend + Vector3f accum; + uint32_t accum_count; } _state[COMPASS_MAX_INSTANCES]; AP_Int16 _offset_max; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 37226e330a..94d71f2252 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -114,11 +114,12 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance, WITH_SEMAPHORE(_sem); - _accum += field; - _accum_count++; - if (max_samples && _accum_count >= max_samples) { - _accum_count /= 2; - _accum /= 2; + Compass::mag_state &state = _compass._state[instance]; + state.accum += field; + state.accum_count++; + if (max_samples && state.accum_count >= max_samples) { + state.accum_count /= 2; + state.accum /= 2; } } @@ -127,19 +128,21 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, { WITH_SEMAPHORE(_sem); - if (_accum_count == 0) { + Compass::mag_state &state = _compass._state[instance]; + + if (state.accum_count == 0) { return; } if (scaling) { - _accum *= *scaling; + state.accum *= *scaling; } - _accum /= _accum_count; + state.accum /= state.accum_count; - publish_filtered_field(_accum, instance); + publish_filtered_field(state.accum, instance); - _accum.zero(); - _accum_count = 0; + state.accum.zero(); + state.accum_count = 0; } /* diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 4685c245ee..2cb343bdd7 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -112,10 +112,6 @@ protected: // semaphore for access to shared frontend data HAL_Semaphore_Recursive _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);