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-25 02:05:32 +03:00
parent 0d8b03ef9a
commit f4044e3939
3 changed files with 232 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
DroneSimulator/Drone.cs
View File

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

108
DroneSimulator/FormMain.Designer.cs generated
View File

@ -34,6 +34,13 @@
pictureBox_2D = new PictureBox();
tabControl_Menu = new TabControl();
tabPage_Main = new TabPage();
groupBox6 = new GroupBox();
checkBox_Freq_Boost = new CheckBox();
label84 = new Label();
numericUpDown_Timing_Freq = new NumericUpDown();
label83 = new Label();
label_Timing = new Label();
label79 = new Label();
groupBox_Visual = new GroupBox();
numericUpDown_Visual_Limit = new NumericUpDown();
label1 = new Label();
@ -221,6 +228,8 @@
((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
tabControl_Menu.SuspendLayout();
tabPage_Main.SuspendLayout();
groupBox6.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Timing_Freq).BeginInit();
groupBox_Visual.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Port).BeginInit();
@ -334,6 +343,7 @@
//
// tabPage_Main
//
tabPage_Main.Controls.Add(groupBox6);
tabPage_Main.Controls.Add(groupBox_Visual);
tabPage_Main.Controls.Add(groupBox_Clients);
tabPage_Main.Location = new Point(4, 24);
@ -345,6 +355,80 @@
tabPage_Main.Text = "Main";
tabPage_Main.UseVisualStyleBackColor = true;
//
// groupBox6
//
groupBox6.Controls.Add(checkBox_Freq_Boost);
groupBox6.Controls.Add(label84);
groupBox6.Controls.Add(numericUpDown_Timing_Freq);
groupBox6.Controls.Add(label83);
groupBox6.Controls.Add(label_Timing);
groupBox6.Controls.Add(label79);
groupBox6.Dock = DockStyle.Top;
groupBox6.Location = new Point(3, 174);
groupBox6.Name = "groupBox6";
groupBox6.Size = new Size(204, 75);
groupBox6.TabIndex = 3;
groupBox6.TabStop = false;
groupBox6.Text = "Frequency";
//
// checkBox_Freq_Boost
//
checkBox_Freq_Boost.AutoSize = true;
checkBox_Freq_Boost.Location = new Point(148, 18);
checkBox_Freq_Boost.Name = "checkBox_Freq_Boost";
checkBox_Freq_Boost.Size = new Size(56, 19);
checkBox_Freq_Boost.TabIndex = 11;
checkBox_Freq_Boost.Text = "boost";
checkBox_Freq_Boost.UseVisualStyleBackColor = true;
checkBox_Freq_Boost.CheckedChanged += numericUpDown_Timing_Freq_ValueChanged;
//
// label84
//
label84.AutoSize = true;
label84.Location = new Point(127, 43);
label84.Name = "label84";
label84.Size = new Size(21, 15);
label84.TabIndex = 10;
label84.Text = "Hz";
//
// numericUpDown_Timing_Freq
//
numericUpDown_Timing_Freq.Location = new Point(70, 41);
numericUpDown_Timing_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Timing_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Timing_Freq.Name = "numericUpDown_Timing_Freq";
numericUpDown_Timing_Freq.Size = new Size(51, 23);
numericUpDown_Timing_Freq.TabIndex = 9;
numericUpDown_Timing_Freq.Value = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Timing_Freq.ValueChanged += numericUpDown_Timing_Freq_ValueChanged;
//
// label83
//
label83.AutoSize = true;
label83.Location = new Point(7, 43);
label83.Name = "label83";
label83.Size = new Size(57, 15);
label83.TabIndex = 2;
label83.Text = "Required:";
//
// label_Timing
//
label_Timing.AutoSize = true;
label_Timing.Location = new Point(70, 19);
label_Timing.Name = "label_Timing";
label_Timing.Size = new Size(13, 15);
label_Timing.TabIndex = 1;
label_Timing.Text = "0";
//
// label79
//
label79.AutoSize = true;
label79.Location = new Point(6, 19);
label79.Name = "label79";
label79.Size = new Size(58, 15);
label79.TabIndex = 0;
label79.Text = "Available:";
//
// groupBox_Visual
//
groupBox_Visual.Controls.Add(numericUpDown_Visual_Limit);
@ -691,7 +775,7 @@
// numericUpDown_Range_Freq
//
numericUpDown_Range_Freq.Location = new Point(70, 16);
numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Range_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Range_Freq.Name = "numericUpDown_Range_Freq";
numericUpDown_Range_Freq.Size = new Size(40, 23);
@ -939,7 +1023,7 @@
// numericUpDown_OF_Freq
//
numericUpDown_OF_Freq.Location = new Point(69, 22);
numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_OF_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Freq.Name = "numericUpDown_OF_Freq";
numericUpDown_OF_Freq.Size = new Size(40, 23);
@ -1108,7 +1192,7 @@
// numericUpDown_Bar_Freq
//
numericUpDown_Bar_Freq.Location = new Point(68, 42);
numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Bar_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Bar_Freq.Name = "numericUpDown_Bar_Freq";
numericUpDown_Bar_Freq.Size = new Size(40, 23);
@ -1232,7 +1316,7 @@
// numericUpDown_Pos_Freq
//
numericUpDown_Pos_Freq.Location = new Point(69, 17);
numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Pos_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Pos_Freq.Name = "numericUpDown_Pos_Freq";
numericUpDown_Pos_Freq.Size = new Size(40, 23);
@ -1307,7 +1391,7 @@
// numericUpDown_Mag_Freq
//
numericUpDown_Mag_Freq.Location = new Point(37, 18);
numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Mag_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Mag_Freq.Name = "numericUpDown_Mag_Freq";
numericUpDown_Mag_Freq.Size = new Size(40, 23);
@ -1499,7 +1583,7 @@
// numericUpDown_Gyr_Freq
//
numericUpDown_Gyr_Freq.Location = new Point(37, 19);
numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Gyr_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Gyr_Freq.Name = "numericUpDown_Gyr_Freq";
numericUpDown_Gyr_Freq.Size = new Size(40, 23);
@ -1658,7 +1742,7 @@
// numericUpDown_Acc_Freq
//
numericUpDown_Acc_Freq.Location = new Point(37, 16);
numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Acc_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Acc_Freq.Name = "numericUpDown_Acc_Freq";
numericUpDown_Acc_Freq.Size = new Size(40, 23);
@ -2387,6 +2471,9 @@
((System.ComponentModel.ISupportInitialize)pictureBox_2D).EndInit();
tabControl_Menu.ResumeLayout(false);
tabPage_Main.ResumeLayout(false);
groupBox6.ResumeLayout(false);
groupBox6.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Timing_Freq).EndInit();
groupBox_Visual.ResumeLayout(false);
groupBox_Visual.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).EndInit();
@ -2666,5 +2753,12 @@
private Label label78;
private NumericUpDown numericUpDown_Physics_Power;
private Label label77;
private GroupBox groupBox6;
private Label label_Timing;
private Label label79;
private Label label84;
private NumericUpDown numericUpDown_Timing_Freq;
private Label label83;
private CheckBox checkBox_Freq_Boost;
}
}

76
DroneSimulator/FormMain.cs
View File

@ -17,8 +17,6 @@ namespace DroneSimulator
NetServerClients netServerClient = new NetServerClients();
NetServerVisual netServerVisual = new NetServerVisual();
List<Drone> AllDrones = new List<Drone>();
public Form_Main()
{
InitializeComponent();
@ -51,20 +49,24 @@ namespace DroneSimulator
screen2D.CreateDrone(Color.Red, data.ID);
AllDrones.Add(drone);
lock (Drone.AllDrones) Drone.AllDrones.Add(drone);
}
else
{
foreach (Drone drone in AllDrones)
Drone? d = null;
lock (Drone.AllDrones)
{
if (drone.ID != data.ID) continue;
drone.Close();
screen2D.RemoveDrone(data.ID);
AllDrones.Remove(drone);
break;
foreach (Drone drone in Drone.AllDrones)
{
if (drone.ID != data.ID) continue;
d = drone;
Drone.AllDrones.Remove(drone);
break;
}
}
if (d != null) screen2D.RemoveDrone(d.ID);
}
}
@ -74,11 +76,14 @@ namespace DroneSimulator
Drone? drone = null;
foreach (Drone d in AllDrones)
lock (Drone.AllDrones)
{
if (d.ID != data.ID) continue;
drone = d;
break;
foreach (Drone d in Drone.AllDrones)
{
if (d.ID != data.ID) continue;
drone = d;
break;
}
}
if (drone == null) return;
@ -123,6 +128,8 @@ namespace DroneSimulator
if (done != NetServerClients.ServerState.Start) return;
Drone.StartThread();
pictureBox_2D.Image = null;
screen2D = new Screen2D(DrawCallback);
@ -155,23 +162,24 @@ namespace DroneSimulator
try
{
foreach (Drone d in AllDrones)
lock (Drone.AllDrones)
{
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
foreach (Drone d in Drone.AllDrones)
{
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
listBox_Drones.Items.Add(line);
listBox_Drones.Items.Add(line);
}
}
}
catch { }
label_Timing.Text = Drone.Timing.ToString() + " Hz";
screen2D.DrawScene();
}
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
{
foreach (Drone d in AllDrones) d.Close();
}
private void VisualConnectionCallback(object o)
{
@ -198,12 +206,15 @@ namespace DroneSimulator
int index = 0;
foreach (Drone d in AllDrones)
lock (Drone.AllDrones)
{
VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
foreach (Drone d in Drone.AllDrones)
{
VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++);
try { data.Client.Send(Drone.getBytes(v)); }
catch { }
try { data.Client.Send(Drone.getBytes(v)); }
catch { }
}
}
}
@ -352,5 +363,16 @@ namespace DroneSimulator
Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value;
Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value;
}
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
{
Drone.StopThread();
}
private void numericUpDown_Timing_Freq_ValueChanged(object sender, EventArgs e)
{
Drone.Freq = (long)numericUpDown_Timing_Freq.Value;
Drone.Boost = checkBox_Freq_Boost.Checked;
}
}
}