Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: publish raw and unfiltered field for AK8963 compass

This commit is contained in:
Siddharth Bharat Purohit 2015-07-30 21:55:16 -07:00 committed by Andrew Tridgell
parent f085e274c7
commit 8cdc9748b3
1 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -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)