From f01cc78d374800ca5d4ec831052a8b51e1c1f74b Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 14 May 2014 18:18:28 +1000
Subject: [PATCH] DataFlash: Add EKF fault status logging

---
 libraries/DataFlash/DataFlash.h | 4 +++-
 libraries/DataFlash/LogFile.cpp | 7 ++++++-
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 5bab018a9..c31865f97 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -323,6 +323,8 @@ struct PACKED log_EKF4 {
     int16_t sqrtvarVT;
     int8_t  offsetNorth;
     int8_t  offsetEast;
+    uint8_t faults;
+    uint8_t divergeRate;
 };
 
 struct PACKED log_Cmd {
@@ -386,7 +388,7 @@ struct PACKED log_Radio {
     { LOG_EKF3_MSG, sizeof(log_EKF3), \
       "EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
     { LOG_EKF4_MSG, sizeof(log_EKF4), \
-      "EKF4","Icccccccbb","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE" }, \
+      "EKF4","IcccccccbbBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,DS" }, \
     { LOG_CMD_MSG, sizeof(log_Cmd), \
       "CMD", "IHHHfffffff","TimeMS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \
     { LOG_RADIO_MSG, sizeof(log_Radio), \
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index f0c9004c8..2c5d20719 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -945,7 +945,10 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
 	Vector3f magVar;
 	float tasVar;
     Vector2f offset;
+    uint8_t faultStatus;
+    float deltaGyroBias;
     ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
+    ahrs.get_NavEKF().getFilterFaults(faultStatus, deltaGyroBias);
     struct log_EKF4 pkt4 = {
         LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
         time_ms : hal.scheduler->millis(),
@@ -957,7 +960,9 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
         sqrtvarMZ : (int16_t)(100*magVar.z),
         sqrtvarVT : (int16_t)(100*tasVar),
         offsetNorth : (int8_t)(offset.x),
-        offsetEast : (int8_t)(offset.y)
+        offsetEast : (int8_t)(offset.y),
+        faults : (uint8_t)(faultStatus),
+        divergeRate : (uint8_t)(100*deltaGyroBias)
     };
     WriteBlock(&pkt4, sizeof(pkt4));
 }
-- 
GitLab