AP_Compass: BMM150: use common method to accumulate samples
This commit is contained in:
parent
565df5ec94
commit
8d54276263
@ -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);
|
||||
}
|
||||
|
||||
|
@ -47,9 +47,6 @@ private:
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
|
||||
Vector3f _mag_accum;
|
||||
uint32_t _accum_count;
|
||||
|
||||
uint8_t _compass_instance;
|
||||
|
||||
struct {
|
||||
|
Loading…
Reference in New Issue
Block a user