From 86ad1e6e66c6a1e3667066345becfd460d4fbbff Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Thu, 24 Sep 2015 11:10:34 +1000
Subject: [PATCH] DataFlash: Update EKF2 data logging

---
 libraries/DataFlash/DataFlash.h | 41 +++++++++++++++++++++++++++++----
 libraries/DataFlash/LogFile.cpp | 20 ++++++++--------
 2 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 704745829e..27e071e9e6 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -382,6 +382,23 @@ struct PACKED log_EKF2 {
     int16_t magZ;
 };
 
+struct PACKED log_NKF2 {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    int8_t AZbias;
+    int16_t scaleX;
+    int16_t scaleY;
+    int16_t scaleZ;
+    int16_t windN;
+    int16_t windE;
+    int16_t magN;
+    int16_t magE;
+    int16_t magD;
+    int16_t magX;
+    int16_t magY;
+    int16_t magZ;
+};
+
 struct PACKED log_EKF3 {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -397,6 +414,22 @@ struct PACKED log_EKF3 {
     int16_t innovVT;
 };
 
+struct PACKED log_NKF3 {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    int16_t innovVN;
+    int16_t innovVE;
+    int16_t innovVD;
+    int16_t innovPN;
+    int16_t innovPE;
+    int16_t innovPD;
+    int16_t innovMX;
+    int16_t innovMY;
+    int16_t innovMZ;
+    int16_t innovYaw;
+    int16_t innovVT;
+};
+
 struct PACKED log_EKF4 {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -786,10 +819,10 @@ Format characters in the format string for binary log messages
       "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
     { LOG_NKF1_MSG, sizeof(log_EKF1), \
       "NKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
-    { LOG_NKF2_MSG, sizeof(log_EKF2), \
-      "NKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
-    { LOG_NKF3_MSG, sizeof(log_EKF3), \
-      "NKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
+    { LOG_NKF2_MSG, sizeof(log_NKF2), \
+      "NKF2","Qcccbcchhhhhh","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","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \
     { LOG_NKF5_MSG, sizeof(log_EKF5), \
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 6df6c90761..eb4d101ad3 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1314,22 +1314,23 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     WriteBlock(&pkt, sizeof(pkt));
 
     // Write second EKF packet
-    float ratio = 0;
-    float az1bias = 0, az2bias = 0;
+    float azbias = 0;
     Vector3f wind;
     Vector3f magNED;
     Vector3f magXYZ;
-    ahrs.get_NavEKF2().getIMU1Weighting(ratio);
-    ahrs.get_NavEKF2().getAccelZBias(az1bias, az2bias);
+    Vector3f gyroScaleFactor;
+    ahrs.get_NavEKF2().getAccelZBias(azbias);
     ahrs.get_NavEKF2().getWind(wind);
     ahrs.get_NavEKF2().getMagNED(magNED);
     ahrs.get_NavEKF2().getMagXYZ(magXYZ);
-    struct log_EKF2 pkt2 = {
+    ahrs.get_NavEKF2().getGyroScaleErrorPercentage(gyroScaleFactor);
+    struct log_NKF2 pkt2 = {
         LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
         time_us : hal.scheduler->micros64(),
-        Ratio   : (int8_t)(100*ratio),
-        AZ1bias : (int8_t)(100*az1bias),
-        AZ2bias : (int8_t)(100*az2bias),
+        AZbias  : (int8_t)(100*azbias),
+        scaleX  : (int16_t)(100*gyroScaleFactor.x),
+        scaleY  : (int16_t)(100*gyroScaleFactor.y),
+        scaleZ  : (int16_t)(100*gyroScaleFactor.z),
         windN   : (int16_t)(100*wind.x),
         windE   : (int16_t)(100*wind.y),
         magN    : (int16_t)(magNED.x),
@@ -1348,7 +1349,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     float tasInnov = 0;
     float yawInnov = 0;
     ahrs.get_NavEKF2().getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
-    struct log_EKF3 pkt3 = {
+    struct log_NKF3 pkt3 = {
         LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
         time_us : hal.scheduler->micros64(),
         innovVN : (int16_t)(100*velInnov.x),
@@ -1360,6 +1361,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
         innovMX : (int16_t)(magInnov.x),
         innovMY : (int16_t)(magInnov.y),
         innovMZ : (int16_t)(magInnov.z),
+        innovYaw : (int16_t)(100*degrees(tasInnov)),
         innovVT : (int16_t)(100*tasInnov)
     };
     WriteBlock(&pkt3, sizeof(pkt3));