diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 46f655673f..e9131a0ece 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -170,6 +170,7 @@ EKF2::~EKF2() perf_free(_ecl_ekf_update_perf); perf_free(_ecl_ekf_update_full_perf); perf_free(_imu_missed_perf); + perf_free(_mag_missed_perf); } bool EKF2::multi_init(int imu, int mag) @@ -222,6 +223,11 @@ int EKF2::print_status() perf_print_counter(_ecl_ekf_update_perf); perf_print_counter(_ecl_ekf_update_full_perf); perf_print_counter(_imu_missed_perf); + + if (_device_id_mag != 0) { + perf_print_counter(_mag_missed_perf); + } + return 0; } @@ -1517,8 +1523,9 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) if (_magnetometer_sub.update(&magnetometer)) { if (_magnetometer_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation, - _magnetometer_sub.get_last_generation()); + perf_count(_mag_missed_perf); + PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation, + _magnetometer_sub.get_last_generation()); } bool reset = false; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 4bf4ce610d..28711f24b5 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -177,6 +177,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")}; + perf_counter_t _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag 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)