From 5d43a1d704d55f9befb034c801a41393f5081895 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 4 Jan 2014 11:15:34 +1100
Subject: [PATCH] AP_AHRS: added inertial nav interfaces to AHRS

---
 libraries/AP_AHRS/AP_AHRS.h          | 14 +++++++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    | 30 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h      |  3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 26 ++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  7 ++++++-
 5 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 0d09bf79a1..ed17af048a 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -201,6 +201,16 @@ public:
     // return a ground vector estimate in meters/second, in North/East order
     virtual Vector2f groundspeed_vector(void);
 
+    // return a ground velocity in meters/second, North/East/Down
+    // order. This will only be accurate if have_inertial_nav() is
+    // true 
+    virtual Vector3f get_velocity_NED(void) = 0;
+
+    // return a position relative to home in meters, North/East/Down
+    // order. This will only be accurate if have_inertial_nav() is
+    // true 
+    virtual Vector3f get_relative_position_NED(void) = 0;
+
     // return ground speed estimate in meters/second. Used by ground vehicles.
     float groundspeed(void) const {
         if (!_gps || _gps->status() <= GPS::NO_FIX) {
@@ -268,6 +278,10 @@ public:
     // current barometer and GPS altitudes correspond to this altitude
     virtual void set_home(int32_t lat, int32_t lon, int32_t alt_cm) = 0;
 
+    // return true if the AHRS object supports inertial navigation,
+    // with very accurate position and velocity
+    virtual bool have_inertial_nav(void) const { return false; }
+
 protected:
     // settable parameters
     AP_Float beta;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index c4f29bd0a5..bdecbbe9dc 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -895,3 +895,33 @@ void AP_AHRS_DCM::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
     _home.lng = lng;
     _home.alt = alt_cm;
 }
+
+// return a ground velocity in meters/second, North/East/Down order
+Vector3f AP_AHRS_DCM::get_velocity_NED(void)
+{
+    Vector2f xy = groundspeed_vector();
+    Vector3f vec(xy.x, xy.y, 0);
+
+    // if we have GPS lock and GPS has raw velocities then use GPS
+    // vertical velocity for z
+    if (_gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
+        vec.z = _gps->velocity_down();
+    } else {
+        // otherwise we have no choice but the baro climb rate
+        vec.z = - _baro.get_climb_rate();
+    }
+    return vec;
+}
+
+// return a relative position in meters, NED order This is only
+// approximate. Use the NavEKF version for accurate positioning
+Vector3f AP_AHRS_DCM::get_relative_position_NED(void)
+{
+    struct Location loc;
+    if (!get_position(loc)) {
+        // not available
+        return Vector3f(0,0,0);
+    }
+    Vector2f xy = location_diff(loc, _home);
+    return Vector3f(xy.x, xy.y, (_home.alt - loc.alt) * 1.0e-2f);
+}
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index b2476f2fb0..b341e35a0f 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -80,6 +80,9 @@ public:
 
     void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
 
+    Vector3f get_velocity_NED(void);
+    Vector3f get_relative_position_NED(void);
+
 private:
     float _ki;
     float _ki_yaw;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 56b1acba86..364df549cb 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -195,4 +195,30 @@ void AP_AHRS_NavEKF::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
     }
 }
 
+// return true if inertial navigation is active
+bool AP_AHRS_NavEKF::have_inertial_nav(void) const 
+{
+    return using_EKF();
+}
+
+// return a ground velocity in meters/second, North/East/Down order
+Vector3f AP_AHRS_NavEKF::get_velocity_NED(void)
+{
+    if (using_EKF()) {
+        Vector3f vec;
+        EKF.getVelNED(vec);
+        return vec;
+    }
+    return AP_AHRS_DCM::get_velocity_NED();
+}
+
+Vector3f AP_AHRS_NavEKF::get_relative_position_NED(void)
+{
+    Vector3f ret;
+    if (using_EKF() && EKF.getPosNED(ret)) {
+        return ret;
+    }
+    return AP_AHRS_DCM::get_relative_position_NED();
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 9367723685..c4f42fc431 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -85,8 +85,13 @@ public:
     // set home location
     void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
 
+    bool have_inertial_nav(void) const;
+
+    Vector3f get_velocity_NED(void);
+    Vector3f get_relative_position_NED(void);
+
 private:
-    bool using_EKF(void) { return ekf_started && _ekf_use && EKF.healthy(); }
+    bool using_EKF(void) const { return ekf_started && _ekf_use && EKF.healthy(); }
 
     NavEKF EKF;
     AP_Baro &_baro;