diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 2bc7fbe1e4..df57efe876 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/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); }; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 7389c6d9b4..815533fa43 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/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 5000) { + lastTimingLogTime_ms = AP_HAL::millis(); + struct ekf_timing timing; + for (uint8_t i=0; i