From d0831c708d63af9adfd31ee3d039a4d34bd70895 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 31 Jan 2014 09:46:10 +1100
Subject: [PATCH] AP_NavEKF : Added position and height reset public method

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 15 +++++++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 18 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index b88f792594..8bb09fbca7 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -82,6 +82,21 @@ bool NavEKF::healthy(void) const
     return true;
 }
 
+void NavEKF::ResetPosition(void)
+{
+    // read the GPS
+    readGpsData();
+
+    // read the barometer
+    readHgtData();
+
+    // write to state vector
+    states[7] = posNE[0];
+    states[8] = posNE[1];
+    states[9] = - _baro.get_altitude();
+
+}
+
 void NavEKF::InitialiseFilter(void)
 {
     lastFixTime_ms = 0;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 66211cd299..6b438e1544 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -74,6 +74,9 @@ public:
     // Initialise the filter states from the AHRS and magnetometer data (if present)
     void InitialiseFilter(void);
 
+    // Reset the position and height states
+    void ResetPosition(void);
+
     // Update Filter States - this should be called whenever new IMU data is available
     void UpdateFilter(void);