From 96f47d47722183de4a198ced1c9215a28e5f46bc Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 28 Jun 2018 17:28:03 -0400
Subject: [PATCH] EKF2 add perf counters (#9795)

---
 src/modules/ekf2/CMakeLists.txt |  1 +
 src/modules/ekf2/ekf2_main.cpp  | 28 ++++++++++++++++++++++++----
 2 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt
index 6818295803..9bfed2e8dd 100644
--- a/src/modules/ekf2/CMakeLists.txt
+++ b/src/modules/ekf2/CMakeLists.txt
@@ -43,4 +43,5 @@ px4_add_module(
 		git_ecl
 		ecl_EKF
 		ecl_geo
+		perf
 	)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index e54c9d4f55..847779551f 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -41,8 +41,9 @@
 #include <cfloat>
 
 #include <drivers/drv_hrt.h>
-#include <ecl/EKF/ekf.h>
-#include <mathlib/mathlib.h>
+#include <lib/ecl/EKF/ekf.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
 #include <px4_defines.h>
 #include <px4_module.h>
 #include <px4_module_params.h>
@@ -119,6 +120,9 @@ private:
 	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
 	int64_t _last_time_slip_us = 0;		///< Last time slip (uSec)
 
+	perf_counter_t _perf_update_data;
+	perf_counter_t _perf_ekf_update;
+
 	// Initialise time stamps used to send sensor data to the EKF and for logging
 	uint8_t _invalid_mag_id_count = 0;	///< number of times an invalid magnetomer device ID has been detected
 
@@ -403,6 +407,8 @@ private:
 
 Ekf2::Ekf2():
 	ModuleParams(nullptr),
+	_perf_update_data(perf_alloc_once(PC_ELAPSED, "EKF2 data acquisition")),
+	_perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")),
 	_vehicle_local_position_pub(ORB_ID(vehicle_local_position)),
 	_vehicle_global_position_pub(ORB_ID(vehicle_global_position)),
 	_params(_ekf.getParamHandle()),
@@ -521,6 +527,9 @@ Ekf2::Ekf2():
 
 Ekf2::~Ekf2()
 {
+	perf_free(_perf_update_data);
+	perf_free(_perf_ekf_update);
+
 	orb_unsubscribe(_airdata_sub);
 	orb_unsubscribe(_airspeed_sub);
 	orb_unsubscribe(_ev_att_sub);
@@ -543,9 +552,14 @@ Ekf2::~Ekf2()
 
 int Ekf2::print_status()
 {
-	PX4_INFO("local position OK %s", (_ekf.local_position_is_valid()) ? "yes" : "no");
-	PX4_INFO("global position OK %s", (_ekf.global_position_is_valid()) ? "yes" : "no");
+	PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid");
+	PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid");
+
 	PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us);
+
+	perf_print_counter(_perf_update_data);
+	perf_print_counter(_perf_ekf_update);
+
 	return 0;
 }
 
@@ -602,6 +616,8 @@ void Ekf2::run()
 			continue;
 		}
 
+		perf_begin(_perf_update_data);
+
 		bool params_updated = false;
 		orb_check(_params_sub, &params_updated);
 
@@ -1010,8 +1026,12 @@ void Ekf2::run()
 			}
 		}
 
+		perf_end(_perf_update_data);
+
 		// run the EKF update and output
+		perf_begin(_perf_ekf_update);
 		const bool updated = _ekf.update();
+		perf_end(_perf_ekf_update);
 
 		if (updated) {