From 8cdc9748b39fd082b120bd9c49c66989196dd6a2 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Thu, 30 Jul 2015 21:55:16 -0700 Subject: [PATCH] AP_Compass: publish raw and unfiltered field for AK8963 compass --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 27 +++++++++++++++------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 2886402a82..c38260727e 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -211,8 +211,7 @@ void AP_Compass_AK8963::read() #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP field.rotate(ROTATION_YAW_90); #endif - - publish_field(field, _compass_instance); + publish_filtered_field(field, _compass_instance); } Vector3f AP_Compass_AK8963::_get_filtered_field() const @@ -264,10 +263,25 @@ void AP_Compass_AK8963::_update() if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) { goto fail; } + uint32_t time_us = hal.scheduler->micros(); + // get raw_field - sensor frame, uncorrected + Vector3f raw_field = Vector3f(mag_x, mag_y, mag_z); - _mag_x_accum += mag_x; - _mag_y_accum += mag_y; - _mag_z_accum += mag_z; + // 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, time_us, _compass_instance); + + // correct raw_field for known errors + correct_field(raw_field, _compass_instance); + + // publish raw_field (corrected point sample) for EKF use + publish_unfiltered_field(raw_field, time_us, _compass_instance); + + _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; @@ -484,9 +498,6 @@ AP_AK8963_SerialBus_I2C::AP_AK8963_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t void AP_AK8963_SerialBus_I2C::register_write(uint8_t address, uint8_t value) { _i2c->writeRegister(_addr, address, value); - rotate_field(field, _compass_instance); - correct_field(field, _compass_instance); - publish_filtered_field(field, _compass_instance); } void AP_AK8963_SerialBus_I2C::register_read(uint8_t address, uint8_t *value, uint8_t count)