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

+++

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-06-28 02:42:27 +03:00
parent 12e518af0e
commit 782d24a8e6
4 changed files with 148 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
DroneSimulator/Drone.cs
View File

@ -1,8 +1,7 @@
using System.Collections.Concurrent;
using System.Net.Sockets;
using System.Diagnostics;
using System.Numerics;
using System.Runtime.InteropServices;
using static System.Net.Mime.MediaTypeNames;
namespace DroneSimulator
{
@ -10,10 +9,12 @@ namespace DroneSimulator
{
public int ID;
public bool Active; // Живой?
private Socket? Client = null;
public bool Active = false; // Живой?
public const float Dynamic = 10; // Динамика вращения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион
public Vector3 PosXYZ = Vector3.Zero, SpdXYZ = Vector3.Zero, AccXYZ = Vector3.Zero; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat = Quaternion.Identity; // Основной кватернион
public float Power = 0; // Тяга всех двигателей (0-1)
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
@ -23,8 +24,6 @@ namespace DroneSimulator
public Vector4 Orientation;
private uint DataTimer;
private const float Gravity = 9.8f;
private const float TO_GRAD = 180 / MathF.PI;
@ -32,12 +31,13 @@ namespace DroneSimulator
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 StepTime = 0;
private uint Timer;
public static Semaphore StepSemaphore = new Semaphore(0, 10);
private uint Timer = 0;
public static bool TimeLimit = false;
private Vector2 MoveOF = Vector2.Zero;
private uint CountOF = 0;
@ -56,6 +56,11 @@ namespace DroneSimulator
private RealMode.Range RealRange = new RealMode.Range();
private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
private static uint Tick()
{
return (uint)(Stopwatch.GetTimestamp() / Stopwatch.Frequency / 1000);
}
public static byte[] getBytes(object data)
{
int size = Marshal.SizeOf(data);
@ -139,42 +144,37 @@ namespace DroneSimulator
private static void ThreadFunction()
{
long last = Stopwatch.GetTimestamp();
long prev = Stopwatch.GetTimestamp();
while (DroneThread != null)
{
if(!Boost) Thread.Yield();
if (StepSemaphore.WaitOne(1))
{
lock (AllDrones)
{
foreach (Drone drone in AllDrones)
{
if (drone.StepTime > 0)
{
uint tick = Tick();
uint step = drone.Timer + drone.StepTime;
long tick = Stopwatch.GetTimestamp();
if (TimeLimit && (step > tick)) step = tick;
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)));
drone.Action(step);
drone.StepTime = 0;
drone.SendStep(step);
break;
}
}
}
}
}
}
public Drone(int id)
public Drone(int id, Socket? client)
{
ID = id;
Active = false;
PosXYZ = Vector3.Zero;
SpdXYZ = Vector3.Zero;
AccXYZ = Vector3.Zero;
Quat = Quaternion.Identity;
Timer = Tick();
Client = client;
}
public int Create()
@ -229,7 +229,7 @@ namespace DroneSimulator
{
uint period = tick - Timer;
if (period <= 0) return;
if (period == 0) return;
float time = period / 1000.0f;
Timer = tick;
@ -356,9 +356,6 @@ namespace DroneSimulator
MoveOF += RealOF.result;
CountOF += 1;
}
DataTimer = tick;
}
private float Range(float pow)
@ -380,6 +377,31 @@ namespace DroneSimulator
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
}
private void SendStep(uint time)
{
DroneData.Step step = new DroneData.Step();
step.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
step.Head.Mode = DroneData.DataMode.Response;
step.Head.Type = DroneData.DataType.Step;
step.Head.Time = Tick();
step.StepTime = time;
byte[] bytes = getBytes(step);
try { Client?.Send(bytes); } catch { }
}
private void RecvStep(byte[] data)
{
DroneData.Step step = (DroneData.Step)fromBytes(data, typeof(DroneData.Step));
StepTime = step.StepTime;
StepSemaphore.Release();
}
private void RecvDataMotor4(byte[] data)
{
DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
@ -394,7 +416,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 = Tick();
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
acc.Time = RealAcc.timer;
@ -409,7 +431,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 = Tick();
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
gyr.Time = RealGyr.timer;
@ -424,10 +446,10 @@ 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 = Tick();
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
mag.Time = DataTimer;
mag.Time = Timer;
return getBytes(mag);
}
@ -439,7 +461,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 = Tick();
range.LiDAR = RealRange.result;
range.Time = RealRange.timer;
@ -454,7 +476,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 = Tick();
local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
local.Time = RealPos.timer;
@ -469,7 +491,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 = Tick();
bar.Pressure = RealBar.result;
bar.Time = RealBar.timer;
@ -484,7 +506,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
of.Head.Time = Tick();
lock (this)
{
@ -506,7 +528,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
gps.Head.Time = Tick();
GPS.Point p = new GPS.Point();
p.x = RealPos.result.Y; p.y= RealPos.result.X;
@ -544,7 +566,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 = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
quat.Head.Time = Tick();
quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
@ -557,8 +579,8 @@ namespace DroneSimulator
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));
head.Type = DroneData.DataType.Ping;
head.Time = Tick();
return getBytes(head);
}
@ -569,6 +591,8 @@ namespace DroneSimulator
switch (head.Type)
{
case DroneData.DataType.Step: if (head.Mode == DroneData.DataMode.Request) RecvStep(body); return zero;
case DroneData.DataType.DataAcc:
{
if (head.Mode == DroneData.DataMode.Request)
@ -603,7 +627,7 @@ namespace DroneSimulator
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;
case DroneData.DataType.Ping: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
}
return zero;
@ -612,7 +636,7 @@ namespace DroneSimulator
private const int DroneStreamCount = 512;
private byte[] DroneStreamData = new byte[DroneStreamCount];
private int DroneStreamIndex = 0;
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.Ping };
public List<byte[]?>? DataStream(byte[]? data, int size)
{