From f358d5e20fc2cd7d8c5a331583e1c4bd62d9b1ac Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 16 Oct 2014 05:56:20 +1100
Subject: [PATCH] DataFlash : Add range measurement to EKF debug message

---
 libraries/DataFlash/DataFlash.h | 3 ++-
 libraries/DataFlash/LogFile.cpp | 6 ++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 68c89c0913..39cdef5efd 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -357,6 +357,7 @@ struct PACKED log_EKF5 {
     float gndPos;
     uint8_t scaler;
     uint8_t quality;
+    uint16_t range;
 };
 
 struct PACKED log_Cmd {
@@ -528,7 +529,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","IfffffBB","TimeMS,flowX,flowY,omegaX,omegaY,gndPos,fScaler,flowQual" }
+      "EKF5","IfffffBBc","TimeMS,flowX,flowY,omegaX,omegaY,gndPos,fScaler,flowQual,rng" }
 
 #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 1580262f38..663b4af6d4 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -998,8 +998,9 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
     float gndPos;
     float flowX, flowY;
     float omegaX, omegaY;
+    float range;
     uint8_t flowQual;
-    ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowX, flowY, omegaX, omegaY, flowQual);
+    ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowX, flowY, omegaX, omegaY, flowQual, range);
     struct log_EKF5 pkt5 = {
         LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
         time_ms : hal.scheduler->millis(),
@@ -1009,7 +1010,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
         omegaY : (float)(omegaY),
         gndPos : (float)(gndPos),
         scaler: (uint8_t)(100*fscale),
-        quality : (uint8_t)(flowQual)
+        quality : (uint8_t)(flowQual),
+        range : (uint16_t)(100*range)
      };
     WriteBlock(&pkt5, sizeof(pkt5));
 }