From bb950a1550ba55de36ed66877817ec7f2f47b349 Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Wed, 10 Mar 2021 16:52:20 +0100 Subject: [PATCH] Expose terrain vpos reset counter --- EKF/ekf.h | 4 ++++ EKF/terrain_estimator.cpp | 1 + 2 files changed, 5 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index ade36dd280..77f40cce5b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -217,6 +217,9 @@ public: // get the estimated terrain vertical position relative to the NED origin float getTerrainVertPos() const { return _terrain_vpos; }; + // get the number of times the vertical terrain position has been reset + uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; + // get the terrain variance float get_terrain_var() const { return _terrain_var; } @@ -510,6 +513,7 @@ private: // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) + uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 75fa5a84fc..2657d8cd08 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -170,6 +170,7 @@ void Ekf::fuseHagl() if (isTimedOut(_time_last_hagl_fuse, timeout)) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance; + _terrain_vpos_reset_counter++; } else { _innov_check_fail_status.flags.reject_hagl = true;