From ddadc458548ed259801c76d1d18f3e157210bdc9 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sat, 12 Dec 2020 10:52:01 +1100
Subject: [PATCH] AP_NavEKF3: Reset states when falling back into non aiding
 mode.

---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index f8421c6dc3..578751195c 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -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