From 8f4b9c02f0f68ddc69b11c5045dac672ccb886b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 10:45:30 +0200 Subject: [PATCH] EKF: Fix for the GPS timeout logic --- .../ekf_att_pos_estimator_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 877bff6585..60be85b2cd 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,6 +74,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we si static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr float EPH_LARGE_VALUE = 1000.0f; +static constexpr float EPV_LARGE_VALUE = 1000.0f; /** * estimator app start / stop handling function @@ -924,8 +926,16 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; + + if (_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; + } if (!isfinite(_global_pos.lat) || !isfinite(_global_pos.lon) || @@ -1424,7 +1434,7 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {