From 733862f649a9115512e76a9b054e5e56853e2c1f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:56:25 +1000 Subject: [PATCH] EKF: move the reset status struct to the Ekf class This protects it from being modified externally --- EKF/ekf.cpp | 1 + EKF/ekf.h | 15 +++++++++++++++ EKF/estimator_interface.cpp | 1 - EKF/estimator_interface.h | 15 --------------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index ffdc8a4814..61d2a57be0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -128,6 +128,7 @@ Ekf::Ekf(): _flow_gyro_bias = {}; _imu_del_ang_of = {}; _gps_check_fail_status.value = 0; + _state_reset_status = {}; } Ekf::~Ekf() diff --git a/EKF/ekf.h b/EKF/ekf.h index af6ca6f3fc..7de59879b7 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -136,6 +136,21 @@ private: static const float _k_earth_rate; static const float _gravity_mss; + // 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; + float _dt_ekf_avg; // average update rate of the ekf stateSample _state; // state struct of the ekf running at the delayed time horizon diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ada6de492e..7427967785 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -71,7 +71,6 @@ 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 9347378857..fc67fe1a9d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -266,21 +266,6 @@ 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;