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

AP_Compass: MAG3110: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 11:11:03 -07:00
parent ac2771b311
commit e83b345cbb
2 changed files with 2 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_Compass/AP_Compass_MAG3110.cpp
View File

@ -207,32 +207,7 @@ void AP_Compass_MAG3110::_update()
Vector3f raw_field = Vector3f((float)_mag_x, (float)_mag_y, (float)_mag_z) * MAG_SCALE;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (!field_ok(raw_field)) {
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count /= 2;
}
_sem->give();
}
accumulate_sample(raw_field, _compass_instance);
}
@ -243,23 +218,5 @@ void AP_Compass_MAG3110::read()
return;
}
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_sem->give();
return;
}
Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
drain_accumulated_samples(_compass_instance);
}

5
libraries/AP_Compass/AP_Compass_MAG3110.h
View File

@ -41,11 +41,6 @@ private:
int32_t _mag_y;
int32_t _mag_z;
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
uint8_t _accum_count;
uint8_t _compass_instance;
bool _initialised;
};