diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 7427967785..ada6de492e 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -71,6 +71,7 @@ EstimatorInterface::EstimatorInterface(): _pos_ref = {}; memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); + _state_reset_status = {}; } EstimatorInterface::~EstimatorInterface() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index fc67fe1a9d..9347378857 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -266,6 +266,21 @@ protected: float _tas_test_ratio; // tas innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status; + // reset event monitoring + // structure containing velocity, position, height and yaw reset information + struct { + uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us) + uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) + uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) + uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) + uint64_t yaw_time_us; // time stamp of the last yaw angle reset event (us) + Vector2f velNE_change; // North East velocity change due to last reset (m) + float velD_change; // Down velocity change due to last reset (m/s) + Vector2f posNE_change; // North, East position change due to last reset (m) + float posD_change; // Down position change due to last reset (m) + float yaw_change; // Yaw angle change due to last reset (rad) + } _state_reset_status; + // data buffer instances RingBuffer _imu_buffer; RingBuffer _gps_buffer;