mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: publish raw and unfiltered field for AK8963 compass
This commit is contained in:
parent
f085e274c7
commit
8cdc9748b3
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue