From aca0336392820d9718f8b007ca794c188d371477 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:28:19 +1000 Subject: [PATCH] EKF: update vertical position and velocity reset capture Use reset event struct members instead of separate variables --- EKF/ekf.cpp | 4 ---- EKF/ekf.h | 6 +----- EKF/ekf_helper.cpp | 16 ++++++++-------- 3 files changed, 9 insertions(+), 17 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c25cc208f0..ffdc8a4814 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -107,10 +107,6 @@ Ekf::Ekf(): _gps_hgt_faulty(false), _rng_hgt_faulty(false), _baro_hgt_offset(0.0f), - _vert_pos_reset_delta(0.0f), - _time_vert_pos_reset(0), - _vert_vel_reset_delta(0.0f), - _time_vert_vel_reset(0), _time_bad_vert_accel(0) { _state = {}; diff --git a/EKF/ekf.h b/EKF/ekf.h index 69b7c1df8a..af6ca6f3fc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -128,7 +128,7 @@ public: void get_gps_check_status(uint16_t *_gps_check_fail_status); // return the amount the local vertical position changed in the last height reset and the time of the reset - void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _vert_pos_reset_delta; *time_us = _time_vert_pos_reset;} + void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} private: @@ -235,10 +235,6 @@ private: int _primary_hgt_source; // priary source of height data set at initialisation float _baro_hgt_offset; // baro height reading at the local NED origin (m) - float _vert_pos_reset_delta; // increase in vertical position state at the last reset(m) - uint64_t _time_vert_pos_reset; // last system time in usec that the vertical position state was reset - float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m) - uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset // imu fault status uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7ba3e2ada2..9dd8e52e1c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -289,18 +289,18 @@ void Ekf::resetHeight() // store the reset amount and time to be published if (vert_pos_reset) { - _vert_pos_reset_delta = _state.pos(2) - old_vert_pos; - _time_vert_pos_reset = _time_last_imu; + _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; + _state_reset_status.posD_time_us = _imu_sample_delayed.time_us; } if (vert_vel_reset) { - _vert_vel_reset_delta = _state.vel(2) - old_vert_vel; - _time_vert_vel_reset = _time_last_imu; + _state_reset_status.velD_change = _state.vel(2) - old_vert_vel; + _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; } // add the reset amount to the output observer states - _output_new.pos(2) += _vert_pos_reset_delta; - _output_new.vel(2) += _vert_vel_reset_delta; + _output_new.pos(2) += _state_reset_status.posD_change; + _output_new.vel(2) += _state_reset_status.velD_change; // add the reset amount to the output observer buffered data outputSample output_states; @@ -309,11 +309,11 @@ void Ekf::resetHeight() output_states = _output_buffer.get_from_index(i); if (vert_pos_reset) { - output_states.pos(2) += _vert_pos_reset_delta; + output_states.pos(2) += _state_reset_status.posD_change; } if (vert_vel_reset) { - output_states.vel(2) += _vert_vel_reset_delta; + output_states.vel(2) += _state_reset_status.velD_change; } _output_buffer.push_to_index(i,output_states);