WIP: ekf2 EKF backend perf counters

src/modules/ekf2/EKF/covariance.cpp

@@ -99,6 +99,8 @@ void Ekf::initialiseCovariance()
 
 void Ekf::predictCovariance()
 {
+	perf_begin(_predict_covariance_perf);
+
 	// assign intermediate state variables
 	const float &q0 = _state.quat_nominal(0);
 	const float &q1 = _state.quat_nominal(1);
@@ -889,6 +891,7 @@ void Ekf::predictCovariance()
 	// columns for un-used states are zero
 	fixCovarianceErrors(false);
 
+	perf_end(_predict_covariance_perf);
 }
 
 void Ekf::fixCovarianceErrors(bool force_symmetry)

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -55,6 +55,11 @@ EstimatorInterface::~EstimatorInterface()
 	delete _ext_vision_buffer;
 	delete _drag_buffer;
 	delete _auxvel_buffer;
+
+	perf_free(_predict_covariance_perf);
+	perf_free(_mag_heading_fusion_perf);
+	perf_free(_mag_3d_fusion_perf);
+	perf_free(_optical_flow_fusion_perf);
 }
 
 // Accumulate imu data and store to buffer at desired rate
@@ -578,4 +583,9 @@ void EstimatorInterface::print_status()
 
 	printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size());
 	printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
+
+	perf_print_counter(_predict_covariance_perf);
+	perf_print_counter(_mag_heading_fusion_perf);
+	perf_print_counter(_mag_3d_fusion_perf);
+	perf_print_counter(_optical_flow_fusion_perf);
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -68,6 +68,7 @@
 #include "utils.hpp"
 
 #include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
@@ -404,6 +405,11 @@ protected:
 	warning_event_status_u _warning_events{};
 	information_event_status_u _information_events{};
 
+	perf_counter_t _predict_covariance_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": predict covariance")};
+	perf_counter_t _mag_heading_fusion_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": heading fusion")};
+	perf_counter_t _mag_3d_fusion_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": mag 3D fusion")};
+	perf_counter_t _optical_flow_fusion_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": optical flow fusion")};
+
 private:
 
 	inline void setDragData(const imuSample &imu);

src/modules/ekf2/EKF/mag_control.cpp

@@ -362,6 +362,8 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 
 void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
+	perf_begin(_mag_3d_fusion_perf);
+
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
 		// covariances need to be corrected to incorporate knowledge of the declination
@@ -381,6 +383,8 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 			fuseDeclination(0.5f);
 		}
 	}
+
+	perf_end(_mag_3d_fusion_perf);
 }
 
 bool Ekf::otherHeadingSourcesHaveStopped()

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -723,6 +723,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 void Ekf::fuseHeading(float measured_hdg, float obs_var)
 {
+	perf_begin(_mag_heading_fusion_perf);
 	// observation variance
 	float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
 
@@ -774,6 +775,8 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 	} else {
 		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
 	}
+
+	perf_end(_mag_heading_fusion_perf);
 }
 
 void Ekf::fuseDeclination(float decl_sigma)

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -49,6 +49,7 @@
 
 void Ekf::fuseOptFlow()
 {
+	perf_begin(_optical_flow_fusion_perf);
 	float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
 
 	// get latest estimated orientation
@@ -327,6 +328,8 @@ void Ekf::fuseOptFlow()
 			_time_last_of_fuse = _time_last_imu;
 		}
 	}
+
+	perf_end(_optical_flow_fusion_perf);
 }
 
 // calculate optical flow body angular rate compensation

src/modules/ekf2/EKF2.cpp

@@ -257,9 +257,9 @@ int EKF2::print_status()
 	perf_print_counter(_msg_missed_odometry_perf);
 	perf_print_counter(_msg_missed_optical_flow_perf);
 
-#if defined(DEBUG_BUILD)
+//#if defined(DEBUG_BUILD)
 	_ekf.print_status();
-#endif // DEBUG_BUILD
+//#endif // DEBUG_BUILD
 
 	return 0;
 }