From 830751c0aede3160845bc20a1fde29287e43be34 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 May 2016 11:06:41 +1000 Subject: [PATCH] AP_NavEKF2: remove un-wanted functionality from mag and yaw reset The function used to reset magnetic field states and yaw angle should not be used when there is no magnetometer. If it is incorrectly called without a magnetometer it should not change the attitude or field states. --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 46a12516e4..e458588288 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -1291,7 +1291,6 @@ void NavEKF2_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const // initialise the earth magnetic field states using declination, suppled roll/pitch // and magnetometer measurements and return initial attitude quaternion -// if no magnetometer data, do not update magnetic field states and assume zero yaw angle Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) { // declare local variables required to calculate initial orientation and magnetic field @@ -1362,8 +1361,8 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) // clear bad magnetic yaw status badMagYaw = false; } else { - initQuat.from_euler(roll, pitch, 0.0f); - yawAlignComplete = false; + // no magnetoemter data so there is nothing we can do + initQuat = stateStruct.quat; } // return attitude quaternion