Copter: save EKF learned compass to primary compass

This commit is contained in:
Randy Mackay 2015-04-10 09:33:05 +09:00
parent 2406e26ab4
commit fea7632eac
1 changed files with 2 additions and 2 deletions

View File

@ -657,8 +657,8 @@ static void init_disarm_motors()
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(0,magOffsets);
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(compass.get_primary(), magOffsets);
}
#if AUTOTUNE_ENABLED == ENABLED