diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index f080be9767..05c3e422e7 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -242,10 +242,6 @@ void SITL_State::_fdm_input(void) return; } - hal.scheduler->stop_clock(d.fg_pkt.timestamp_us); - _synthetic_clock_mode = true; - got_fg_input = true; - if (d.fg_pkt.latitude == 0 || d.fg_pkt.longitude == 0 || d.fg_pkt.altitude <= 0) { @@ -253,6 +249,10 @@ void SITL_State::_fdm_input(void) return; } + hal.scheduler->stop_clock(d.fg_pkt.timestamp_us); + _synthetic_clock_mode = true; + got_fg_input = true; + if (_sitl != NULL) { _sitl->state = d.fg_pkt; // prevent bad inputs from SIM from corrupting our state