From 92cc067f1802dd9063cef7f036d79a3e30f88915 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 28 Apr 2022 10:28:48 -0400 Subject: [PATCH] WIP: ekf2 EKF backend perf counters --- src/modules/ekf2/EKF/covariance.cpp | 3 +++ src/modules/ekf2/EKF/estimator_interface.cpp | 10 ++++++++++ src/modules/ekf2/EKF/estimator_interface.h | 6 ++++++ src/modules/ekf2/EKF/mag_control.cpp | 4 ++++ src/modules/ekf2/EKF/mag_fusion.cpp | 3 +++ src/modules/ekf2/EKF/optflow_fusion.cpp | 3 +++ src/modules/ekf2/EKF2.cpp | 4 ++-- 7 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f1b7478bd9..d68c2eac68 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/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) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index fb6ccb71db..65e8b1b8df 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/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); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 328c5fd537..3c6864695a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -68,6 +68,7 @@ #include "utils.hpp" #include +#include #include #include #include @@ -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); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 6b631e3deb..050ac8f93e 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/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() diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 5922d3211e..ee56017e98 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/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) diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index d158f90523..465b48261b 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 166a08f1af..8c07b95ca8 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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; }