diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp index d4cabe072d..7a33909760 100644 --- a/libraries/SITL/SIM_Precland.cpp +++ b/libraries/SITL/SIM_Precland.cpp @@ -103,8 +103,6 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = { void SIM_Precland::update(const Location &loc, const Vector3f &position) { - const uint32_t now = AP_HAL::millis(); - if (!_enable) { _healthy = false; return; @@ -114,11 +112,6 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position) return; } - if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { - return; - } - _last_update_ms = now; - const float distance_z = -position.z; const float origin_height_m = _origin_height * 0.01f; if (distance_z > _alt_limit + origin_height_m) { @@ -136,6 +129,11 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position) return; } center = center * 0.01f; // cm to m + const uint32_t now = AP_HAL::millis(); + if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { + return; + } + _last_update_ms = now; switch (_type) { case PRECLAND_TYPE_CONE: {