From 6b9733c0138e8180f02e79733d3b0cd3d3e65040 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 31 Jan 2014 09:11:54 +1100
Subject: [PATCH] AP_NavEKF : Additional flash logging

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 11 +++++++++--
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 5ef2917151..5cadaf113d 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -6,10 +6,10 @@
 
 // uncomment this to force the optimisation of this code, note that 
 // this makes debugging harder
-// #pragma GCC optimize("O3")
+#pragma GCC optimize("O3")
 
 #include "AP_NavEKF.h"
-#include <stdio.h>
+//#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -1923,6 +1923,13 @@ void NavEKF::getAccelBias(Vector3f &accelBias)
     accelBias.z = states[15]*dtIMUAvgInv;
 }
 
+void NavEKF::getWind(Vector3f &wind)
+{
+    wind.x = states[16];
+    wind.y = states[17];
+    wind.z = 0.0f; // curently don't estimate this
+}
+
 void NavEKF::getMagNED(Vector3f &magNED)
 {
     magNED.x = states[18]*1000.0f;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 0a61b6b247..edd6b388be 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -92,6 +92,9 @@ public:
     // return delta velocity bias estimates
     void getAccelBias(Vector3f &accelBias);
 
+    // return the NE wind speed estimates
+    void getWind(Vector3f &wind);
+
     // return earth magnetic field estimates
     void getMagNED(Vector3f &magNED);