DataFlash: log gyro and accel error counts

libraries/DataFlash/DataFlash.h

@@ -217,6 +217,7 @@ struct PACKED log_IMU {
     uint32_t timestamp;
     float gyro_x, gyro_y, gyro_z;
     float accel_x, accel_y, accel_z;
+    uint32_t gyro_error, accel_error;
 };
 
 struct PACKED log_RCIN {
@@ -467,7 +468,7 @@ struct PACKED log_Esc {
     { LOG_GPS_MSG, sizeof(log_GPS), \
       "GPS",  "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \
     { LOG_IMU_MSG, sizeof(log_IMU), \
-      "IMU",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
+      "IMU",  "IffffffII",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA" }, \
     { LOG_MESSAGE_MSG, sizeof(log_Message), \
       "MSG",  "Z",     "Message"}, \
     { LOG_RCIN_MSG, sizeof(log_RCIN), \
@@ -490,9 +491,9 @@ struct PACKED log_Esc {
     { LOG_GPS2_MSG, sizeof(log_GPS2), \
       "GPS2",  "BIHBcLLeEefIBI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,T,DSc,DAg" }, \
     { LOG_IMU2_MSG, sizeof(log_IMU), \
-      "IMU2",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
+      "IMU2",  "IffffffII",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA" }, \
     { LOG_IMU3_MSG, sizeof(log_IMU), \
-      "IMU3",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
+      "IMU3",  "IffffffII",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA" }, \
     { LOG_AHR2_MSG, sizeof(log_AHRS), \
       "AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
     { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \

libraries/DataFlash/LogFile.cpp

@@ -790,7 +790,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
         gyro_z  : gyro.z,
         accel_x : accel.x,
         accel_y : accel.y,
-        accel_z : accel.z
+        accel_z : accel.z,
+        gyro_error  : ins.get_gyro_error_count(0),
+        accel_error : ins.get_accel_error_count(0)
     };
     WriteBlock(&pkt, sizeof(pkt));
     if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
@@ -807,7 +809,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
         gyro_z  : gyro2.z,
         accel_x : accel2.x,
         accel_y : accel2.y,
-        accel_z : accel2.z
+        accel_z : accel2.z,
+        gyro_error  : ins.get_gyro_error_count(1),
+        accel_error : ins.get_accel_error_count(1)
     };
     WriteBlock(&pkt2, sizeof(pkt2));
     if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
@@ -823,7 +827,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
         gyro_z  : gyro3.z,
         accel_x : accel3.x,
         accel_y : accel3.y,
-        accel_z : accel3.z
+        accel_z : accel3.z,
+        gyro_error  : ins.get_gyro_error_count(2),
+        accel_error : ins.get_accel_error_count(3)
     };
     WriteBlock(&pkt3, sizeof(pkt3));
 #endif