Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: move accum fields to frontend

Browse Source 
this is needed to ensure they are per-instance. Without this the SITL
backend produces zero for all but the first instance. The same would
be true for any multi-instance backend
This commit is contained in:
Andrew Tridgell 2018-10-16 19:59:51 +11:00
parent 83065d5e88
commit 925555c995
3 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.h
View File

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

25
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -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;
}
/*

4
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -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);