diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 96617e2003..441c3f1b22 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -169,6 +169,7 @@ EKF2::~EKF2() { perf_free(_ecl_ekf_update_perf); perf_free(_ecl_ekf_update_full_perf); + perf_free(_imu_missed_perf); } bool EKF2::multi_init(int imu, int mag) @@ -219,6 +220,7 @@ int EKF2::print_status() _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); perf_print_counter(_ecl_ekf_update_perf); perf_print_counter(_ecl_ekf_update_full_perf); + perf_print_counter(_imu_missed_perf); return 0; } @@ -298,8 +300,9 @@ void EKF2::Run() imu_updated = _vehicle_imu_sub.update(&imu); if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { - PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation, - _vehicle_imu_sub.get_last_generation()); + perf_count(_imu_missed_perf); + PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation, + _vehicle_imu_sub.get_last_generation()); } imu_sample_new.time_us = imu.timestamp_sample; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 81045774db..4bf4ce610d 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -176,6 +176,7 @@ private: perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; + perf_counter_t _imu_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; // Used to check, save and use learned magnetometer biases hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)