AP_NavEKF : Added position and height reset public method

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;

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);