revert e30f1f0f35
revert Merge branch 'main' of https://git.skbkit.ru/CPL/Simulator
This commit is contained in:
@ -1,6 +1,4 @@
|
||||
using System.Collections.Concurrent;
|
||||
using System.Diagnostics;
|
||||
using System.Numerics;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
using static System.Net.Mime.MediaTypeNames;
|
||||
|
||||
@ -30,17 +28,10 @@ namespace DroneSimulator
|
||||
private const float TO_GRAD = 180 / MathF.PI;
|
||||
private const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
public static List<Drone> AllDrones = new List<Drone>();
|
||||
private static Thread? DroneThread = null;
|
||||
public static long Timing = 0;
|
||||
public static long Lag = 0;
|
||||
public static long Freq = 1000;
|
||||
public static bool Boost = false;
|
||||
|
||||
private uint Timer;
|
||||
private Thread DroneThread;
|
||||
private int Timer;
|
||||
|
||||
private Vector2 MoveOF = Vector2.Zero;
|
||||
private uint CountOF = 0;
|
||||
|
||||
public struct Physics
|
||||
{
|
||||
@ -121,49 +112,12 @@ namespace DroneSimulator
|
||||
return drone;
|
||||
}
|
||||
|
||||
public static void StartThread()
|
||||
private void ThreadFunction()
|
||||
{
|
||||
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)
|
||||
{
|
||||
if(!Boost) Thread.Yield();
|
||||
|
||||
long tick = Stopwatch.GetTimestamp();
|
||||
|
||||
if (tick == prev) continue;
|
||||
|
||||
Timing = Stopwatch.Frequency / (tick - prev);
|
||||
prev = tick;
|
||||
|
||||
long quant = Stopwatch.Frequency / Freq;
|
||||
|
||||
if (tick < last + quant) continue;
|
||||
|
||||
if (tick > (last + quant) + quant * 0.1) Lag++;
|
||||
|
||||
last = tick;
|
||||
|
||||
lock (AllDrones)
|
||||
foreach (Drone drone in AllDrones)
|
||||
drone.Action((uint)(tick / (Stopwatch.Frequency / 1000)));
|
||||
Action(Environment.TickCount);
|
||||
Thread.Sleep(1);
|
||||
}
|
||||
}
|
||||
|
||||
@ -175,6 +129,10 @@ 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()
|
||||
@ -184,6 +142,11 @@ 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)
|
||||
@ -225,13 +188,9 @@ 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(uint tick)
|
||||
public void Action(int tick)
|
||||
{
|
||||
uint period = tick - Timer;
|
||||
|
||||
if (period <= 0) return;
|
||||
|
||||
float time = period / 1000.0f;
|
||||
float time = (tick - Timer) / 1000.0f;
|
||||
Timer = tick;
|
||||
|
||||
if (!Active) return;
|
||||
@ -243,6 +202,8 @@ 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;
|
||||
|
||||
@ -280,49 +241,39 @@ namespace DroneSimulator
|
||||
if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
|
||||
if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
|
||||
|
||||
/*if (PosXYZ.Z < 0)
|
||||
if (PosXYZ.Z < 0)
|
||||
{
|
||||
SpdPRY = Vector3.Zero;
|
||||
SpdXYZ.X = 0;
|
||||
SpdXYZ.Y = 0;
|
||||
Quat = Quaternion.Identity;
|
||||
}
|
||||
else */
|
||||
Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
|
||||
else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
|
||||
|
||||
Vector4 ori = GetOrientation();
|
||||
|
||||
Orientation = ori;
|
||||
float range = 0;
|
||||
|
||||
if (PosXYZ.Z <= 0)
|
||||
{
|
||||
PosXYZ.Z = 0;
|
||||
SpdXYZ.Z = 0;
|
||||
LaserRange = 0;
|
||||
Acc = new Vector3(0, 0, 1);
|
||||
}
|
||||
|
||||
/*if (PosXYZ.Z < 0)
|
||||
if (PosXYZ.Z < 0)
|
||||
{
|
||||
PosXYZ.Z = 0;
|
||||
|
||||
//if (SpdXYZ.Z < -5)
|
||||
//{
|
||||
// Active = false; // Сильно ударился о землю
|
||||
//}
|
||||
/*if (SpdXYZ.Z < -5)
|
||||
{
|
||||
Active = false; // Сильно ударился о землю
|
||||
}*/
|
||||
|
||||
//if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
|
||||
//{
|
||||
// Active = false; // Повредил винты при посадке
|
||||
//}
|
||||
/*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
|
||||
{
|
||||
Active = false; // Повредил винты при посадке
|
||||
}*/
|
||||
|
||||
SpdXYZ.Z = 0;
|
||||
|
||||
Acc = new Vector3(0, 0, 1);
|
||||
Gyr = Vector3.Zero;
|
||||
LaserRange = 0;
|
||||
}*/
|
||||
}
|
||||
else
|
||||
{
|
||||
if (ori.W < 0)
|
||||
@ -337,28 +288,21 @@ namespace DroneSimulator
|
||||
|
||||
float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
|
||||
|
||||
range = PosXYZ.Z / MathF.Cos(tilt);
|
||||
|
||||
if (tilt < 90 && ori.W > 0) LaserRange = range;
|
||||
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
|
||||
else LaserRange = float.MaxValue;
|
||||
}
|
||||
|
||||
RealAcc.Update(Acc, tick);
|
||||
RealGyr.Update(Gyr, tick);
|
||||
RealRange.Update(LaserRange, tick);
|
||||
RealBar.Update(PosXYZ.Z, tick);
|
||||
RealPos.Update(PosXYZ, tick);
|
||||
MoveOF.X += SpdXYZ.X - Gyr.Y;
|
||||
MoveOF.Y += SpdXYZ.Y + Gyr.X;
|
||||
|
||||
bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
|
||||
RealAcc.Update(Acc, (uint)tick);
|
||||
RealGyr.Update(Gyr, (uint)tick);
|
||||
RealRange.Update(LaserRange, (uint)tick);
|
||||
RealBar.Update(PosXYZ.Z * 11, (uint)tick);
|
||||
RealPos.Update(PosXYZ, (uint)tick);
|
||||
RealOF.Update(MoveOF, LaserRange, (uint)tick);
|
||||
|
||||
if(of) lock (this)
|
||||
{
|
||||
MoveOF += RealOF.result;
|
||||
CountOF += 1;
|
||||
}
|
||||
|
||||
|
||||
DataTimer = tick;
|
||||
DataTimer = (uint)tick;
|
||||
}
|
||||
|
||||
private float Range(float pow)
|
||||
@ -394,7 +338,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
acc.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
|
||||
acc.Time = RealAcc.timer;
|
||||
@ -409,7 +353,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
gyr.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
|
||||
gyr.Time = RealGyr.timer;
|
||||
@ -424,7 +368,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
mag.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
|
||||
mag.Time = DataTimer;
|
||||
@ -439,7 +383,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
range.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
range.LiDAR = RealRange.result;
|
||||
range.Time = RealRange.timer;
|
||||
@ -454,7 +398,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
local.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
|
||||
local.Time = RealPos.timer;
|
||||
@ -469,7 +413,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
bar.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
bar.Pressure = RealBar.result;
|
||||
bar.Time = RealBar.timer;
|
||||
@ -484,17 +428,13 @@ 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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
of.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
lock (this)
|
||||
{
|
||||
of.X = MoveOF.X / CountOF;
|
||||
of.Y = MoveOF.Y / CountOF;
|
||||
of.Time = RealOF.timer;
|
||||
of.X = RealOF.result.X;
|
||||
of.Y = RealOF.result.Y;
|
||||
of.Time = RealBar.timer;
|
||||
|
||||
MoveOF = Vector2.Zero;
|
||||
CountOF = 0;
|
||||
}
|
||||
MoveOF = Vector2.Zero;
|
||||
|
||||
return getBytes(of);
|
||||
}
|
||||
@ -506,10 +446,10 @@ 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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
gps.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
GPS.Point p = new GPS.Point();
|
||||
p.x = RealPos.result.Y; p.y= RealPos.result.X;
|
||||
p.x = PosXYZ.Y; p.y= PosXYZ.X;
|
||||
|
||||
GPS.GlobalCoords g = new GPS.GlobalCoords();
|
||||
g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
|
||||
@ -519,7 +459,7 @@ namespace DroneSimulator
|
||||
gps.Lat = g.latitude; gps.Lon = g.longitude;
|
||||
|
||||
gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
|
||||
gps.Alt = GPS.Home.Alt + RealPos.result.Z;
|
||||
gps.Alt = GPS.Home.Alt + PosXYZ.Z;
|
||||
|
||||
DateTime tim = DateTime.Now;
|
||||
gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
|
||||
@ -532,8 +472,6 @@ namespace DroneSimulator
|
||||
gps.Vdop = GPS.State.Vdop;
|
||||
gps.Pdop = GPS.State.Pdop;
|
||||
|
||||
gps.Time = RealPos.timer;
|
||||
|
||||
return getBytes(gps);
|
||||
}
|
||||
|
||||
@ -544,25 +482,13 @@ 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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
|
||||
quat.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
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>();
|
||||
@ -602,8 +528,6 @@ 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