From 163cfd839d5eb1a77da99e0aa98519a0e3ef7838 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 30 Dec 2013 16:19:50 +1100
Subject: [PATCH] AP_NavEKF: enable airspeed and fix getLLH()

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 17 ++++++++---------
 libraries/AP_NavEKF/AP_NavEKF.h   |  1 -
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 8aaf179acc..671309f522 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define MATH_CHECK_INDEXES 1
+// #define MATH_CHECK_INDEXES 1
 
 #include <AP_HAL.h>
 #include "AP_NavEKF.h"
@@ -20,7 +20,7 @@ extern const AP_HAL::HAL& hal;
 NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
     _ahrs(ahrs),
     _baro(baro),
-    useAirspeed(false),
+    useAirspeed(true),
     useCompass(true),
     fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
@@ -172,7 +172,7 @@ void NavEKF::UpdateFilter()
         // Update states using GPS, altimeter, compass and airspeed observations
         SelectVelPosFusion();
         SelectMagFusion();
-        //SelectTasFusion();
+        SelectTasFusion();
     }
 }
 
@@ -1887,14 +1887,13 @@ void NavEKF::calcposNE(float lat, float lon)
     posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef);
 }
 
-#if 0
-void NavEKF::calcLLH(float &lat, float &lon, float &hgt)
+bool NavEKF::getLLH(struct Location &loc)
 {
-    lat = latRef + posNED[0] / RADIUS_OF_EARTH;
-    lon = lonRef + (posNED[1] / RADIUS_OF_EARTH) / cosf(latRef);
-    hgt = hgtRef - posNED[2];
+    loc.lat = 1.0e7 * degrees(latRef + states[7] / RADIUS_OF_EARTH);
+    loc.lng = 1.0e7 * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
+    loc.alt = 1.0e2 * (hgtRef - states[9]);
+    return true;
 }
-#endif
 
 void NavEKF::OnGroundCheck()
 {
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 77abd6a86c..9c3fae2555 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -171,7 +171,6 @@ private:
     Vector3f velNED; // North, East, Down velocity measurements (m/s)
     Vector2 posNE; // North, East position measurements (m)
     float hgtMea; //  height measurement relative to reference point  (m)
-    Vector3f posNED; // North, East Down position relative to reference point (m)
     Vector24 statesAtVelTime; // States at the effective time of velNED measurements
     Vector24 statesAtPosTime; // States at the effective time of posNE measurements
     Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement