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-27 11:45:17 +03:00
parent c763581ebb
commit 39c81a227b
4 changed files with 81 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
DroneSimulator/Drone.cs
View File

@ -33,12 +33,14 @@ 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 Timer;
private Vector2 MoveOF = Vector2.Zero;
private uint CountOF = 0;
public struct Physics
{
@ -151,7 +153,11 @@ namespace DroneSimulator
Timing = Stopwatch.Frequency / (tick - prev);
prev = tick;
if (tick < last + Stopwatch.Frequency / Freq) continue;
long quant = Stopwatch.Frequency / Freq;
if (tick < last + quant) continue;
if (tick > (last + quant) + quant * 0.1) Lag++;
last = tick;
@ -334,15 +340,22 @@ namespace DroneSimulator
else LaserRange = float.MaxValue;
}
MoveOF.X += SpdXYZ.X - Gyr.Y;
MoveOF.Y += SpdXYZ.Y + Gyr.X;
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);
bool of = RealOF.Update(new Vector2(SpdXYZ.X - Gyr.Y, SpdXYZ.Y + Gyr.X), LaserRange, tick);
if(of) lock (this)
{
MoveOF += RealOF.result;
CountOF += 1;
}
DataTimer = tick;
}
@ -472,11 +485,15 @@ namespace DroneSimulator
of.Head.Type = DroneData.DataType.DataOF;
of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
of.X = RealOF.result.X;
of.Y = RealOF.result.Y;
of.Time = RealOF.timer;
lock (this)
{
of.X = MoveOF.X / CountOF;
of.Y = MoveOF.Y / CountOF;
of.Time = RealOF.timer;
MoveOF = Vector2.Zero;
MoveOF = Vector2.Zero;
CountOF = 0;
}
return getBytes(of);
}

137
DroneSimulator/FormMain.Designer.cs generated
View File

@ -78,14 +78,9 @@
checkBox_Model_OF_Real = new CheckBox();
label40 = new Label();
numericUpDown_OF_Lens = new NumericUpDown();
label39 = new Label();
numericUpDown_OF_Wait = new NumericUpDown();
label53 = new Label();
numericUpDown_OF_Laten = new NumericUpDown();
label54 = new Label();
numericUpDown_OF_Error = new NumericUpDown();
label38 = new Label();
label37 = new Label();
numericUpDown_OF_Len = new NumericUpDown();
checkBox_OF_Enable = new CheckBox();
label17 = new Label();
@ -224,6 +219,8 @@
comboBox_Drone_Rotor = new ComboBox();
comboBox_Drone = new ComboBox();
timer_Test = new System.Windows.Forms.Timer(components);
label37 = new Label();
label_Timing_Lag = new Label();
groupBox_Screen.SuspendLayout();
((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
tabControl_Menu.SuspendLayout();
@ -245,9 +242,7 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).BeginInit();
groupBox_OF.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit();
@ -312,7 +307,7 @@
groupBox_Screen.Dock = DockStyle.Fill;
groupBox_Screen.Location = new Point(218, 0);
groupBox_Screen.Name = "groupBox_Screen";
groupBox_Screen.Size = new Size(466, 816);
groupBox_Screen.Size = new Size(466, 781);
groupBox_Screen.TabIndex = 1;
groupBox_Screen.TabStop = false;
//
@ -322,7 +317,7 @@
pictureBox_2D.Dock = DockStyle.Fill;
pictureBox_2D.Location = new Point(3, 19);
pictureBox_2D.Name = "pictureBox_2D";
pictureBox_2D.Size = new Size(460, 794);
pictureBox_2D.Size = new Size(460, 759);
pictureBox_2D.SizeMode = PictureBoxSizeMode.Zoom;
pictureBox_2D.TabIndex = 0;
pictureBox_2D.TabStop = false;
@ -338,7 +333,7 @@
tabControl_Menu.Location = new Point(0, 0);
tabControl_Menu.Name = "tabControl_Menu";
tabControl_Menu.SelectedIndex = 0;
tabControl_Menu.Size = new Size(218, 816);
tabControl_Menu.Size = new Size(218, 781);
tabControl_Menu.TabIndex = 2;
//
// tabPage_Main
@ -349,7 +344,7 @@
tabPage_Main.Location = new Point(4, 24);
tabPage_Main.Name = "tabPage_Main";
tabPage_Main.Padding = new Padding(3);
tabPage_Main.Size = new Size(210, 788);
tabPage_Main.Size = new Size(210, 753);
tabPage_Main.TabIndex = 0;
tabPage_Main.Tag = "#main";
tabPage_Main.Text = "Main";
@ -357,9 +352,11 @@
//
// groupBox6
//
groupBox6.Controls.Add(label_Timing_Lag);
groupBox6.Controls.Add(numericUpDown_Timing_Freq);
groupBox6.Controls.Add(label37);
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);
@ -385,7 +382,7 @@
// label84
//
label84.AutoSize = true;
label84.Location = new Point(127, 43);
label84.Location = new Point(116, 43);
label84.Name = "label84";
label84.Size = new Size(21, 15);
label84.TabIndex = 10;
@ -393,7 +390,7 @@
//
// numericUpDown_Timing_Freq
//
numericUpDown_Timing_Freq.Location = new Point(70, 41);
numericUpDown_Timing_Freq.Location = new Point(64, 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";
@ -612,7 +609,7 @@
tabPage_Model.Location = new Point(4, 24);
tabPage_Model.Name = "tabPage_Model";
tabPage_Model.Padding = new Padding(3);
tabPage_Model.Size = new Size(210, 788);
tabPage_Model.Size = new Size(210, 753);
tabPage_Model.TabIndex = 1;
tabPage_Model.Tag = "#model";
tabPage_Model.Text = "Model";
@ -630,7 +627,7 @@
panel_Menu_Model.Dock = DockStyle.Fill;
panel_Menu_Model.Location = new Point(3, 3);
panel_Menu_Model.Name = "panel_Menu_Model";
panel_Menu_Model.Size = new Size(204, 782);
panel_Menu_Model.Size = new Size(204, 747);
panel_Menu_Model.TabIndex = 5;
//
// groupBox1
@ -650,7 +647,7 @@
groupBox1.Controls.Add(label46);
groupBox1.Controls.Add(label47);
groupBox1.Dock = DockStyle.Top;
groupBox1.Location = new Point(0, 652);
groupBox1.Location = new Point(0, 623);
groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(204, 126);
groupBox1.TabIndex = 8;
@ -807,14 +804,9 @@
groupBox_OF.Controls.Add(checkBox_Model_OF_Real);
groupBox_OF.Controls.Add(label40);
groupBox_OF.Controls.Add(numericUpDown_OF_Lens);
groupBox_OF.Controls.Add(label39);
groupBox_OF.Controls.Add(numericUpDown_OF_Wait);
groupBox_OF.Controls.Add(label53);
groupBox_OF.Controls.Add(numericUpDown_OF_Laten);
groupBox_OF.Controls.Add(label54);
groupBox_OF.Controls.Add(numericUpDown_OF_Error);
groupBox_OF.Controls.Add(label38);
groupBox_OF.Controls.Add(label37);
groupBox_OF.Controls.Add(numericUpDown_OF_Len);
groupBox_OF.Controls.Add(checkBox_OF_Enable);
groupBox_OF.Controls.Add(label17);
@ -828,7 +820,7 @@
groupBox_OF.Dock = DockStyle.Top;
groupBox_OF.Location = new Point(0, 489);
groupBox_OF.Name = "groupBox_OF";
groupBox_OF.Size = new Size(204, 163);
groupBox_OF.Size = new Size(204, 134);
groupBox_OF.TabIndex = 4;
groupBox_OF.TabStop = false;
groupBox_OF.Text = "Optical flow";
@ -867,31 +859,10 @@
numericUpDown_OF_Lens.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Lens.ValueChanged += numericUpDown_OF_Update;
//
// label39
//
label39.AutoSize = true;
label39.Location = new Point(168, 105);
label39.Name = "label39";
label39.Size = new Size(24, 15);
label39.TabIndex = 34;
label39.Text = "sec";
//
// numericUpDown_OF_Wait
//
numericUpDown_OF_Wait.DecimalPlaces = 2;
numericUpDown_OF_Wait.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.Location = new Point(119, 103);
numericUpDown_OF_Wait.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Wait.Name = "numericUpDown_OF_Wait";
numericUpDown_OF_Wait.Size = new Size(47, 23);
numericUpDown_OF_Wait.TabIndex = 33;
numericUpDown_OF_Wait.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.ValueChanged += numericUpDown_OF_Update;
//
// label53
//
label53.AutoSize = true;
label53.Location = new Point(113, 132);
label53.Location = new Point(114, 107);
label53.Name = "label53";
label53.Size = new Size(24, 15);
label53.TabIndex = 32;
@ -901,7 +872,7 @@
//
numericUpDown_OF_Laten.DecimalPlaces = 2;
numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_OF_Laten.Location = new Point(66, 130);
numericUpDown_OF_Laten.Location = new Point(67, 105);
numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten";
numericUpDown_OF_Laten.Size = new Size(41, 23);
@ -912,41 +883,12 @@
// label54
//
label54.AutoSize = true;
label54.Location = new Point(3, 132);
label54.Location = new Point(4, 107);
label54.Name = "label54";
label54.Size = new Size(55, 15);
label54.TabIndex = 30;
label54.Text = "Lateness:";
//
// numericUpDown_OF_Error
//
numericUpDown_OF_Error.DecimalPlaces = 1;
numericUpDown_OF_Error.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Error.Location = new Point(38, 103);
numericUpDown_OF_Error.Name = "numericUpDown_OF_Error";
numericUpDown_OF_Error.Size = new Size(47, 23);
numericUpDown_OF_Error.TabIndex = 24;
numericUpDown_OF_Error.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Error.ValueChanged += numericUpDown_OF_Update;
//
// label38
//
label38.AutoSize = true;
label38.Location = new Point(83, 105);
label38.Name = "label38";
label38.Size = new Size(17, 15);
label38.TabIndex = 25;
label38.Text = "%";
//
// label37
//
label37.AutoSize = true;
label37.Location = new Point(1, 105);
label37.Name = "label37";
label37.Size = new Size(35, 15);
label37.TabIndex = 23;
label37.Text = "Error:";
//
// numericUpDown_OF_Len
//
numericUpDown_OF_Len.Location = new Point(68, 76);
@ -962,7 +904,7 @@
checkBox_OF_Enable.AutoSize = true;
checkBox_OF_Enable.Checked = true;
checkBox_OF_Enable.CheckState = CheckState.Checked;
checkBox_OF_Enable.Location = new Point(159, 134);
checkBox_OF_Enable.Location = new Point(160, 109);
checkBox_OF_Enable.Name = "checkBox_OF_Enable";
checkBox_OF_Enable.Size = new Size(39, 19);
checkBox_OF_Enable.TabIndex = 22;
@ -1765,7 +1707,7 @@
tabPage_Area.Controls.Add(groupBox3);
tabPage_Area.Location = new Point(4, 24);
tabPage_Area.Name = "tabPage_Area";
tabPage_Area.Size = new Size(210, 788);
tabPage_Area.Size = new Size(210, 753);
tabPage_Area.TabIndex = 2;
tabPage_Area.Tag = "#area";
tabPage_Area.Text = "Area";
@ -2016,7 +1958,7 @@
tabPage_GPS.Location = new Point(4, 24);
tabPage_GPS.Name = "tabPage_GPS";
tabPage_GPS.Padding = new Padding(3);
tabPage_GPS.Size = new Size(210, 788);
tabPage_GPS.Size = new Size(210, 753);
tabPage_GPS.TabIndex = 3;
tabPage_GPS.Text = "GPS";
tabPage_GPS.UseVisualStyleBackColor = true;
@ -2274,7 +2216,7 @@
tabPage_Drone.Location = new Point(4, 24);
tabPage_Drone.Name = "tabPage_Drone";
tabPage_Drone.Padding = new Padding(3);
tabPage_Drone.Size = new Size(210, 788);
tabPage_Drone.Size = new Size(210, 753);
tabPage_Drone.TabIndex = 4;
tabPage_Drone.Text = "Drone";
tabPage_Drone.UseVisualStyleBackColor = true;
@ -2395,7 +2337,7 @@
groupBox_Navi.Dock = DockStyle.Right;
groupBox_Navi.Location = new Point(684, 0);
groupBox_Navi.Name = "groupBox_Navi";
groupBox_Navi.Size = new Size(200, 816);
groupBox_Navi.Size = new Size(200, 781);
groupBox_Navi.TabIndex = 3;
groupBox_Navi.TabStop = false;
groupBox_Navi.Tag = "#navigation";
@ -2409,7 +2351,7 @@
panel1.Dock = DockStyle.Fill;
panel1.Location = new Point(3, 42);
panel1.Name = "panel1";
panel1.Size = new Size(194, 771);
panel1.Size = new Size(194, 736);
panel1.TabIndex = 3;
//
// listBox_Drones
@ -2454,16 +2396,34 @@
timer_Test.Interval = 10;
timer_Test.Tick += timer_Test_Tick;
//
// label37
//
label37.AutoSize = true;
label37.Location = new Point(143, 43);
label37.Name = "label37";
label37.Size = new Size(29, 15);
label37.TabIndex = 12;
label37.Text = "Lag:";
//
// label_Timing_Lag
//
label_Timing_Lag.AutoSize = true;
label_Timing_Lag.Location = new Point(168, 43);
label_Timing_Lag.Name = "label_Timing_Lag";
label_Timing_Lag.Size = new Size(13, 15);
label_Timing_Lag.TabIndex = 13;
label_Timing_Lag.Text = "0";
//
// Form_Main
//
AutoScaleDimensions = new SizeF(7F, 15F);
AutoScaleMode = AutoScaleMode.Font;
ClientSize = new Size(884, 816);
ClientSize = new Size(884, 781);
Controls.Add(groupBox_Screen);
Controls.Add(groupBox_Navi);
Controls.Add(tabControl_Menu);
Icon = (Icon)resources.GetObject("$this.Icon");
MinimumSize = new Size(900, 855);
MinimumSize = new Size(900, 820);
Name = "Form_Main";
Text = "Drone Simulator V1.0";
FormClosing += Form_Main_FormClosing;
@ -2493,9 +2453,7 @@
groupBox_OF.ResumeLayout(false);
groupBox_OF.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit();
@ -2650,9 +2608,6 @@
private Label label34;
private Label label36;
private Label label35;
private NumericUpDown numericUpDown_OF_Error;
private Label label37;
private Label label38;
private GroupBox groupBox1;
private CheckBox checkBox_Range_Enable;
private NumericUpDown numericUpDown_Range_Max;
@ -2686,8 +2641,6 @@
private NumericUpDown numericUpDown_Range_Laten;
private Label label56;
private ListBox listBox_Drones;
private Label label39;
private NumericUpDown numericUpDown_OF_Wait;
private Label label40;
private NumericUpDown numericUpDown_OF_Lens;
private GroupBox groupBox3;
@ -2760,5 +2713,7 @@
private NumericUpDown numericUpDown_Timing_Freq;
private Label label83;
private CheckBox checkBox_Freq_Boost;
private Label label_Timing_Lag;
private Label label37;
}
}

6
DroneSimulator/FormMain.cs
View File

@ -177,6 +177,7 @@ namespace DroneSimulator
catch { }
label_Timing.Text = Drone.Timing.ToString() + " Hz";
label_Timing_Lag.Text = Drone.Lag.ToString();
screen2D.DrawScene();
}
@ -317,11 +318,8 @@ namespace DroneSimulator
RealMode.OpticalFlow.Lateness = (float)numericUpDown_OF_Laten.Value;
RealMode.OpticalFlow.Enable = checkBox_OF_Enable.Checked;
RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10;
RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value;
RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
RealMode.OpticalFlow.Error = (float)numericUpDown_OF_Error.Value * 10;
RealMode.OpticalFlow.Wait = (uint)numericUpDown_OF_Wait.Value * 1000;
}
private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)

25
DroneSimulator/RealMode.cs
View File

@ -273,8 +273,6 @@ namespace DroneSimulator
public static uint Freq;
public static float Noise;
public static float Lateness;
public static float Error;
public static uint Wait;
public static float Lens;
public static bool RealSimulation;
@ -290,31 +288,21 @@ namespace DroneSimulator
public uint timer = 0;
public Vector2 result;
public void Update(Vector2 value, float Range, uint time)
public bool Update(Vector2 value, float Range, uint time)
{
value *= Lens;
if (!Enable)
{
result = Vector2.NaN;
return;
return true;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
value *= Lens;
if (rand.Next(0, 1000) < (Error * 10))
{
value = Vector2.Zero;
delay = time + Wait;
}
else if (delay > time)
{
value = Vector2.Zero;
return true;
}
if (Range > MaxHeight) value = Vector2.Zero;
@ -344,7 +332,10 @@ namespace DroneSimulator
{
result = value;
timer = time;
return true;
}
return false;
}
}