diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 4b53469..4e19dc9 100644 --- a/DroneSimulator/Drone.cs +++ b/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(step); + drone.Action(next); 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); }