diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index 431e09d9de..073332f983 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -155,49 +155,12 @@ void AP_Compass_LSM9DS1::_update(void) raw_field *= _scaling; - // 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 (_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 = 5; - } - _sem->give(); - } + accumulate_sample(raw_field, _compass_instance); } void AP_Compass_LSM9DS1::read() { - if (!_sem->take_nonblocking()) { - return; - } - if (_accum_count == 0) { - /* We're not ready to publish*/ - _sem->give(); - return; - } - - Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum); - field /= _accum_count; - _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; - _accum_count = 0; - - _sem->give(); - - publish_filtered_field(field, _compass_instance); + drain_accumulated_samples(_compass_instance); } bool AP_Compass_LSM9DS1::_check_id(void) diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index f2139eaecd..611051c3cd 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h @@ -38,9 +38,5 @@ private: AP_HAL::OwnPtr _dev; uint8_t _compass_instance; float _scaling; - float _mag_x_accum; - float _mag_y_accum; - float _mag_z_accum; - uint32_t _accum_count; enum Rotation _rotation; };