From 9e0f525f69af2eb91562f9452a503a1004b97eee Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 28 Aug 2019 14:09:18 +1000
Subject: [PATCH] AP_NavEKF: move logging in from AP_Logger

---
 libraries/AP_NavEKF/AP_Nav_Common.cpp | 25 +++++++++++++++++++++++++
 libraries/AP_NavEKF/AP_Nav_Common.h   |  3 +++
 2 files changed, 28 insertions(+)
 create mode 100644 libraries/AP_NavEKF/AP_Nav_Common.cpp

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 <AP_HAL/AP_HAL.h>
+#include <AP_Logger/AP_Logger.h>
+
+/*
+  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 <stdint.h>
+
 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);