AP_NavEKF : Additional flash logging

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;

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);