fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Expose terrain vpos reset counter

Browse Source
This commit is contained in:
Morten Fyhn Amundsen 2021-03-10 16:52:20 +01:00 committed by Mathieu Bresciani
parent ab69681744
commit bb950a1550
2 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/ekf.h
View File

@ -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

1
EKF/terrain_estimator.cpp
View File

@ -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;