fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 19:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: tighten terrain validity requirements

Browse Source 
- require valid fusion from a range finder or optical flow before
   considering terrain valid again
This commit is contained in:
Daniel Agar
2024-10-04 10:31:20 +02:00
committed by Mathieu Bresciani
parent 5b85859bfe
commit 34ee097f02
3 changed files with 42 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+5 -1
View File
@@ -99,7 +99,7 @@ public:
#if defined(CONFIG_EKF2_TERRAIN)
// terrain estimate
bool isTerrainEstimateValid() const;
bool isTerrainEstimateValid() const { return _terrain_valid; }
// get the estimated terrain vertical position relative to the NED origin
float getTerrainVertPos() const { return _state.terrain; };
@@ -500,6 +500,8 @@ private:
#if defined(CONFIG_EKF2_TERRAIN)
// Terrain height state estimation
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
bool _terrain_valid{false};
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -733,6 +735,8 @@ private:
float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
void controlTerrainFakeFusion();
void updateTerrainValidity();
# if defined(CONFIG_EKF2_RANGE_FINDER)
// update the terrain vertical position estimate using a height above ground measurement from the range finder
bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);