From b99b3c7b9d02511cf8168776d08d5db21b092d3e Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 24 Dec 2014 07:39:23 +1100
Subject: [PATCH] DataFlash: Don't log EKF optical flow data if not required

---
 libraries/DataFlash/DataFlash.h |  2 +-
 libraries/DataFlash/LogFile.cpp | 45 ++++++++++++++++++---------------
 2 files changed, 25 insertions(+), 22 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 68fcfe2f34..a439c4b59d 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -69,7 +69,7 @@ public:
     void Log_Write_Power(void);
     void Log_Write_AHRS2(AP_AHRS &ahrs);
 #if AP_AHRS_NAVEKF_AVAILABLE
-    void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
+    void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
 #endif
     void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
     void Log_Write_Radio(const mavlink_radio_t &packet);
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 32af97e9ba..d2c17d8818 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -894,7 +894,7 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
 }
 
 #if AP_AHRS_NAVEKF_AVAILABLE
-void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
+void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
 {
 	// Write first EKF packet
     Vector3f euler;
@@ -1005,27 +1005,30 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
     };
     WriteBlock(&pkt4, sizeof(pkt4));
 
+
     // Write fifth EKF packet
-    float fscale;
-    float gndPos;
-    float flowInnovX, flowInnovY;
-    float augFlowInnovX, augFlowInnovY;
-    float rngInnov;
-    float range;
-    ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
-    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),
-        scaler: (uint8_t)(100*fscale),
-        RI : (int16_t)(100*rngInnov),
-        range : (uint16_t)(100*range)
-     };
-    WriteBlock(&pkt5, sizeof(pkt5));
+    if (optFlowEnabled) {
+        float fscale;
+        float gndPos;
+        float flowInnovX, flowInnovY;
+        float augFlowInnovX, augFlowInnovY;
+        float rngInnov;
+        float range;
+        ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
+        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),
+            scaler: (uint8_t)(100*fscale),
+            RI : (int16_t)(100*rngInnov),
+            range : (uint16_t)(100*range)
+         };
+        WriteBlock(&pkt5, sizeof(pkt5));
+    }
 }
 #endif