Browse Source

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

sbg
Lorenz Meier 10 years ago
parent
commit
5db1490a03
  1. 7
      src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

7
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@ -932,9 +932,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() @@ -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;

Loading…
Cancel
Save