From 3fc88b19b87486a6863ef4f9055ca4240e687d87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Jun 2021 11:18:18 +1000 Subject: [PATCH] HAL_SITL: implement initial GPS position offsets --- libraries/AP_HAL_SITL/sitl_gps.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 52088ed540..4bc470e059 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -1219,6 +1219,15 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, { char c; + if (AP_HAL::millis() < 20000) { + // apply the init offsets for the first 20s. This allows for + // having the origin a long way from the takeoff location, + // which makes testing long flights easier + latitude += _sitl->gps_init_lat_ofs; + longitude += _sitl->gps_init_lon_ofs; + altitude += _sitl->gps_init_alt_ofs; + } + //Capture current position as basestation location for if (!_gps_has_basestation_position && _have_lock &&