DataFlash: added logging of timing statistics

libraries/DataFlash/DataFlash.h

@@ -279,4 +279,6 @@ private:
     void validate_structures(const struct LogStructure *structures, const uint8_t num_types);
     void dump_structure_field(const struct LogStructure *structure, const char *label, const uint8_t fieldnum);
     void dump_structures(const struct LogStructure *structures, const uint8_t num_types);
+
+    void Log_Write_EKF_Timing(const char *name, uint64_t time_us, const struct ekf_timing &timing);
 };

libraries/DataFlash/LogFile.cpp

@@ -1092,6 +1092,24 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     }
 }
 
+
+/*
+  write an EKF timing message
+ */
+void DataFlash_Class::Log_Write_EKF_Timing(const char *name, uint64_t time_us, const struct ekf_timing &timing)
+{
+    Log_Write(name,
+              "TimeUS,Cnt,IMUMin,IMUMax,AngMin,AngMax,VelMin,VelMax", "QIffffff",
+              time_us,
+              timing.count,
+              (double)timing.dtIMUavg_min,
+              (double)timing.dtIMUavg_max,
+              (double)timing.delAngDT_min,
+              (double)timing.delAngDT_max,
+              (double)timing.delVelDT_min,
+              (double)timing.delVelDT_max);
+}
+
 void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
 {
     uint64_t time_us = AP_HAL::micros64();
@@ -1412,6 +1430,17 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
             }
         }
     }
+
+    // log EKF timing statistics every 5s
+    static uint32_t lastTimingLogTime_ms = 0;
+    if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
+        lastTimingLogTime_ms = AP_HAL::millis();
+        struct ekf_timing timing;
+        for (uint8_t i=0; i<ahrs.get_NavEKF2().activeCores(); i++) {
+            ahrs.get_NavEKF2().getTimingStatistics(i, timing);
+            Log_Write_EKF_Timing(i==0?"NKT1":"NKT2", time_us, timing);
+        }
+    }
 }
 
 
@@ -1747,6 +1776,17 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
         WriteBlock(&pkt11, sizeof(pkt11));
         updateTime_ms = lastUpdateTime_ms;
     }
+
+    // log EKF timing statistics every 5s
+    static uint32_t lastTimingLogTime_ms = 0;
+    if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
+        lastTimingLogTime_ms = AP_HAL::millis();
+        struct ekf_timing timing;
+        for (uint8_t i=0; i<ahrs.get_NavEKF3().activeCores(); i++) {
+            ahrs.get_NavEKF3().getTimingStatistics(i, timing);
+            Log_Write_EKF_Timing(i==0?"XKT1":"XKT2", time_us, timing);
+        }
+    }
 }
 #endif