diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index e422e97f45..b7d608dc4b 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -330,6 +330,10 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) smooth_sensors(); } fdm.timestamp_us = time_now_us; + if (fdm.home.lat == 0 && fdm.home.lng == 0) { + // initialise home + fdm.home = home; + } fdm.latitude = location.lat * 1.0e-7; fdm.longitude = location.lng * 1.0e-7; fdm.altitude = location.alt * 1.0e-2; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index d6e7ae73f6..648b87ab89 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -9,6 +9,7 @@ namespace SITL { struct sitl_fdm { // this is the structure passed between FDM models and the main SITL code uint64_t timestamp_us; + Location home; double latitude, longitude; // degrees double altitude; // MSL double heading; // degrees