From b39a5062e992f2e331f74140d67fbd11000ac38c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 Jan 2014 14:25:41 +1100
Subject: [PATCH] AP_AHRS: use NavEKF for ground vector when available

---
 libraries/AP_AHRS/AP_AHRS.h          |  2 +-
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  3 +++
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index db7b19c22d..6689297f97 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -210,7 +210,7 @@ public:
     }
 
     // return a ground vector estimate in meters/second, in North/East order
-    Vector2f groundspeed_vector(void);
+    virtual Vector2f groundspeed_vector(void);
 
     // return ground speed estimate in meters/second. Used by ground vehicles.
     float groundspeed(void) const {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 115105841a..a00d00d72a 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -178,4 +178,15 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
     return false;
 }
 
+// EKF has a better ground speed vector estimate
+Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
+{
+    if (!ekf_started || !_ekf_use) {
+        return AP_AHRS_DCM::groundspeed_vector();
+    }
+    Vector3f vec;
+    EKF.getVelNED(vec);
+    return Vector2f(vec.x, vec.y);
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index e673d1799b..9de682db5a 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -79,6 +79,9 @@ public:
     // return secondary position solution if available
     bool get_secondary_position(struct Location &loc);
 
+    // EKF has a better ground speed vector estimate
+    Vector2f groundspeed_vector(void);
+
 private:
     NavEKF EKF;
     AP_Baro &_baro;