diff --git a/libraries/AP_NavEKF/AP_Nav_Common.cpp b/libraries/AP_NavEKF/AP_Nav_Common.cpp new file mode 100644 index 0000000000..96604edaf6 --- /dev/null +++ b/libraries/AP_NavEKF/AP_Nav_Common.cpp @@ -0,0 +1,25 @@ +#include "AP_Nav_Common.h" + +#include +#include + +/* + write an EKF timing message + */ +void Log_EKF_Timing(const char *name, uint64_t time_us, const struct ekf_timing &timing) +{ + AP::logger().Write( + name, + "TimeUS,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", + "QIffffffff", + time_us, + timing.count, + (double)timing.dtIMUavg_min, + (double)timing.dtIMUavg_max, + (double)timing.dtEKFavg_min, + (double)timing.dtEKFavg_max, + (double)timing.delAngDT_min, + (double)timing.delAngDT_max, + (double)timing.delVelDT_min, + (double)timing.delVelDT_max); +} diff --git a/libraries/AP_NavEKF/AP_Nav_Common.h b/libraries/AP_NavEKF/AP_Nav_Common.h index 585873d25d..9b087b3d2c 100644 --- a/libraries/AP_NavEKF/AP_Nav_Common.h +++ b/libraries/AP_NavEKF/AP_Nav_Common.h @@ -16,6 +16,8 @@ */ #pragma once +#include + union nav_filter_status { struct { bool attitude : 1; // 0 - true if attitude estimate is valid @@ -73,3 +75,4 @@ struct ekf_timing { float delVelDT_max; float delVelDT_min; }; +void Log_EKF_Timing(const char *name, uint64_t time_us, const struct ekf_timing &timing);