diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 13c6afd12a..ea09791dc9 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -159,17 +159,6 @@ void Aircraft::update_position(void) location.alt = static_cast(home.alt - position.z * 100.0f); - // we only advance time if it hasn't been advanced already by the - // backend - // TODO : make this a function - if (last_time_us == time_now_us) { - time_now_us += frame_time_us; - } - last_time_us = time_now_us; - if (use_time_sync) { - sync_frame_time(); - } - #if 0 // logging of raw sitl data Vector3f accel_ef = dcm * accel_body; @@ -217,9 +206,17 @@ void Aircraft::update_mag_field_bf() } /* advance time by deltat in seconds */ -void Aircraft::time_advance(float deltat) +void Aircraft::time_advance() { - time_now_us += deltat * 1.0e6f; + // we only advance time if it hasn't been advanced already by the + // backend + if (last_time_us == time_now_us) { + time_now_us += frame_time_us; + } + last_time_us = time_now_us; + if (use_time_sync) { + sync_frame_time(); + } } /* setup the frame step time */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index e88990b129..97c1556f5c 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -191,7 +191,7 @@ protected: void update_mag_field_bf(void); /* advance time by deltat in seconds */ - void time_advance(float deltat); + void time_advance(); /* setup the frame step time */ void setup_frame_time(float rate, float speedup); diff --git a/libraries/SITL/SIM_Balloon.cpp b/libraries/SITL/SIM_Balloon.cpp index 847577f4dd..6c00cfa99a 100644 --- a/libraries/SITL/SIM_Balloon.cpp +++ b/libraries/SITL/SIM_Balloon.cpp @@ -70,6 +70,7 @@ void Balloon::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_CRRCSim.cpp b/libraries/SITL/SIM_CRRCSim.cpp index 58aa117225..22db2e9d75 100644 --- a/libraries/SITL/SIM_CRRCSim.cpp +++ b/libraries/SITL/SIM_CRRCSim.cpp @@ -151,6 +151,7 @@ void CRRCSim::update(const struct sitl_input &input) send_servos(input); recv_fdm(input); update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Calibration.cpp b/libraries/SITL/SIM_Calibration.cpp index 7159e9cbfb..119f8eccf3 100644 --- a/libraries/SITL/SIM_Calibration.cpp +++ b/libraries/SITL/SIM_Calibration.cpp @@ -48,6 +48,7 @@ void SITL::Calibration::update(const struct sitl_input& input) accel_body(0, 0, 0); update_dynamics(rot_accel); update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 32af5104d2..0d98e54e2e 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -366,6 +366,7 @@ void FlightAxis::update(const struct sitl_input &input) } update_position(); + time_advance(); time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6; if (frame_counter++ % 1000 == 0) { diff --git a/libraries/SITL/SIM_Gazebo.cpp b/libraries/SITL/SIM_Gazebo.cpp index aebcb378d6..0093ee1083 100644 --- a/libraries/SITL/SIM_Gazebo.cpp +++ b/libraries/SITL/SIM_Gazebo.cpp @@ -164,6 +164,7 @@ void Gazebo::update(const struct sitl_input &input) recv_fdm(input); update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); drain_sockets(); diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index de08a109be..ea22b4f017 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -164,6 +164,7 @@ void Helicopter::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 0b00f2e2bd..355e5b78df 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -69,6 +69,7 @@ void MultiCopter::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 2a80cc66de..2fea1a0813 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -316,6 +316,7 @@ void Plane::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index be9e9bb9ad..139dfc45f1 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -114,6 +114,7 @@ void QuadPlane::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index c4ad53671b..b7c6ed0da9 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -151,6 +151,7 @@ void SimRover::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp index 50aa9f5fd3..f4e28828b7 100644 --- a/libraries/SITL/SIM_SingleCopter.cpp +++ b/libraries/SITL/SIM_SingleCopter.cpp @@ -101,6 +101,7 @@ void SingleCopter::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index 1611e5e302..5668f7d9e0 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -107,6 +107,7 @@ void Submarine::update(const struct sitl_input &input) // update lat/lon/altitude update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_Tracker.cpp b/libraries/SITL/SIM_Tracker.cpp index c2391685e6..e0da8ae32f 100644 --- a/libraries/SITL/SIM_Tracker.cpp +++ b/libraries/SITL/SIM_Tracker.cpp @@ -134,6 +134,7 @@ void Tracker::update(const struct sitl_input &input) // new velocity vector velocity_ef.zero(); update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index a2ec5ef277..2b4dc8f02b 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -297,6 +297,7 @@ bool XPlane::receive_data(void) } position = pos + position_zero; update_position(); + time_advance(); accel_earth = dcm * accel_body; accel_earth.z += GRAVITY_MSS; @@ -314,6 +315,7 @@ bool XPlane::receive_data(void) position.y = 0; position.z = 0; update_position(); + time_advance(); } update_mag_field_bf(); @@ -355,6 +357,7 @@ failed: velocity_air_bf = dcm.transposed() * velocity_air_ef; update_position(); + time_advance(); update_mag_field_bf(); report.frame_count++; return false; diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp index e7069d3653..31f2b2b337 100644 --- a/libraries/SITL/SIM_last_letter.cpp +++ b/libraries/SITL/SIM_last_letter.cpp @@ -130,6 +130,7 @@ void last_letter::update(const struct sitl_input &input) sync_frame_time(); update_position(); + time_advance(); // update magnetic field update_mag_field_bf(); }