CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Update Drone.cs

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-07-03 01:32:11 +03:00
parent 87c1210b4b
commit 5357dd69fc
1 changed files with 21 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
DroneSimulator/Drone.cs
View File

@ -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);
}