From a9eec29e45bc6fa96c34e07b0c8c7ccedef43a61 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 3 Jan 2015 13:50:47 +1100
Subject: [PATCH] DataFlash: Update EKF optical flow data logging

---
 libraries/DataFlash/DataFlash.h | 11 ++++++-----
 libraries/DataFlash/LogFile.cpp | 16 +++++++++-------
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index a439c4b59d..218ac9002d 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -354,12 +354,13 @@ struct PACKED log_EKF5 {
     uint32_t time_ms;
     int16_t FIX;
     int16_t FIY;
-    int16_t AFIX;
-    int16_t AFIY;
-    int16_t gndPos;
+    uint8_t normInnovFX;
+    uint8_t normInnovFY;
+    uint16_t estHAGL;
     uint8_t scaler;
     int16_t RI;
-    uint16_t range;
+    uint16_t meaRng;
+    uint16_t errHAGL;
 };
 
 struct PACKED log_Cmd {
@@ -531,7 +532,7 @@ struct PACKED log_Esc {
     { LOG_ESC8_MSG, sizeof(log_Esc), \
       "ESC8",  "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
     { LOG_EKF5_MSG, sizeof(log_EKF5), \
-      "EKF5","IhhhhcBcC","TimeMS,FIX,FIY,AFIX,AFIY,gndPos,fScaler,RI,rng" }
+      "EKF5","IhhBBCBcCC","TimeMS,FIX,FIY,SFX,SFY,estHAGL,fScaler,RI,meaRng,errHAGL" }
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index d2c17d8818..3c310c5311 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1009,23 +1009,25 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     // Write fifth EKF packet
     if (optFlowEnabled) {
         float fscale;
-        float gndPos;
+        float estHAGL;
         float flowInnovX, flowInnovY;
-        float augFlowInnovX, augFlowInnovY;
+        float flowVarX, flowVarY;
         float rngInnov;
         float range;
-        ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
+        float gndOffsetErr;
+        ahrs.get_NavEKF().getFlowDebug(fscale, estHAGL, flowInnovX, flowInnovY, flowVarX, flowVarY, rngInnov, range, gndOffsetErr);
         struct log_EKF5 pkt5 = {
             LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
             time_ms : hal.scheduler->millis(),
             FIX : (int16_t)(1000*flowInnovX),
             FIY : (int16_t)(1000*flowInnovY),
-            AFIX : (int16_t)(1000*augFlowInnovX),
-            AFIY : (int16_t)(1000*augFlowInnovY),
-            gndPos : (int16_t)(100*gndPos),
+            normInnovFX : min((uint8_t)(100*flowVarX),255),
+            normInnovFY : min((uint8_t)(100*flowVarY),255),
+            estHAGL : (uint16_t)(100*estHAGL),
             scaler: (uint8_t)(100*fscale),
             RI : (int16_t)(100*rngInnov),
-            range : (uint16_t)(100*range)
+            meaRng : (uint16_t)(100*range),
+            errHAGL : (uint16_t)(100*gndOffsetErr)
          };
         WriteBlock(&pkt5, sizeof(pkt5));
     }