From a82c8b241fa33b9dfc3f561bcc8184c0ee4943de Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Sat, 10 Oct 2015 06:08:32 +1100
Subject: [PATCH] DataFlash: Update NKF4 data logging

Add roll/pitch error metric
Reduce normalised magnetometer vector with a vector length
---
 libraries/DataFlash/DataFlash.h | 21 +++++++++++++++++++--
 libraries/DataFlash/LogFile.cpp | 10 ++++++----
 2 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index ad033ebadc..9749c8c21c 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -448,6 +448,23 @@ struct PACKED log_EKF4 {
     uint16_t gps;
 };
 
+struct PACKED log_NKF4 {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    int16_t sqrtvarV;
+    int16_t sqrtvarP;
+    int16_t sqrtvarH;
+    int16_t sqrtvarM;
+    int16_t sqrtvarVT;
+    float   tiltErr;
+    int8_t  offsetNorth;
+    int8_t  offsetEast;
+    uint8_t faults;
+    uint8_t timeouts;
+    uint16_t solution;
+    uint16_t gps;
+};
+
 struct PACKED log_EKF5 {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -815,8 +832,8 @@ Format characters in the format string for binary log messages
       "NKF2","Qbccccchhhhhh","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
     { LOG_NKF3_MSG, sizeof(log_NKF3), \
       "NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
-    { LOG_NKF4_MSG, sizeof(log_EKF4), \
-      "NKF4","QcccccccbbBBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS,GPS" }, \
+    { LOG_NKF4_MSG, sizeof(log_NKF4), \
+      "NKF4","QcccccfbbBBHH","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,EFE,FS,TS,SS,GPS" }, \
     { LOG_NKF5_MSG, sizeof(log_EKF5), \
       "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
     { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index f2873d8b62..2e15d5f835 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1380,20 +1380,22 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     nav_filter_status solutionStatus {};
     nav_gps_status gpsStatus {};
     ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
+    float magLength = magVar.length();
     ahrs.get_NavEKF2().getFilterFaults(faultStatus);
     ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus);
     ahrs.get_NavEKF2().getFilterStatus(solutionStatus);
     ahrs.get_NavEKF2().getFilterGpsStatus(gpsStatus);
-    struct log_EKF4 pkt4 = {
+    float tiltError;
+    ahrs.get_NavEKF2().getTiltError(tiltError);
+    struct log_NKF4 pkt4 = {
         LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
         time_us : hal.scheduler->micros64(),
         sqrtvarV : (int16_t)(100*velVar),
         sqrtvarP : (int16_t)(100*posVar),
         sqrtvarH : (int16_t)(100*hgtVar),
-        sqrtvarMX : (int16_t)(100*magVar.x),
-        sqrtvarMY : (int16_t)(100*magVar.y),
-        sqrtvarMZ : (int16_t)(100*magVar.z),
+        sqrtvarM : (int16_t)(100*magLength),
         sqrtvarVT : (int16_t)(100*tasVar),
+        tiltErr : (float)tiltError,
         offsetNorth : (int8_t)(offset.x),
         offsetEast : (int8_t)(offset.y),
         faults : (uint8_t)(faultStatus),