From e3a1b4a3b3e16990973cf89b28a58f2ad08032f3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:24:42 +1000 Subject: [PATCH] EKF: capture velocity reset events --- EKF/ekf_helper.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5ab4b978dc..0f344573ed 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -84,6 +84,13 @@ bool Ekf::resetVelocity() _output_buffer.push_to_index(index,output_states); } + // capture the reset event + _state_reset_status.velNE_change(0) = velocity_change(0); + _state_reset_status.velNE_change(1) = velocity_change(1); + _state_reset_status.velD_change = velocity_change(2); + _state_reset_status.velNE_time_us = _imu_sample_delayed.time_us; + _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; + } // Reset position states. If we have a recent and valid