mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: Fix timer wrapping bug
This commit is contained in:
parent
0d965d2a1c
commit
53df8a0238
|
@ -620,7 +620,7 @@ void NavEKF3_core::runYawEstimator()
|
|||
}
|
||||
|
||||
// action an external reset request
|
||||
if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms < (EKFGSF_yaw_reset_request_ms + YAW_RESET_TO_GSF_TIMEOUT_MS)) {
|
||||
if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
|
||||
EKFGSF_resetMainFilterYaw();
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue