From 3e4973a129fbeafaf19d5ea90452eb51d220fcc4 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Thu, 19 Jun 2025 01:07:53 +0300 Subject: [PATCH 1/7] Update Drone.cs --- DroneSimulator/Drone.cs | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index f5b3186..cee859c 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -241,32 +241,40 @@ 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; - if (PosXYZ.Z < 0) + if(PosXYZ.Z < 0) + { + PosXYZ.Z = 0; + SpdXYZ.Z = 0; + LaserRange = 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; @@ -274,7 +282,7 @@ namespace DroneSimulator Gyr = Vector3.Zero; LaserRange = 0; } - else + else*/ { if (ori.W < 0) { From 4b78b7d146543c6877065fa9478fa68a9c101c96 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Mon, 23 Jun 2025 02:35:31 +0300 Subject: [PATCH 2/7] Update Drone.cs --- DroneSimulator/Drone.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index cee859c..5031cd1 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -255,11 +255,12 @@ namespace DroneSimulator Orientation = ori; - if(PosXYZ.Z < 0) + if(PosXYZ.Z <= 0) { PosXYZ.Z = 0; SpdXYZ.Z = 0; LaserRange = 0; + Acc = new Vector3(0, 0, 1); } /*if (PosXYZ.Z < 0) @@ -281,8 +282,8 @@ namespace DroneSimulator Acc = new Vector3(0, 0, 1); Gyr = Vector3.Zero; LaserRange = 0; - } - else*/ + }*/ + else { if (ori.W < 0) { From 0d8b03ef9a056ea2ba1d682de14da247a5b08c60 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Tue, 24 Jun 2025 03:50:23 +0300 Subject: [PATCH 3/7] +++ --- DroneSimulator/Drone.cs | 2 +- DroneSimulator/RealMode.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 5031cd1..9beeb6d 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -307,7 +307,7 @@ namespace DroneSimulator RealAcc.Update(Acc, (uint)tick); RealGyr.Update(Gyr, (uint)tick); RealRange.Update(LaserRange, (uint)tick); - RealBar.Update(PosXYZ.Z * 11, (uint)tick); + RealBar.Update(PosXYZ.Z, (uint)tick); RealPos.Update(PosXYZ, (uint)tick); RealOF.Update(MoveOF, LaserRange, (uint)tick); diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs index ce26081..df3af2a 100644 --- a/DroneSimulator/RealMode.cs +++ b/DroneSimulator/RealMode.cs @@ -225,7 +225,7 @@ namespace DroneSimulator public void Update(float value, uint time) { - value = Pressure - value; + value = Pressure - value * 12.15f; if (!Enable) { From f4044e3939ff4b0b4ab59798ff78763238fc3fc2 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Wed, 25 Jun 2025 02:05:32 +0300 Subject: [PATCH 4/7] +++ --- DroneSimulator/Drone.cs | 117 +++++++++++++++++++--------- DroneSimulator/FormMain.Designer.cs | 108 +++++++++++++++++++++++-- DroneSimulator/FormMain.cs | 76 +++++++++++------- 3 files changed, 232 insertions(+), 69 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 9beeb6d..ebd4442 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -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 AllDrones = new List(); + 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(); @@ -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; diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index 0b9eac1..bdd322c 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -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; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 90cd72f..d21b8b8 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -17,8 +17,6 @@ namespace DroneSimulator NetServerClients netServerClient = new NetServerClients(); NetServerVisual netServerVisual = new NetServerVisual(); - List AllDrones = new List(); - 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; + } } } From c763581ebbf1df8d00fb20f2be3ef33dca08a999 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Fri, 27 Jun 2025 02:01:41 +0300 Subject: [PATCH 5/7] Update Drone.cs --- DroneSimulator/Drone.cs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index ebd4442..6e9d371 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -474,7 +474,7 @@ namespace DroneSimulator of.X = RealOF.result.X; of.Y = RealOF.result.Y; - of.Time = RealBar.timer; + of.Time = RealOF.timer; MoveOF = Vector2.Zero; @@ -491,7 +491,7 @@ namespace DroneSimulator gps.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); GPS.Point p = new GPS.Point(); - p.x = PosXYZ.Y; p.y= PosXYZ.X; + p.x = RealPos.result.Y; p.y= RealPos.result.X; GPS.GlobalCoords g = new GPS.GlobalCoords(); g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon; @@ -501,7 +501,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 + PosXYZ.Z; + gps.Alt = GPS.Home.Alt + RealPos.result.Z; DateTime tim = DateTime.Now; gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000; @@ -514,6 +514,8 @@ namespace DroneSimulator gps.Vdop = GPS.State.Vdop; gps.Pdop = GPS.State.Pdop; + gps.Time = RealPos.timer; + return getBytes(gps); } From 39c81a227bd780b8cabec9d6cc2fc125ddacc44d Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Fri, 27 Jun 2025 11:45:17 +0300 Subject: [PATCH 6/7] +++ --- DroneSimulator/Drone.cs | 33 +++++-- DroneSimulator/FormMain.Designer.cs | 137 ++++++++++------------------ DroneSimulator/FormMain.cs | 6 +- DroneSimulator/RealMode.cs | 25 ++--- 4 files changed, 81 insertions(+), 120 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 6e9d371..045bb61 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -33,12 +33,14 @@ namespace DroneSimulator public static List AllDrones = new List(); 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); } diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index bdd322c..9e828a6 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -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; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index d21b8b8..6787643 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -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) diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs index df3af2a..176cca2 100644 --- a/DroneSimulator/RealMode.cs +++ b/DroneSimulator/RealMode.cs @@ -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; } } From 12e518af0e041cd1570174cbebd80027bc594b30 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Fri, 27 Jun 2025 12:12:24 +0300 Subject: [PATCH 7/7] Update Drone.cs --- DroneSimulator/Drone.cs | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 045bb61..2548ff4 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -293,8 +293,9 @@ namespace DroneSimulator Vector4 ori = GetOrientation(); Orientation = ori; + float range = 0; - if(PosXYZ.Z <= 0) + if (PosXYZ.Z <= 0) { PosXYZ.Z = 0; SpdXYZ.Z = 0; @@ -336,19 +337,19 @@ namespace DroneSimulator float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI; - if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt); + range = PosXYZ.Z / MathF.Cos(tilt); + + if (tilt < 90 && ori.W > 0) LaserRange = range; 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); - bool of = RealOF.Update(new Vector2(SpdXYZ.X - Gyr.Y, SpdXYZ.Y + Gyr.X), LaserRange, tick); + bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick); if(of) lock (this) {