From 3565372ffc005de4a69d893883085bed4cd96d4a Mon Sep 17 00:00:00 2001 From: sasklyarov Date: Mon, 4 Aug 2025 18:02:42 +0000 Subject: [PATCH] revert e30f1f0f3597ffb781d7967c869fbe95fa631b45 revert Merge branch 'main' of https://git.skbkit.ru/CPL/Simulator --- DroneSimulator/Drone.cs | 188 +++++++--------------- DroneSimulator/FormMain.Designer.cs | 239 +++++++++++----------------- DroneSimulator/FormMain.cs | 82 ++++------ DroneSimulator/RealMode.cs | 27 ++-- 4 files changed, 200 insertions(+), 336 deletions(-) diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 2548ff4..f5b3186 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -1,6 +1,4 @@ -using System.Collections.Concurrent; -using System.Diagnostics; -using System.Numerics; +using System.Numerics; using System.Runtime.InteropServices; using static System.Net.Mime.MediaTypeNames; @@ -30,17 +28,10 @@ namespace DroneSimulator private const float TO_GRAD = 180 / MathF.PI; private const float TO_RADI = MathF.PI / 180; - 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 Thread DroneThread; + private int Timer; private Vector2 MoveOF = Vector2.Zero; - private uint CountOF = 0; public struct Physics { @@ -121,49 +112,12 @@ namespace DroneSimulator return drone; } - public static void StartThread() + private void ThreadFunction() { - 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) { - if(!Boost) Thread.Yield(); - - long tick = Stopwatch.GetTimestamp(); - - if (tick == prev) continue; - - Timing = Stopwatch.Frequency / (tick - prev); - prev = tick; - - long quant = Stopwatch.Frequency / Freq; - - if (tick < last + quant) continue; - - if (tick > (last + quant) + quant * 0.1) Lag++; - - last = tick; - - lock (AllDrones) - foreach (Drone drone in AllDrones) - drone.Action((uint)(tick / (Stopwatch.Frequency / 1000))); + Action(Environment.TickCount); + Thread.Sleep(1); } } @@ -175,6 +129,10 @@ 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() @@ -184,6 +142,11 @@ 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) @@ -225,13 +188,9 @@ 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(uint tick) + public void Action(int tick) { - uint period = tick - Timer; - - if (period <= 0) return; - - float time = period / 1000.0f; + float time = (tick - Timer) / 1000.0f; Timer = tick; if (!Active) return; @@ -243,6 +202,8 @@ 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; @@ -280,49 +241,39 @@ 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; - float range = 0; - if (PosXYZ.Z <= 0) - { - PosXYZ.Z = 0; - SpdXYZ.Z = 0; - LaserRange = 0; - Acc = new Vector3(0, 0, 1); - } - - /*if (PosXYZ.Z < 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; Acc = new Vector3(0, 0, 1); Gyr = Vector3.Zero; LaserRange = 0; - }*/ + } else { if (ori.W < 0) @@ -337,28 +288,21 @@ namespace DroneSimulator float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI; - range = PosXYZ.Z / MathF.Cos(tilt); - - if (tilt < 90 && ori.W > 0) LaserRange = range; + if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt); 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); + MoveOF.X += SpdXYZ.X - Gyr.Y; + MoveOF.Y += SpdXYZ.Y + Gyr.X; - bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick); + RealAcc.Update(Acc, (uint)tick); + RealGyr.Update(Gyr, (uint)tick); + RealRange.Update(LaserRange, (uint)tick); + RealBar.Update(PosXYZ.Z * 11, (uint)tick); + RealPos.Update(PosXYZ, (uint)tick); + RealOF.Update(MoveOF, LaserRange, (uint)tick); - if(of) lock (this) - { - MoveOF += RealOF.result; - CountOF += 1; - } - - - DataTimer = tick; + DataTimer = (uint)tick; } private float Range(float pow) @@ -394,7 +338,7 @@ namespace DroneSimulator acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc)); acc.Head.Mode = DroneData.DataMode.Response; acc.Head.Type = DroneData.DataType.DataAcc; - acc.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + acc.Head.Time = (uint)Environment.TickCount; acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z; acc.Time = RealAcc.timer; @@ -409,7 +353,7 @@ namespace DroneSimulator gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr)); gyr.Head.Mode = DroneData.DataMode.Response; gyr.Head.Type = DroneData.DataType.DataGyr; - gyr.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + gyr.Head.Time = (uint)Environment.TickCount; gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z; gyr.Time = RealGyr.timer; @@ -424,7 +368,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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + mag.Head.Time = (uint)Environment.TickCount; mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0; mag.Time = DataTimer; @@ -439,7 +383,7 @@ namespace DroneSimulator range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange)); range.Head.Mode = DroneData.DataMode.Response; range.Head.Type = DroneData.DataType.DataRange; - range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + range.Head.Time = (uint)Environment.TickCount; range.LiDAR = RealRange.result; range.Time = RealRange.timer; @@ -454,7 +398,7 @@ namespace DroneSimulator local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal)); local.Head.Mode = DroneData.DataMode.Response; local.Head.Type = DroneData.DataType.DataLocal; - local.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + local.Head.Time = (uint)Environment.TickCount; local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z; local.Time = RealPos.timer; @@ -469,7 +413,7 @@ namespace DroneSimulator bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar)); bar.Head.Mode = DroneData.DataMode.Response; bar.Head.Type = DroneData.DataType.DataBar; - bar.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + bar.Head.Time = (uint)Environment.TickCount; bar.Pressure = RealBar.result; bar.Time = RealBar.timer; @@ -484,17 +428,13 @@ namespace DroneSimulator of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF)); of.Head.Mode = DroneData.DataMode.Response; of.Head.Type = DroneData.DataType.DataOF; - of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + of.Head.Time = (uint)Environment.TickCount; - lock (this) - { - of.X = MoveOF.X / CountOF; - of.Y = MoveOF.Y / CountOF; - of.Time = RealOF.timer; + of.X = RealOF.result.X; + of.Y = RealOF.result.Y; + of.Time = RealBar.timer; - MoveOF = Vector2.Zero; - CountOF = 0; - } + MoveOF = Vector2.Zero; return getBytes(of); } @@ -506,10 +446,10 @@ namespace DroneSimulator gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS)); gps.Head.Mode = DroneData.DataMode.Response; gps.Head.Type = DroneData.DataType.DataGPS; - gps.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + gps.Head.Time = (uint)Environment.TickCount; GPS.Point p = new GPS.Point(); - p.x = RealPos.result.Y; p.y= RealPos.result.X; + p.x = PosXYZ.Y; p.y= PosXYZ.X; GPS.GlobalCoords g = new GPS.GlobalCoords(); g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon; @@ -519,7 +459,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 + RealPos.result.Z; + gps.Alt = GPS.Home.Alt + PosXYZ.Z; DateTime tim = DateTime.Now; gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000; @@ -532,8 +472,6 @@ namespace DroneSimulator gps.Vdop = GPS.State.Vdop; gps.Pdop = GPS.State.Pdop; - gps.Time = RealPos.timer; - return getBytes(gps); } @@ -544,25 +482,13 @@ namespace DroneSimulator quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat)); quat.Head.Mode = DroneData.DataMode.Response; quat.Head.Type = DroneData.DataType.DataQuat; - quat.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000)); + quat.Head.Time = (uint)Environment.TickCount; 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(); @@ -602,8 +528,6 @@ 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 9e828a6..0b9eac1 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -34,13 +34,6 @@ 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(); @@ -78,9 +71,14 @@ 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(); @@ -219,14 +217,10 @@ 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(); 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(); @@ -242,7 +236,9 @@ ((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(); @@ -307,7 +303,7 @@ groupBox_Screen.Dock = DockStyle.Fill; groupBox_Screen.Location = new Point(218, 0); groupBox_Screen.Name = "groupBox_Screen"; - groupBox_Screen.Size = new Size(466, 781); + groupBox_Screen.Size = new Size(466, 816); groupBox_Screen.TabIndex = 1; groupBox_Screen.TabStop = false; // @@ -317,7 +313,7 @@ pictureBox_2D.Dock = DockStyle.Fill; pictureBox_2D.Location = new Point(3, 19); pictureBox_2D.Name = "pictureBox_2D"; - pictureBox_2D.Size = new Size(460, 759); + pictureBox_2D.Size = new Size(460, 794); pictureBox_2D.SizeMode = PictureBoxSizeMode.Zoom; pictureBox_2D.TabIndex = 0; pictureBox_2D.TabStop = false; @@ -333,99 +329,22 @@ tabControl_Menu.Location = new Point(0, 0); tabControl_Menu.Name = "tabControl_Menu"; tabControl_Menu.SelectedIndex = 0; - tabControl_Menu.Size = new Size(218, 781); + tabControl_Menu.Size = new Size(218, 816); tabControl_Menu.TabIndex = 2; // // 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); tabPage_Main.Name = "tabPage_Main"; tabPage_Main.Padding = new Padding(3); - tabPage_Main.Size = new Size(210, 753); + tabPage_Main.Size = new Size(210, 788); tabPage_Main.TabIndex = 0; tabPage_Main.Tag = "#main"; tabPage_Main.Text = "Main"; tabPage_Main.UseVisualStyleBackColor = true; // - // 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(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(116, 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(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"; - 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); @@ -609,7 +528,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, 753); + tabPage_Model.Size = new Size(210, 788); tabPage_Model.TabIndex = 1; tabPage_Model.Tag = "#model"; tabPage_Model.Text = "Model"; @@ -627,7 +546,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, 747); + panel_Menu_Model.Size = new Size(204, 782); panel_Menu_Model.TabIndex = 5; // // groupBox1 @@ -647,7 +566,7 @@ groupBox1.Controls.Add(label46); groupBox1.Controls.Add(label47); groupBox1.Dock = DockStyle.Top; - groupBox1.Location = new Point(0, 623); + groupBox1.Location = new Point(0, 652); groupBox1.Name = "groupBox1"; groupBox1.Size = new Size(204, 126); groupBox1.TabIndex = 8; @@ -772,7 +691,7 @@ // numericUpDown_Range_Freq // numericUpDown_Range_Freq.Location = new Point(70, 16); - numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -804,9 +723,14 @@ 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); @@ -820,7 +744,7 @@ groupBox_OF.Dock = DockStyle.Top; groupBox_OF.Location = new Point(0, 489); groupBox_OF.Name = "groupBox_OF"; - groupBox_OF.Size = new Size(204, 134); + groupBox_OF.Size = new Size(204, 163); groupBox_OF.TabIndex = 4; groupBox_OF.TabStop = false; groupBox_OF.Text = "Optical flow"; @@ -859,10 +783,31 @@ 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(114, 107); + label53.Location = new Point(113, 132); label53.Name = "label53"; label53.Size = new Size(24, 15); label53.TabIndex = 32; @@ -872,7 +817,7 @@ // numericUpDown_OF_Laten.DecimalPlaces = 2; numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); - numericUpDown_OF_Laten.Location = new Point(67, 105); + numericUpDown_OF_Laten.Location = new Point(66, 130); 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); @@ -883,12 +828,41 @@ // label54 // label54.AutoSize = true; - label54.Location = new Point(4, 107); + label54.Location = new Point(3, 132); 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); @@ -904,7 +878,7 @@ checkBox_OF_Enable.AutoSize = true; checkBox_OF_Enable.Checked = true; checkBox_OF_Enable.CheckState = CheckState.Checked; - checkBox_OF_Enable.Location = new Point(160, 109); + checkBox_OF_Enable.Location = new Point(159, 134); checkBox_OF_Enable.Name = "checkBox_OF_Enable"; checkBox_OF_Enable.Size = new Size(39, 19); checkBox_OF_Enable.TabIndex = 22; @@ -965,7 +939,7 @@ // numericUpDown_OF_Freq // numericUpDown_OF_Freq.Location = new Point(69, 22); - numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1134,7 +1108,7 @@ // numericUpDown_Bar_Freq // numericUpDown_Bar_Freq.Location = new Point(68, 42); - numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1258,7 +1232,7 @@ // numericUpDown_Pos_Freq // numericUpDown_Pos_Freq.Location = new Point(69, 17); - numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1333,7 +1307,7 @@ // numericUpDown_Mag_Freq // numericUpDown_Mag_Freq.Location = new Point(37, 18); - numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1525,7 +1499,7 @@ // numericUpDown_Gyr_Freq // numericUpDown_Gyr_Freq.Location = new Point(37, 19); - numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1684,7 +1658,7 @@ // numericUpDown_Acc_Freq // numericUpDown_Acc_Freq.Location = new Point(37, 16); - numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 200, 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); @@ -1707,7 +1681,7 @@ tabPage_Area.Controls.Add(groupBox3); tabPage_Area.Location = new Point(4, 24); tabPage_Area.Name = "tabPage_Area"; - tabPage_Area.Size = new Size(210, 753); + tabPage_Area.Size = new Size(210, 788); tabPage_Area.TabIndex = 2; tabPage_Area.Tag = "#area"; tabPage_Area.Text = "Area"; @@ -1958,7 +1932,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, 753); + tabPage_GPS.Size = new Size(210, 788); tabPage_GPS.TabIndex = 3; tabPage_GPS.Text = "GPS"; tabPage_GPS.UseVisualStyleBackColor = true; @@ -2216,7 +2190,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, 753); + tabPage_Drone.Size = new Size(210, 788); tabPage_Drone.TabIndex = 4; tabPage_Drone.Text = "Drone"; tabPage_Drone.UseVisualStyleBackColor = true; @@ -2337,7 +2311,7 @@ groupBox_Navi.Dock = DockStyle.Right; groupBox_Navi.Location = new Point(684, 0); groupBox_Navi.Name = "groupBox_Navi"; - groupBox_Navi.Size = new Size(200, 781); + groupBox_Navi.Size = new Size(200, 816); groupBox_Navi.TabIndex = 3; groupBox_Navi.TabStop = false; groupBox_Navi.Tag = "#navigation"; @@ -2351,7 +2325,7 @@ panel1.Dock = DockStyle.Fill; panel1.Location = new Point(3, 42); panel1.Name = "panel1"; - panel1.Size = new Size(194, 736); + panel1.Size = new Size(194, 771); panel1.TabIndex = 3; // // listBox_Drones @@ -2396,34 +2370,16 @@ 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, 781); + ClientSize = new Size(884, 816); Controls.Add(groupBox_Screen); Controls.Add(groupBox_Navi); Controls.Add(tabControl_Menu); Icon = (Icon)resources.GetObject("$this.Icon"); - MinimumSize = new Size(900, 820); + MinimumSize = new Size(900, 855); Name = "Form_Main"; Text = "Drone Simulator V1.0"; FormClosing += Form_Main_FormClosing; @@ -2431,9 +2387,6 @@ ((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(); @@ -2453,7 +2406,9 @@ 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(); @@ -2608,6 +2563,9 @@ 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; @@ -2641,6 +2599,8 @@ 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; @@ -2706,14 +2666,5 @@ 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; - private Label label_Timing_Lag; - private Label label37; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 6787643..90cd72f 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -17,6 +17,8 @@ namespace DroneSimulator NetServerClients netServerClient = new NetServerClients(); NetServerVisual netServerVisual = new NetServerVisual(); + List AllDrones = new List(); + public Form_Main() { InitializeComponent(); @@ -49,24 +51,20 @@ namespace DroneSimulator screen2D.CreateDrone(Color.Red, data.ID); - lock (Drone.AllDrones) Drone.AllDrones.Add(drone); + AllDrones.Add(drone); } else { - Drone? d = null; - - lock (Drone.AllDrones) + foreach (Drone drone in AllDrones) { - foreach (Drone drone in Drone.AllDrones) - { - if (drone.ID != data.ID) continue; - d = drone; - Drone.AllDrones.Remove(drone); - break; - } - } + if (drone.ID != data.ID) continue; + drone.Close(); - if (d != null) screen2D.RemoveDrone(d.ID); + screen2D.RemoveDrone(data.ID); + + AllDrones.Remove(drone); + break; + } } } @@ -76,14 +74,11 @@ namespace DroneSimulator Drone? drone = null; - lock (Drone.AllDrones) + foreach (Drone d in AllDrones) { - foreach (Drone d in Drone.AllDrones) - { - if (d.ID != data.ID) continue; - drone = d; - break; - } + if (d.ID != data.ID) continue; + drone = d; + break; } if (drone == null) return; @@ -128,8 +123,6 @@ namespace DroneSimulator if (done != NetServerClients.ServerState.Start) return; - Drone.StartThread(); - pictureBox_2D.Image = null; screen2D = new Screen2D(DrawCallback); @@ -162,25 +155,23 @@ namespace DroneSimulator try { - lock (Drone.AllDrones) + foreach (Drone d in AllDrones) { - foreach (Drone d in Drone.AllDrones) - { - screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation()); + 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"; - label_Timing_Lag.Text = Drone.Lag.ToString(); - screen2D.DrawScene(); } + private void Form_Main_FormClosing(object sender, FormClosingEventArgs e) + { + foreach (Drone d in AllDrones) d.Close(); + } private void VisualConnectionCallback(object o) { @@ -207,15 +198,12 @@ namespace DroneSimulator int index = 0; - lock (Drone.AllDrones) + foreach (Drone d in AllDrones) { - foreach (Drone d in Drone.AllDrones) - { - VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++); + VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++); - try { data.Client.Send(Drone.getBytes(v)); } - catch { } - } + try { data.Client.Send(Drone.getBytes(v)); } + catch { } } } @@ -318,8 +306,11 @@ 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; + RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10; 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) @@ -361,16 +352,5 @@ 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; - } } } diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs index 176cca2..ce26081 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 * 12.15f; + value = Pressure - value; if (!Enable) { @@ -273,6 +273,8 @@ 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; @@ -288,21 +290,31 @@ namespace DroneSimulator public uint timer = 0; public Vector2 result; - public bool Update(Vector2 value, float Range, uint time) + public void Update(Vector2 value, float Range, uint time) { - value *= Lens; - if (!Enable) { result = Vector2.NaN; - return true; + return; } if (!RealSimulation) { result = value; timer = time; - return true; + return; + } + + value *= Lens; + + if (rand.Next(0, 1000) < (Error * 10)) + { + value = Vector2.Zero; + delay = time + Wait; + } + else if (delay > time) + { + value = Vector2.Zero; } if (Range > MaxHeight) value = Vector2.Zero; @@ -332,10 +344,7 @@ namespace DroneSimulator { result = value; timer = time; - return true; } - - return false; } }