From 0de15b1b2088810397148cbd402d94b76382e4f8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:28:56 +1000 Subject: [PATCH] EKF: Add variables to monitor vertical position and height offset --- EKF/ekf.cpp | 6 +++++- EKF/ekf.h | 4 ++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 74a9562dc8..b5e8df7487 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -85,7 +85,11 @@ Ekf::Ekf(): _baro_hgt_faulty(false), _gps_hgt_faulty(false), _rng_hgt_faulty(false), - _baro_hgt_offset(0.0f) + _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) { _state = {}; _last_known_posNE.setZero(); diff --git a/EKF/ekf.h b/EKF/ekf.h index e2df78fdac..2c62540d3f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -232,6 +232,10 @@ private: int _primary_hgt_source; // priary source of height data set at initialisation float _baro_hgt_offset; // number of metres the baro height origin is above 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 // update the real time complementary filter states. This includes the prediction // and the correction step