fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Only publish with low EPH / EPV if the position estimate is actually valid / in place.

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-26 20:07:56 +02:00
parent 56adce9432
commit 5db1490a03
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+6 -1
View File
@@ -932,9 +932,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
if (!_local_pos.xy_global ||
!_local_pos.v_xy_valid ||
_gps.timestamp_position == 0 ||
(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_global_pos.eph = EPH_LARGE_VALUE;
_global_pos.epv = EPV_LARGE_VALUE;
} else {
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;