diff --git a/EKF/control.cpp b/EKF/control.cpp index 751ac41aa3..63288eec45 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -126,7 +126,7 @@ void Ekf::controlExternalVisionAiding() } // capture the reset event - _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + _state_reset_status.quat_counter++; // flag the yaw as aligned _control_status.flags.yaw_align = true; diff --git a/EKF/ekf.h b/EKF/ekf.h index 4b83b66379..88237bd3f3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -127,23 +127,23 @@ public: // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); - // return the amount the local vertical position changed in the last reset and the time of the reset - void get_posD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} + // return the amount the local vertical position changed in the last reset and the number of reset events + void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;} - // return the amount the local vertical velocity changed in the last reset and the time of the reset - void get_velD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.velD_change; *time_us = _state_reset_status.velD_time_us;} + // return the amount the local vertical velocity changed in the last reset and the number of reset events + void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;} - // return the amount the local horizontal position changed in the last reset and the time of the reset - void get_posNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.posNE_change; *time_us = _state_reset_status.posNE_time_us;} + // return the amount the local horizontal position changed in the last reset and the number of reset events + void get_posNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.posNE_change; *counter = _state_reset_status.posNE_counter;} - // return the amount the local horizontal velocity changed in the last reset and the time of the reset - void get_velNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.velNE_change; *time_us = _state_reset_status.velNE_time_us;} + // return the amount the local horizontal velocity changed in the last reset and the number of reset events + void get_velNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.velNE_change; *counter = _state_reset_status.velNE_counter;} - // return the amount the quaternion has changed in the last reset and the time of the reset - void get_quat_reset(Quaternion *delta, uint64_t *time_us) + // return the amount the quaternion has changed in the last reset and the number of reset events + void get_quat_reset(Quaternion *delta, uint8_t *counter) { *delta = _state_reset_status.quat_change; - *time_us = _state_reset_status.quat_time_us; + *counter = _state_reset_status.quat_counter; } private: @@ -155,11 +155,11 @@ private: // 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 quat_time_us; // time stamp of the last quaternion reset event (us) + uint8_t velNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t velD_counter; // number of vertical velocity reset events (allow to wrap if count exceeds 255) + uint8_t posNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t posD_counter; // number of vertical position reset events (allow to wrap if count exceeds 255) + uint8_t quat_counter; // number of quaternion reset events (allow to wrap if count exceeds 255) 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) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5a74ddd54b..a9a91fc595 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -88,8 +88,8 @@ bool Ekf::resetVelocity() _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; + _state_reset_status.velNE_counter++; + _state_reset_status.velD_counter++; } @@ -162,7 +162,7 @@ bool Ekf::resetPosition() // capture the reset event _state_reset_status.posNE_change = posNE_change; - _state_reset_status.posNE_time_us = _imu_sample_delayed.time_us; + _state_reset_status.posNE_counter++; } @@ -290,12 +290,12 @@ void Ekf::resetHeight() // store the reset amount and time to be published if (vert_pos_reset) { _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; - _state_reset_status.posD_time_us = _imu_sample_delayed.time_us; + _state_reset_status.posD_counter++; } if (vert_vel_reset) { _state_reset_status.velD_change = _state.vel(2) - old_vert_vel; - _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; + _state_reset_status.velD_counter++; } // add the reset amount to the output observer states @@ -497,7 +497,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) } // capture the reset event - _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + _state_reset_status.quat_counter++; return true; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 81ff0d3dc8..34d8bc8596 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -217,20 +217,20 @@ public: // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; - // return the amount the local vertical position changed in the last reset and the time of the reset - virtual void get_posD_reset(float *delta, uint64_t *time_us) = 0; + // return the amount the local vertical position changed in the last reset and the number of reset events + virtual void get_posD_reset(float *delta, uint8_t *counter) = 0; - // return the amount the local vertical velocity changed in the last reset and the time of the reset - virtual void get_velD_reset(float *delta, uint64_t *time_us) = 0; + // return the amount the local vertical velocity changed in the last reset and the number of reset events + virtual void get_velD_reset(float *delta, uint8_t *counter) = 0; - // return the amount the local horizontal position changed in the last reset and the time of the reset - virtual void get_posNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + // return the amount the local horizontal position changed in the last reset and the number of reset events + virtual void get_posNE_reset(Vector2f *delta, uint8_t *counter) = 0; - // return the amount the local horizontal velocity changed in the last reset and the time of the reset - virtual void get_velNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + // return the amount the local horizontal velocity changed in the last reset and the number of reset events + virtual void get_velNE_reset(Vector2f *delta, uint8_t *counter) = 0; - // return the amount the quaternion has changed in the last reset and the time of the reset - virtual void get_quat_reset(Quaternion *delta, uint64_t *time_us) = 0; + // return the amount the quaternion has changed in the last reset and the number of reset events + virtual void get_quat_reset(Quaternion *delta, uint8_t *counter) = 0; // get EKF innovation consistency check status virtual void get_innovation_test_status(uint16_t *val)