From 2c519d5176a9c2ea1990ecdb234c633be5e563b6 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 29 Apr 2019 13:45:08 +0530 Subject: [PATCH] SITL: Fix to sync to JSBSim simulation time SITL: Additional fix to remove time-lag in JSBSim --- libraries/SITL/SIM_JSBSim.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index 04ed54a5ec..4dd6391e0a 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -415,11 +415,14 @@ void JSBSim::recv_fdm(const struct sitl_input &input) { FGNetFDM fdm; check_stdout(); - while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) { - send_servos(input); - check_stdout(); - } - fdm.ByteSwap(); + + do { + while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) { + send_servos(input); + check_stdout(); + } + fdm.ByteSwap(); + } while (fdm.cur_time == time_now_us); accel_body = Vector3f(fdm.A_X_pilot, fdm.A_Y_pilot, fdm.A_Z_pilot) * FEET_TO_METERS; @@ -443,8 +446,7 @@ void JSBSim::recv_fdm(const struct sitl_input &input) rpm1 = fdm.rpm[0]; rpm2 = fdm.rpm[1]; - // assume 1kHz for now - time_now_us += 1e6/rate_hz; + time_now_us = fdm.cur_time; } void JSBSim::drain_control_socket()