From 2814605a2ce0e355ce3e4b8d7b7eb1283f0e4781 Mon Sep 17 00:00:00 2001 From: Shiv Khanna Date: Tue, 6 Sep 2022 12:34:39 -0700 Subject: [PATCH] SITL: Fixed rounding lat/lng issue when running JSBSim SITL Fixed an issue in recieving aircraft state data from JSBSim where the location was being converted to degrees but as a float. This caused rounding issues in the lat and lng. Changed the conversion to use a double. --- libraries/SITL/SIM_JSBSim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index ca1128143d..ea798601db 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -436,8 +436,8 @@ void JSBSim::recv_fdm(const struct sitl_input &input) gyro = Vector3f(p, q, r); velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS; - location.lat = degrees(fdm.latitude) * 1.0e7; - location.lng = degrees(fdm.longitude) * 1.0e7; + location.lat = RAD_TO_DEG_DOUBLE * fdm.latitude * 1.0e7; + location.lng = RAD_TO_DEG_DOUBLE * fdm.longitude * 1.0e7; location.alt = fdm.agl*100 + home.alt; dcm.from_euler(fdm.phi, fdm.theta, fdm.psi); airspeed = fdm.vcas * KNOTS_TO_METERS_PER_SECOND;