dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: move the reset status struct to the Ekf class 

This protects it from being modified externally
This commit is contained in:
Paul Riseborough 2016-05-23 19:56:25 +10:00
parent aca0336392
commit 733862f649
4 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/ekf.cpp
View File

@ -128,6 +128,7 @@ Ekf::Ekf():
_flow_gyro_bias = {}; _flow_gyro_bias = {};
_imu_del_ang_of = {}; _imu_del_ang_of = {};
_gps_check_fail_status.value = 0; _gps_check_fail_status.value = 0;
_state_reset_status = {};
} }
Ekf::~Ekf() Ekf::~Ekf()

15
EKF/ekf.h
View File

@ -136,6 +136,21 @@ private:
static const float _k_earth_rate; static const float _k_earth_rate;
static const float _gravity_mss; 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 float _dt_ekf_avg; // average update rate of the ekf
stateSample _state; // state struct of the ekf running at the delayed time horizon stateSample _state; // state struct of the ekf running at the delayed time horizon

1
EKF/estimator_interface.cpp
View File

@ -71,7 +71,6 @@ EstimatorInterface::EstimatorInterface():
_pos_ref = {}; _pos_ref = {};
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
_state_reset_status = {};
} }
EstimatorInterface::~EstimatorInterface() EstimatorInterface::~EstimatorInterface()

15
EKF/estimator_interface.h
View File

@ -266,21 +266,6 @@ protected:
float _tas_test_ratio; // tas innovation consistency check ratio float _tas_test_ratio; // tas innovation consistency check ratio
innovation_fault_status_u _innov_check_fail_status; 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 // data buffer instances
RingBuffer<imuSample> _imu_buffer; RingBuffer<imuSample> _imu_buffer;
RingBuffer<gpsSample> _gps_buffer; RingBuffer<gpsSample> _gps_buffer;