DataFlash : Added logging of NavEKF innovations and variances

libraries/DataFlash/DataFlash.h

@@ -215,6 +215,7 @@ struct PACKED log_BARO {
     float   altitude;
     float   pressure;
     int16_t temperature;
+};
 
 struct PACKED log_AHRS {
     LOG_PACKET_HEADER;
@@ -247,9 +248,9 @@ struct PACKED log_EKF1 {
     float posN;
     float posE;
     float posD;
-    int8_t gyrX;
-    int8_t gyrY;
-    int8_t gyrZ;
+    int16_t gyrX;
+    int16_t gyrY;
+    int16_t gyrZ;
 };
 
 struct PACKED log_EKF2 {
@@ -268,7 +269,35 @@ struct PACKED log_EKF2 {
     int16_t magZ;
 };
 
+struct PACKED log_EKF3 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    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 innovVT;
+};
 
+struct PACKED log_EKF4 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t sqrtvarVN;
+    int16_t sqrtvarVE;
+	int16_t sqrtvarVD;
+    int16_t sqrtvarPN;
+    int16_t sqrtvarPE;
+    int16_t sqrtvarPD;
+    int16_t sqrtvarMX;
+    int16_t sqrtvarMY;
+    int16_t sqrtvarMZ;
+    int16_t sqrtvarVT;
+};
 
 #define LOG_COMMON_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
@@ -290,15 +319,19 @@ struct PACKED log_EKF2 {
     { LOG_BARO_MSG, sizeof(log_BARO), \
       "BARO",  "Iffc",     "TimeMS,Alt,Press,Temp" }, \
     { LOG_POWR_MSG, sizeof(log_POWR), \
-      "POWR","ICCH","TimeMS,Vcc,VServo,Flags" } \
+      "POWR","ICCH","TimeMS,Vcc,VServo,Flags" },  \
     { LOG_AHR2_MSG, sizeof(log_AHRS), \
       "AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
     { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
       "SIM","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
     { LOG_EKF1_MSG, sizeof(log_EKF1), \
-      "EKF1","IccCffffffbbb","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
+      "EKF1","IccCffffffccc","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
     { LOG_EKF2_MSG, sizeof(log_EKF2), \
-      "EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }
+      "EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
+    { 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","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }
 
 // message types for common messages
 #define LOG_FORMAT_MSG	  128
@@ -315,6 +348,8 @@ struct PACKED log_EKF2 {
 #define LOG_SIMSTATE_MSG  139
 #define LOG_EKF1_MSG      140
 #define LOG_EKF2_MSG      141
+#define LOG_EKF3_MSG      142
+#define LOG_EKF4_MSG      143
 
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"

libraries/DataFlash/LogFile.cpp

@@ -803,7 +803,7 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
         return;
     }
     struct log_AHRS pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_AHRS2_MSG),
+        LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
         time_ms : hal.scheduler->millis(),
         roll  : (int16_t)(degrees(euler.x)*100),
         pitch : (int16_t)(degrees(euler.y)*100),
@@ -816,9 +816,9 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
 }
 
 #if AP_AHRS_NAVEKF_AVAILABLE
-// Write first EKF packet
 void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
 {
+	// Write first EKF packet
     Vector3f euler;
     Vector3f posNED;
     Vector3f velNED;
@@ -841,12 +841,13 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
         posN    : (float)(posNED.x), // metres North
         posE    : (float)(posNED.y), // metres East
         posD    : (float)(posNED.z), // metres Down
-        gyrX    : (int8_t)(gyroBias.x), // deg/min
-        gyrY    : (int8_t)(gyroBias.y), // deg/min
-        gyrZ    : (int8_t)(gyroBias.z)  // deg/min
+        gyrX    : (int8_t)(60*degrees(gyroBias.x)), // deg/min
+        gyrY    : (int8_t)(60*degrees(gyroBias.y)), // deg/min
+        gyrZ    : (int8_t)(60*degrees(gyroBias.z))  // deg/min
     };
     WriteBlock(&pkt, sizeof(pkt));
 
+	// Write second EKF packet
     Vector3f accelBias;
     Vector3f wind;
     Vector3f magNED;
@@ -871,6 +872,50 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
         magZ    : (int16_t)(magXYZ.z)
     };
     WriteBlock(&pkt2, sizeof(pkt2));
-#endif
+
+	// Write third EKF packet
+	Vector3f velInnov;
+	Vector3f posInnov;
+	Vector3f magInnov;
+	float tasInnov;
+    ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov);
+    struct log_EKF3 pkt3 = {
+        LOG_PACKET_HEADER_INIT(LOG_EKF3_MSG),
+        time_ms : hal.scheduler->millis(),
+        innovVN : (int16_t)(100*velInnov.x),
+        innovVE : (int16_t)(100*velInnov.y),
+        innovVD : (int16_t)(100*velInnov.z),
+        innovPN : (int16_t)(100*posInnov.x),
+        innovPE : (int16_t)(100*posInnov.y),
+        innovPD : (int16_t)(100*posInnov.z),
+        innovMX : (int16_t)(magInnov.x),
+        innovMY : (int16_t)(magInnov.y),
+        innovMZ : (int16_t)(magInnov.z),
+        innovVT : (int16_t)(100*tasInnov)
+    };
+    WriteBlock(&pkt3, sizeof(pkt3));
+	
+	// Write fourth EKF packet
+	Vector3f velVar;
+	Vector3f posVar;
+	Vector3f magVar;
+	float tasVar;
+    ahrs.get_NavEKF().getVariances(velVar, posVar, magVar, tasVar);
+    struct log_EKF4 pkt4 = {
+        LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
+        time_ms : hal.scheduler->millis(),
+        sqrtvarVN : (int16_t)(100*sqrtf(velVar.x)),
+        sqrtvarVE : (int16_t)(100*sqrtf(velVar.y)),
+        sqrtvarVD : (int16_t)(100*sqrtf(velVar.z)),
+        sqrtvarPN : (int16_t)(100*sqrtf(posVar.x)),
+        sqrtvarPE : (int16_t)(100*sqrtf(posVar.y)),
+        sqrtvarPD : (int16_t)(100*sqrtf(posVar.z)),
+        sqrtvarMX : (int16_t)(sqrtf(magVar.x)),
+        sqrtvarMY : (int16_t)(sqrtf(magVar.y)),
+        sqrtvarMZ : (int16_t)(sqrtf(magVar.z)),
+        sqrtvarVT : (int16_t)(100*sqrtf(tasVar))
+    };
+    WriteBlock(&pkt4, sizeof(pkt4));
 }
+#endif