Update Drone.cs

DroneSimulator/Drone.cs

@@ -56,9 +56,13 @@ namespace DroneSimulator
     private RealMode.Range RealRange = new RealMode.Range();
     private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
 
-    private static uint Tick()
+    private uint TickCount = 0;
+
+    private uint Tick()
     {
-      return (uint)(Stopwatch.GetTimestamp() / Stopwatch.Frequency / 1000);
+      uint tick = (uint)(Stopwatch.GetTimestamp() / Stopwatch.Frequency / 1000);
+      TickCount += tick;
+      return tick;
     }
 
     public static byte[] getBytes(object data)
@@ -154,15 +158,12 @@ namespace DroneSimulator
             {
               if (drone.StepTime > 0)
               {
-                uint tick = Tick();
                 uint prev = drone.Timer;
-                uint step = prev + drone.StepTime;
+                uint next = prev + drone.StepTime;
 
-                if (TimeLimit && (step > tick)) step = tick;
+                drone.Action(next);
-
-                drone.Action(step);
                 drone.StepTime = 0;
-                drone.SendStep(step, prev);
+                drone.SendStep(next, prev);
                 break;
               }
             }
@@ -174,7 +175,7 @@ namespace DroneSimulator
     public Drone(int id, Socket? client)
     {
       ID = id;
-      Timer = Tick();
+      Timer = 0;
       Client = client;
     }
 
@@ -389,7 +390,7 @@ namespace DroneSimulator
       step.Head.Size = Marshal.SizeOf(typeof(DroneData.Step));
       step.Head.Mode = DroneData.DataMode.Response;
       step.Head.Type = DroneData.DataType.Step;
-      step.Head.Time = Tick();
+      step.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       step.StepTime = time;
       step.PrevTime = prev;
@@ -422,7 +423,7 @@ namespace DroneSimulator
       acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
       acc.Head.Mode = DroneData.DataMode.Response;
       acc.Head.Type = DroneData.DataType.DataAcc;
-      acc.Head.Time = Tick();
+      acc.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
       acc.Time = RealAcc.timer;
@@ -437,7 +438,7 @@ namespace DroneSimulator
       gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
       gyr.Head.Mode = DroneData.DataMode.Response;
       gyr.Head.Type = DroneData.DataType.DataGyr;
-      gyr.Head.Time = Tick();
+      gyr.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
       gyr.Time = RealGyr.timer;
@@ -452,7 +453,7 @@ namespace DroneSimulator
       mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
       mag.Head.Mode = DroneData.DataMode.Response;
       mag.Head.Type = DroneData.DataType.DataMag;
-      mag.Head.Time = Tick();
+      mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
       mag.Time = Timer;
@@ -467,7 +468,7 @@ namespace DroneSimulator
       range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
       range.Head.Mode = DroneData.DataMode.Response;
       range.Head.Type = DroneData.DataType.DataRange;
-      range.Head.Time = Tick();
+      range.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       range.LiDAR = RealRange.result;
       range.Time = RealRange.timer;
@@ -482,7 +483,7 @@ namespace DroneSimulator
       local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
       local.Head.Mode = DroneData.DataMode.Response;
       local.Head.Type = DroneData.DataType.DataLocal;
-      local.Head.Time = Tick();
+      local.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
       local.Time = RealPos.timer;
@@ -497,7 +498,7 @@ namespace DroneSimulator
       bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
       bar.Head.Mode = DroneData.DataMode.Response;
       bar.Head.Type = DroneData.DataType.DataBar;
-      bar.Head.Time = Tick();
+      bar.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       bar.Pressure = RealBar.result;
       bar.Time = RealBar.timer;
@@ -512,7 +513,7 @@ namespace DroneSimulator
       of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
       of.Head.Mode = DroneData.DataMode.Response;
       of.Head.Type = DroneData.DataType.DataOF;
-      of.Head.Time = Tick();
+      of.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       lock (this)
       {
@@ -534,7 +535,7 @@ namespace DroneSimulator
       gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
       gps.Head.Mode = DroneData.DataMode.Response;
       gps.Head.Type = DroneData.DataType.DataGPS;
-      gps.Head.Time = Tick();
+      gps.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       GPS.Point p = new GPS.Point();
       p.x = RealPos.result.Y; p.y= RealPos.result.X;
@@ -572,7 +573,7 @@ namespace DroneSimulator
       quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
       quat.Head.Mode = DroneData.DataMode.Response;
       quat.Head.Type = DroneData.DataType.DataQuat;
-      quat.Head.Time = Tick();
+      quat.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
 
@@ -586,7 +587,7 @@ namespace DroneSimulator
       head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
       head.Mode = DroneData.DataMode.Response;
       head.Type = DroneData.DataType.Ping;
-      head.Time = Tick();
+      head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       return getBytes(head);
     }