From 29d35699f2e1cfb27a76e69e6a5d738c38a16417 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@retdtechfrance.com>
Date: Fri, 6 Jan 2017 12:02:20 +0100
Subject: [PATCH] SITL: Gazebo, throw old paquet based on timestamp as we are
 using UDP Add some comments

---
 libraries/SITL/SIM_Gazebo.cpp | 19 ++++++++-----------
 libraries/SITL/SIM_Gazebo.h   |  2 +-
 2 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/libraries/SITL/SIM_Gazebo.cpp b/libraries/SITL/SIM_Gazebo.cpp
index 89cc84327e..a25649d8a6 100644
--- a/libraries/SITL/SIM_Gazebo.cpp
+++ b/libraries/SITL/SIM_Gazebo.cpp
@@ -88,6 +88,11 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
         send_servos(input);
     }
 
+    double deltat = pkt.timestamp - last_timestamp;  // in seconds
+    if (deltat < 0) {  // don't use old paquet
+        time_now_us += 1;
+        return;
+    }
     // get imu stuff
     accel_body = Vector3f(static_cast<float>(pkt.imu_linear_acceleration_xyz[0]),
                           static_cast<float>(pkt.imu_linear_acceleration_xyz[1]),
@@ -114,7 +119,6 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
 
 
     // auto-adjust to simulation frame rate
-    double deltat = pkt.timestamp - last_timestamp;
     time_now_us += static_cast<uint64_t>(deltat * 1.0e6);
 
     if (deltat < 0.01 && deltat > 0) {
@@ -122,18 +126,11 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
     }
     last_timestamp = pkt.timestamp;
 
-    /* copied below from iris_ros.py */
-    /*
-    bearing = to_degrees(atan2(position.y, position.x));
-    distance = math.sqrt(self.position.x**2 + self.position.y**2)
-    (self.latitude, self.longitude) = util.gps_newpos(
-      self.home_latitude, self.home_longitude, bearing, distance)
-    self.altitude  = self.home_altitude - self.position.z
-    velocity_body = self.dcm.transposed() * self.velocity
-    self.accelerometer = self.accel_body.copy()
-    */
 }
 
+/*
+  Drain remaining data on the socket to prevent phase lag.
+ */
 void Gazebo::drain_sockets()
 {
     const uint16_t buflen = 1024;
diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h
index c88e778ae4..ad602fd275 100644
--- a/libraries/SITL/SIM_Gazebo.h
+++ b/libraries/SITL/SIM_Gazebo.h
@@ -54,7 +54,7 @@ private:
       reply packet sent from Gazebo to ArduPilot
      */
     struct fdm_packet {
-      double timestamp;
+      double timestamp;  // in seconds
       double imu_angular_velocity_rpy[3];
       double imu_linear_acceleration_xyz[3];
       double imu_orientation_quat[4];