aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: BMM150: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 11:09:12 -07:00
parent 565df5ec94
commit 8d54276263
2 changed files with 2 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -314,47 +314,14 @@ void AP_Compass_BMM150::_update()
/* convert uT to milligauss */
raw_field *= 10;
/* 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);
_last_read_ms = AP_HAL::millis();
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_accum += raw_field;
_accum_count++;
if (_accum_count == 10) {
_mag_accum /= 2;
_accum_count = 5;
}
_sem->give();
accumulate_sample(raw_field, _compass_instance);
_dev->check_next_register();
}
void AP_Compass_BMM150::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(_mag_accum);
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
drain_accumulated_samples(_compass_instance);
}

3
libraries/AP_Compass/AP_Compass_BMM150.h
View File

@ -47,9 +47,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
Vector3f _mag_accum;
uint32_t _accum_count;
uint8_t _compass_instance;
struct {