From 39c81a227bd780b8cabec9d6cc2fc125ddacc44d Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Fri, 27 Jun 2025 11:45:17 +0300 Subject: [PATCH] +++ --- 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; } }