forked from CPL/Simulator
+++
This commit is contained in:
@ -1,4 +1,6 @@
|
||||
using System.Numerics;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Diagnostics;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
using static System.Net.Mime.MediaTypeNames;
|
||||
|
||||
@ -28,8 +30,13 @@ namespace DroneSimulator
|
||||
private const float TO_GRAD = 180 / MathF.PI;
|
||||
private const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
private Thread DroneThread;
|
||||
private int Timer;
|
||||
public static List<Drone> AllDrones = new List<Drone>();
|
||||
private static Thread? DroneThread = null;
|
||||
public static long Timing = 0;
|
||||
public static long Freq = 1000;
|
||||
public static bool Boost = false;
|
||||
|
||||
private uint Timer;
|
||||
|
||||
private Vector2 MoveOF = Vector2.Zero;
|
||||
|
||||
@ -112,12 +119,45 @@ namespace DroneSimulator
|
||||
return drone;
|
||||
}
|
||||
|
||||
private void ThreadFunction()
|
||||
public static void StartThread()
|
||||
{
|
||||
if (DroneThread != null) return;
|
||||
|
||||
DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
|
||||
DroneThread.Priority = ThreadPriority.Highest;
|
||||
DroneThread.Start();
|
||||
}
|
||||
|
||||
public static void StopThread()
|
||||
{
|
||||
if (DroneThread == null) return;
|
||||
|
||||
DroneThread = null;
|
||||
}
|
||||
|
||||
private static void ThreadFunction()
|
||||
{
|
||||
long last = Stopwatch.GetTimestamp();
|
||||
long prev = Stopwatch.GetTimestamp();
|
||||
|
||||
while (DroneThread != null)
|
||||
{
|
||||
Action(Environment.TickCount);
|
||||
Thread.Sleep(1);
|
||||
if(!Boost) Thread.Yield();
|
||||
|
||||
long tick = Stopwatch.GetTimestamp();
|
||||
|
||||
if (tick == prev) continue;
|
||||
|
||||
Timing = Stopwatch.Frequency / (tick - prev);
|
||||
prev = tick;
|
||||
|
||||
if (tick < last + Stopwatch.Frequency / Freq) continue;
|
||||
|
||||
last = tick;
|
||||
|
||||
lock (AllDrones)
|
||||
foreach (Drone drone in AllDrones)
|
||||
drone.Action((uint)(tick / (Stopwatch.Frequency / 1000)));
|
||||
}
|
||||
}
|
||||
|
||||
@ -129,10 +169,6 @@ namespace DroneSimulator
|
||||
SpdXYZ = Vector3.Zero;
|
||||
AccXYZ = Vector3.Zero;
|
||||
Quat = Quaternion.Identity;
|
||||
|
||||
DroneThread = new Thread(new ThreadStart(ThreadFunction));
|
||||
Timer = Environment.TickCount;
|
||||
DroneThread.Start();
|
||||
}
|
||||
|
||||
public int Create()
|
||||
@ -142,11 +178,6 @@ namespace DroneSimulator
|
||||
return ID;
|
||||
}
|
||||
|
||||
public void Close()
|
||||
{
|
||||
DroneThread = null;
|
||||
}
|
||||
|
||||
private float GetAngle(float a1, float a2, float az)
|
||||
{
|
||||
if (a2 == 0.0f && az == 0.0f)
|
||||
@ -188,9 +219,13 @@ namespace DroneSimulator
|
||||
return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
|
||||
}
|
||||
|
||||
public void Action(int tick)
|
||||
public void Action(uint tick)
|
||||
{
|
||||
float time = (tick - Timer) / 1000.0f;
|
||||
uint period = tick - Timer;
|
||||
|
||||
if (period <= 0) return;
|
||||
|
||||
float time = period / 1000.0f;
|
||||
Timer = tick;
|
||||
|
||||
if (!Active) return;
|
||||
@ -202,8 +237,6 @@ namespace DroneSimulator
|
||||
flow += flow * 0.1f; // Воздушная подушка
|
||||
}
|
||||
|
||||
|
||||
|
||||
float wind_x = 0, wind_y = 0, wind_z = 0;
|
||||
float wind_p = 0, wind_r = 0, wind_w = 0;
|
||||
|
||||
@ -304,14 +337,14 @@ namespace DroneSimulator
|
||||
MoveOF.X += SpdXYZ.X - Gyr.Y;
|
||||
MoveOF.Y += SpdXYZ.Y + Gyr.X;
|
||||
|
||||
RealAcc.Update(Acc, (uint)tick);
|
||||
RealGyr.Update(Gyr, (uint)tick);
|
||||
RealRange.Update(LaserRange, (uint)tick);
|
||||
RealBar.Update(PosXYZ.Z, (uint)tick);
|
||||
RealPos.Update(PosXYZ, (uint)tick);
|
||||
RealOF.Update(MoveOF, LaserRange, (uint)tick);
|
||||
RealAcc.Update(Acc, tick);
|
||||
RealGyr.Update(Gyr, tick);
|
||||
RealRange.Update(LaserRange, tick);
|
||||
RealBar.Update(PosXYZ.Z, tick);
|
||||
RealPos.Update(PosXYZ, tick);
|
||||
RealOF.Update(MoveOF, LaserRange, tick);
|
||||
|
||||
DataTimer = (uint)tick;
|
||||
DataTimer = tick;
|
||||
}
|
||||
|
||||
private float Range(float pow)
|
||||
@ -347,7 +380,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 = (uint)Environment.TickCount;
|
||||
acc.Head.Time = (uint)(Stopwatch.GetTimestamp() / (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;
|
||||
@ -362,7 +395,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 = (uint)Environment.TickCount;
|
||||
gyr.Head.Time = (uint)(Stopwatch.GetTimestamp() / (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;
|
||||
@ -377,7 +410,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 = (uint)Environment.TickCount;
|
||||
mag.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
|
||||
mag.Time = DataTimer;
|
||||
@ -392,7 +425,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 = (uint)Environment.TickCount;
|
||||
range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
range.LiDAR = RealRange.result;
|
||||
range.Time = RealRange.timer;
|
||||
@ -407,7 +440,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 = (uint)Environment.TickCount;
|
||||
local.Head.Time = (uint)(Stopwatch.GetTimestamp() / (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;
|
||||
@ -422,7 +455,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 = (uint)Environment.TickCount;
|
||||
bar.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
bar.Pressure = RealBar.result;
|
||||
bar.Time = RealBar.timer;
|
||||
@ -437,7 +470,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 = (uint)Environment.TickCount;
|
||||
of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
of.X = RealOF.result.X;
|
||||
of.Y = RealOF.result.Y;
|
||||
@ -455,7 +488,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 = (uint)Environment.TickCount;
|
||||
gps.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
GPS.Point p = new GPS.Point();
|
||||
p.x = PosXYZ.Y; p.y= PosXYZ.X;
|
||||
@ -491,13 +524,25 @@ 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 = (uint)Environment.TickCount;
|
||||
quat.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
|
||||
|
||||
return getBytes(quat);
|
||||
}
|
||||
|
||||
private byte[] SendPingPong()
|
||||
{
|
||||
DroneData.DataHead head = new DroneData.DataHead();
|
||||
|
||||
head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
|
||||
head.Mode = DroneData.DataMode.Response;
|
||||
head.Type = DroneData.DataType.None;
|
||||
head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
|
||||
return getBytes(head);
|
||||
}
|
||||
|
||||
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
|
||||
{
|
||||
byte[] zero = Array.Empty<byte>();
|
||||
@ -537,6 +582,8 @@ namespace DroneSimulator
|
||||
case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero;
|
||||
|
||||
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
|
||||
|
||||
case DroneData.DataType.None: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
|
||||
}
|
||||
|
||||
return zero;
|
||||
|
||||
Reference in New Issue
Block a user