EKF: capture velocity reset events

9 years ago · e3a1b4a3b3
1 changed files with 7 additions and 0 deletions
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@ -84,6 +84,13 @@ bool Ekf::resetVelocity()
				@@ -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