From 2d34a3e0966b917bab70c2692f70267bf0cc6b91 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 May 2017 18:13:17 +1000 Subject: [PATCH] ekf2: monitor estimator time slip Used to check if the ekf2 module is failing to keep up with the IMU data --- src/modules/ekf2/ekf2_main.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index fdcdb07e65..985dc48874 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -135,6 +135,10 @@ private: float _default_ev_pos_noise = 0.05f; // external vision position noise used when an invalid value is supplied float _default_ev_ang_noise = 0.05f; // external vision angle noise used when an invalid value is supplied + // time slip monitoring + uint64_t integrated_time_us = 0; // integral of gyro delta time from start (usec) + uint64_t start_time_us = 0; // system time at start (usec) + // Initialise time stamps used to send sensor data to the EKF and for logging uint64_t _timestamp_mag_us = 0; uint64_t _timestamp_balt_us = 0; @@ -755,6 +759,14 @@ void Ekf2::task_main() // run the EKF update and output if (_ekf.update()) { + // integrate time to monitor time slippage + if (start_time_us == 0) { + start_time_us = now; + + } else if (start_time_us > 0) { + integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6); + } + matrix::Quaternion q; _ekf.copy_quaternion(q.data()); @@ -997,6 +1009,14 @@ void Ekf2::task_main() _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.get_imu_vibe_metrics(status.vibe); + // monitor time slippage + if (start_time_us != 0 && now > start_time_us) { + status.time_slip = (float)(1e-6 * ((double)(now - start_time_us) - (double) integrated_time_us)); + + } else { + status.time_slip = 0.0f; + } + if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);