DataFlash : EKF optical flow fusion support preliminary changes

libraries/DataFlash/DataFlash.h

@@ -347,6 +347,18 @@ struct PACKED log_EKF4 {
     uint8_t staticmode;
 };
 
+struct PACKED log_EKF5 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float obsX;
+    float obsY;
+    float innovX;
+    float innovY;
+    float gndPos;
+    uint8_t scaler;
+    uint8_t quality;
+};
+
 struct PACKED log_Cmd {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
@@ -514,7 +526,9 @@ struct PACKED log_Esc {
     { LOG_ESC7_MSG, sizeof(log_Esc), \
       "ESC7",  "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
     { LOG_ESC8_MSG, sizeof(log_Esc), \
-      "ESC8",  "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }
+      "ESC8",  "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
+    { LOG_EKF5_MSG, sizeof(log_EKF5), \
+      "EKF5","IfffffBB","TimeMS,FX,FY,IFX,IFY,GP,SF,Q" }
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@@ -559,6 +573,7 @@ struct PACKED log_Esc {
 #define LOG_ESC6_MSG      159
 #define LOG_ESC7_MSG      160
 #define LOG_ESC8_MSG      161
+#define LOG_EKF5_MSG      162
 
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use

libraries/DataFlash/LogFile.cpp

@@ -992,6 +992,26 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
         staticmode : (uint8_t)(ahrs.get_NavEKF().getStaticMode())
     };
     WriteBlock(&pkt4, sizeof(pkt4));
+
+    // Write fifth EKF packet
+    float fscale;
+    float gndPos;
+    float obsX, obsY;
+    float innovX, innovY;
+    uint8_t flowQual;
+    ahrs.get_NavEKF().getFlowDebug(fscale, obsX, obsY, innovX, innovY, gndPos, flowQual);
+    struct log_EKF5 pkt5 = {
+        LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
+        time_ms : hal.scheduler->millis(),
+        obsX : (float)(obsX),
+        obsY : (float)(obsY),
+        innovX : (float)(innovX),
+        innovY : (float)(innovY),
+        gndPos : (float)(gndPos),
+        scaler: (uint8_t)(100*fscale),
+        quality : (uint8_t)(flowQual)
+     };
+    WriteBlock(&pkt5, sizeof(pkt5));
 }
 #endif