From 24c0309e853a233a637c103074dcf2ce5eec5582 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Mon, 12 Oct 2015 23:13:32 +1100
Subject: [PATCH] AP_InertialNav: Enable flight using EKF2

All EKF functions are accessed via the AHRS library enabling AHRS_EKF_TYPE to determine which EKF is being used by the control loops
---
 .../AP_InertialNav/AP_InertialNav_NavEKF.cpp  | 28 +++++++++----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
index 3746ede709..d57c0f51b6 100644
--- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
@@ -16,17 +16,20 @@
 */
 void AP_InertialNav_NavEKF::update(float dt)
 {
-    _ahrs_ekf.get_NavEKF().getPosNED(_relpos_cm);
+    // get the position relative to the local earth frame origin
+    _ahrs_ekf.get_relative_position_NED(_relpos_cm);
     _relpos_cm *= 100; // convert to cm
 
+    // get the absolute WGS-84 position
     _haveabspos = _ahrs_ekf.get_position(_abspos);
 
-    _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm);
+    // get the velocity relative to the local earth frame
+    _ahrs_ekf.get_velocity_NED(_velocity_cm);
     _velocity_cm *= 100; // convert to cm/s
 
-    // A derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
+    // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
     // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
-    _ahrs_ekf.get_NavEKF().getPosDownDerivative(_pos_z_rate);
+    _ahrs_ekf.get_vert_pos_rate(_pos_z_rate);
     _pos_z_rate *= 100; // convert to cm/s
 
     // InertialNav is NEU
@@ -40,9 +43,9 @@ void AP_InertialNav_NavEKF::update(float dt)
  */
 nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
 {
-    nav_filter_status ret;
-    _ahrs_ekf.get_NavEKF().getFilterStatus(ret);
-    return ret;
+    nav_filter_status status;
+    _ahrs_ekf.get_filter_status(status);
+    return status;
 }
 
 /**
@@ -51,10 +54,7 @@ nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
 struct Location AP_InertialNav_NavEKF::get_origin() const
 {
     struct Location ret;
-    if (!_ahrs_ekf.get_NavEKF().getOriginLLH(ret)) {
-        // initialise location to all zeros if origin not yet set
-        memset(&ret, 0, sizeof(ret));
-    }
+    ret = _ahrs_ekf.get_origin();
     return ret;
 }
 
@@ -74,7 +74,7 @@ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
  */
 bool AP_InertialNav_NavEKF::get_location(struct Location &loc) const
 {
-    return _ahrs_ekf.get_NavEKF().getLLH(loc);
+    return _ahrs_ekf.get_location(loc);
 }
 
 /**
@@ -142,7 +142,7 @@ float AP_InertialNav_NavEKF::get_altitude() const
 bool AP_InertialNav_NavEKF::get_hagl(float &height) const
 {
     // true when estimate is valid
-    bool valid = _ahrs_ekf.get_NavEKF().getHAGL(height);
+    bool valid = _ahrs_ekf.get_hagl(height);
     // convert height from m to cm
     height *= 100.0f;
     return valid;
@@ -157,7 +157,7 @@ bool AP_InertialNav_NavEKF::get_hagl(float &height) const
 bool AP_InertialNav_NavEKF::get_hgt_ctrl_limit(float& limit) const
 {
     // true when estimate is valid
-    if (_ahrs_ekf.get_NavEKF().getHeightControlLimit(limit)) {
+    if (_ahrs_ekf.get_hgt_ctrl_limit(limit)) {
         // convert height from m to cm
         limit *= 100.0f;
         return true;