AP_NavEKF3: Reset states when falling back into non aiding mode.

4 years ago · ddadc45854
1 changed files with 3 additions and 0 deletions
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@ -217,6 +217,9 @@ void NavEKF3_core::setAidingMode()
				@@ -217,6 +217,9 @@ void NavEKF3_core::setAidingMode()
        PV_AidingMode = AID_NONE;
        yawAlignComplete = false;
        finalInflightYawInit = false;
+        ResetVelocity(resetDataSource::DEFAULT);
+        ResetPosition(resetDataSource::DEFAULT);
+        ResetHeight();
    }

    // Determine if we should change aiding mode