// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
memset(&storedMag[0], 0, sizeof(storedMag));
}
}
}
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
StoreMag();
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations
if (_ahrs->get_compass()->healthy(0)) {
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && firstMagYawInit) {
stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
}
lastMagOffsets = nowMagOffsets;
}
// save magnetometer measurement to buffer to be fused later
StoreMag();
}
}
