From cdf8c18d9b1a260f37f62738da866ca96427d269 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Tue, 15 Apr 2025 01:23:54 +0300 Subject: [PATCH] Add real mode (bar) --- DroneSimulator/Drone.cs | 24 ++- DroneSimulator/DroneData.cs | 24 +-- DroneSimulator/FormMain.Designer.cs | 280 +++++++++++++----------- DroneSimulator/FormMain.cs | 39 ++-- DroneSimulator/RealMode.cs | 317 ++++++++++++++++++++++++++++ 5 files changed, 525 insertions(+), 159 deletions(-) create mode 100644 DroneSimulator/RealMode.cs diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index a4663ca..6d3c334 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -3,6 +3,7 @@ using System.Numerics; using System.Runtime.InteropServices; using System.Security.Cryptography; using static DroneSimulator.Drone; +using static System.Windows.Forms.VisualStyles.VisualStyleElement; using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar; namespace DroneSimulator @@ -35,11 +36,15 @@ namespace DroneSimulator private Random MainRandom = new Random(); + RealMode.Accelerometer RealAcc = new RealMode.Accelerometer(); + RealMode.Gyroscope RealGyr = new RealMode.Gyroscope(); + RealMode.Position RealPos = new RealMode.Position(); + RealMode.Barometer RealBar = new RealMode.Barometer(); + RealMode.Range RealRange = new RealMode.Range(); + public struct DataBarometer { - public int Value, Pressure; // Значение давления в Паскальях - public float Accuracy; - public int Frequency; + public float Pressure; // Значение давления в Паскальях public uint Time; } @@ -252,13 +257,10 @@ namespace DroneSimulator else LaserRange = float.MaxValue; } - if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer) - { - float bar = dataBarometer.Pressure - PosXYZ.Z * 11; - int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10; - dataBarometer.Value = (int)(bar + rnd); - dataBarometer.Time = DataTimer; - } + float v_bar = PosXYZ.Z * 11; + uint t_bar = (uint)tick; + RealBar.Update(ref v_bar, ref t_bar); + dataBarometer.Pressure = v_bar; DataTimer = (uint)tick; } @@ -373,7 +375,7 @@ namespace DroneSimulator bar.Head.Type = DroneData.DataType.DataBar; bar.Head.Time = (uint)Environment.TickCount; - bar.Pressure = dataBarometer.Value; + bar.Pressure = dataBarometer.Pressure; bar.Time = dataBarometer.Time; return getBytes(bar); diff --git a/DroneSimulator/DroneData.cs b/DroneSimulator/DroneData.cs index 70a7c3d..ac1310f 100644 --- a/DroneSimulator/DroneData.cs +++ b/DroneSimulator/DroneData.cs @@ -1,4 +1,4 @@ -using System.Runtime.InteropServices; +using System.Runtime.InteropServices; namespace DroneData { @@ -25,7 +25,7 @@ namespace DroneData public DataMode Mode; public DataType Type; - public uint Time; // + public uint Time; // Общее время static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); } @@ -37,7 +37,7 @@ namespace DroneData public DataHead Head; public XYZ Acc; - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc)); } @@ -47,7 +47,7 @@ namespace DroneData public DataHead Head; public XYZ Gyr; - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr)); } @@ -57,7 +57,7 @@ namespace DroneData public DataHead Head; public XYZ Mag; - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag)); } @@ -65,9 +65,9 @@ namespace DroneData public struct DataRange { public DataHead Head; - public float LiDAR; // + public float LiDAR; // Датчик посадки - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange)); } @@ -75,9 +75,9 @@ namespace DroneData public struct DataLocal { public DataHead Head; - public XYZ Local; // + public XYZ Local; // Локальные координаты - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal)); } @@ -85,9 +85,9 @@ namespace DroneData public struct DataBar { public DataHead Head; - public int Pressure; // + public float Pressure; // Давление - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); } @@ -107,4 +107,4 @@ namespace DroneData static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); } -} \ No newline at end of file +} diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index ad6dd19..8cc7058 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -56,12 +56,15 @@ tabPage_Model = new TabPage(); panel_Menu_Model = new Panel(); groupBox1 = new GroupBox(); + label55 = new Label(); + numericUpDown18 = new NumericUpDown(); + label56 = new Label(); checkBox1 = new CheckBox(); numericUpDown9 = new NumericUpDown(); label42 = new Label(); label43 = new Label(); label44 = new Label(); - numericUpDown10 = new NumericUpDown(); + numericUpDown_Range_Noise = new NumericUpDown(); label45 = new Label(); numericUpDown11 = new NumericUpDown(); label46 = new Label(); @@ -82,7 +85,7 @@ label17 = new Label(); label16 = new Label(); label12 = new Label(); - numericUpDown_OF_Accur = new NumericUpDown(); + numericUpDown_OF_Noise = new NumericUpDown(); label13 = new Label(); numericUpDown_OF_Freq = new NumericUpDown(); label14 = new Label(); @@ -96,7 +99,7 @@ label18 = new Label(); checkBox_Bar_Enable = new CheckBox(); label9 = new Label(); - numericUpDown_Bar_Accur = new NumericUpDown(); + numericUpDown_Bar_Noise = new NumericUpDown(); label8 = new Label(); numericUpDown_Bar_Freq = new NumericUpDown(); label5 = new Label(); @@ -106,7 +109,7 @@ numericUpDown12 = new NumericUpDown(); label4 = new Label(); label6 = new Label(); - numericUpDown13 = new NumericUpDown(); + numericUpDown_Pos_Noise = new NumericUpDown(); label10 = new Label(); numericUpDown14 = new NumericUpDown(); label11 = new Label(); @@ -147,6 +150,8 @@ label23 = new Label(); numericUpDown_Acc_Freq = new NumericUpDown(); label20 = new Label(); + groupBox2 = new GroupBox(); + checkBox_Mode_Real = new CheckBox(); tabPage_Area = new TabPage(); groupBox_Navi = new GroupBox(); panel1 = new Panel(); @@ -154,9 +159,6 @@ comboBox_Drone_Rotor = new ComboBox(); comboBox_Drone = new ComboBox(); timer_Test = new System.Windows.Forms.Timer(components); - label55 = new Label(); - numericUpDown18 = new NumericUpDown(); - label56 = new Label(); menuStrip_Menu.SuspendLayout(); groupBox_Screen.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit(); @@ -171,23 +173,24 @@ tabPage_Model.SuspendLayout(); panel_Menu_Model.SuspendLayout(); groupBox1.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown18).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown9).BeginInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown10).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown11).BeginInit(); groupBox_OF.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown17).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown8).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown7).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown1).BeginInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Accur).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit(); groupBox_Barometer.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Laten).BeginInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Accur).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Freq).BeginInit(); groupBox_GPS.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown12).BeginInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown13).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown14).BeginInit(); groupBox_Mag.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Noise).BeginInit(); @@ -205,9 +208,9 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown3).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit(); + groupBox2.SuspendLayout(); groupBox_Navi.SuspendLayout(); panel1.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)numericUpDown18).BeginInit(); SuspendLayout(); // // menuStrip_Menu @@ -241,7 +244,7 @@ groupBox_Screen.Dock = DockStyle.Fill; groupBox_Screen.Location = new Point(218, 24); groupBox_Screen.Name = "groupBox_Screen"; - groupBox_Screen.Size = new Size(466, 877); + groupBox_Screen.Size = new Size(466, 917); groupBox_Screen.TabIndex = 1; groupBox_Screen.TabStop = false; // @@ -251,7 +254,7 @@ pictureBox_2D.Dock = DockStyle.Fill; pictureBox_2D.Location = new Point(3, 19); pictureBox_2D.Name = "pictureBox_2D"; - pictureBox_2D.Size = new Size(460, 855); + pictureBox_2D.Size = new Size(460, 895); pictureBox_2D.SizeMode = PictureBoxSizeMode.Zoom; pictureBox_2D.TabIndex = 0; pictureBox_2D.TabStop = false; @@ -265,7 +268,7 @@ tabControl_Menu.Location = new Point(0, 24); tabControl_Menu.Name = "tabControl_Menu"; tabControl_Menu.SelectedIndex = 0; - tabControl_Menu.Size = new Size(218, 877); + tabControl_Menu.Size = new Size(218, 917); tabControl_Menu.TabIndex = 2; // // tabPage_Main @@ -275,7 +278,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, 849); + tabPage_Main.Size = new Size(210, 889); tabPage_Main.TabIndex = 0; tabPage_Main.Tag = "#main"; tabPage_Main.Text = "Main"; @@ -464,7 +467,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, 849); + tabPage_Model.Size = new Size(210, 889); tabPage_Model.TabIndex = 1; tabPage_Model.Tag = "#model"; tabPage_Model.Text = "Model"; @@ -479,10 +482,11 @@ panel_Menu_Model.Controls.Add(groupBox_Mag); panel_Menu_Model.Controls.Add(groupBox_Gyr); panel_Menu_Model.Controls.Add(groupBox_Acc); + panel_Menu_Model.Controls.Add(groupBox2); 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, 843); + panel_Menu_Model.Size = new Size(204, 883); panel_Menu_Model.TabIndex = 5; // // groupBox1 @@ -495,20 +499,49 @@ groupBox1.Controls.Add(label42); groupBox1.Controls.Add(label43); groupBox1.Controls.Add(label44); - groupBox1.Controls.Add(numericUpDown10); + groupBox1.Controls.Add(numericUpDown_Range_Noise); groupBox1.Controls.Add(label45); groupBox1.Controls.Add(numericUpDown11); groupBox1.Controls.Add(label46); groupBox1.Controls.Add(label47); groupBox1.Dock = DockStyle.Top; groupBox1.Enabled = false; - groupBox1.Location = new Point(0, 705); + groupBox1.Location = new Point(0, 749); groupBox1.Name = "groupBox1"; groupBox1.Size = new Size(204, 133); groupBox1.TabIndex = 8; groupBox1.TabStop = false; groupBox1.Text = "Range"; // + // label55 + // + label55.AutoSize = true; + label55.Location = new Point(115, 105); + label55.Name = "label55"; + label55.Size = new Size(24, 15); + label55.TabIndex = 35; + label55.Text = "sec"; + // + // numericUpDown18 + // + numericUpDown18.DecimalPlaces = 2; + numericUpDown18.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); + numericUpDown18.Location = new Point(68, 103); + numericUpDown18.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown18.Name = "numericUpDown18"; + numericUpDown18.Size = new Size(41, 23); + numericUpDown18.TabIndex = 34; + numericUpDown18.Value = new decimal(new int[] { 1, 0, 0, 65536 }); + // + // label56 + // + label56.AutoSize = true; + label56.Location = new Point(5, 105); + label56.Name = "label56"; + label56.Size = new Size(55, 15); + label56.TabIndex = 33; + label56.Text = "Lateness:"; + // // checkBox1 // checkBox1.AutoSize = true; @@ -543,7 +576,7 @@ // label43 // label43.AutoSize = true; - label43.Location = new Point(2, 76); + label43.Location = new Point(0, 76); label43.Name = "label43"; label43.Size = new Size(70, 15); label43.TabIndex = 28; @@ -559,25 +592,25 @@ label44.TabIndex = 27; label44.Text = "m"; // - // numericUpDown10 + // numericUpDown_Range_Noise // - numericUpDown10.DecimalPlaces = 3; - numericUpDown10.Location = new Point(70, 45); - numericUpDown10.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); - numericUpDown10.Name = "numericUpDown10"; - numericUpDown10.Size = new Size(47, 23); - numericUpDown10.TabIndex = 26; - numericUpDown10.Value = new decimal(new int[] { 1, 0, 0, 131072 }); + numericUpDown_Range_Noise.DecimalPlaces = 3; + numericUpDown_Range_Noise.Location = new Point(70, 45); + numericUpDown_Range_Noise.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_Range_Noise.Name = "numericUpDown_Range_Noise"; + numericUpDown_Range_Noise.Size = new Size(47, 23); + numericUpDown_Range_Noise.TabIndex = 26; + numericUpDown_Range_Noise.Value = new decimal(new int[] { 1, 0, 0, 131072 }); // // label45 // label45.AutoSize = true; - label45.Location = new Point(8, 47); + label45.Location = new Point(24, 47); label45.Name = "label45"; - label45.Size = new Size(59, 15); + label45.Size = new Size(40, 15); label45.TabIndex = 25; label45.Tag = "#accuracy"; - label45.Text = "Accuracy:"; + label45.Text = "Noise:"; // // numericUpDown11 // @@ -601,7 +634,7 @@ // label47 // label47.AutoSize = true; - label47.Location = new Point(7, 18); + label47.Location = new Point(2, 18); label47.Name = "label47"; label47.Size = new Size(65, 15); label47.TabIndex = 23; @@ -625,14 +658,14 @@ groupBox_OF.Controls.Add(label17); groupBox_OF.Controls.Add(label16); groupBox_OF.Controls.Add(label12); - groupBox_OF.Controls.Add(numericUpDown_OF_Accur); + groupBox_OF.Controls.Add(numericUpDown_OF_Noise); groupBox_OF.Controls.Add(label13); groupBox_OF.Controls.Add(numericUpDown_OF_Freq); groupBox_OF.Controls.Add(label14); groupBox_OF.Controls.Add(label15); groupBox_OF.Dock = DockStyle.Top; groupBox_OF.Enabled = false; - groupBox_OF.Location = new Point(0, 519); + groupBox_OF.Location = new Point(0, 563); groupBox_OF.Name = "groupBox_OF"; groupBox_OF.Size = new Size(204, 186); groupBox_OF.TabIndex = 4; @@ -768,7 +801,7 @@ // label16 // label16.AutoSize = true; - label16.Location = new Point(1, 82); + label16.Location = new Point(-1, 82); label16.Name = "label16"; label16.Size = new Size(70, 15); label16.TabIndex = 19; @@ -784,25 +817,25 @@ label12.TabIndex = 18; label12.Text = "Deg"; // - // numericUpDown_OF_Accur + // numericUpDown_OF_Noise // - numericUpDown_OF_Accur.DecimalPlaces = 1; - numericUpDown_OF_Accur.Location = new Point(69, 51); - numericUpDown_OF_Accur.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); - numericUpDown_OF_Accur.Name = "numericUpDown_OF_Accur"; - numericUpDown_OF_Accur.Size = new Size(40, 23); - numericUpDown_OF_Accur.TabIndex = 17; - numericUpDown_OF_Accur.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_OF_Noise.DecimalPlaces = 1; + numericUpDown_OF_Noise.Location = new Point(69, 51); + numericUpDown_OF_Noise.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_OF_Noise.Name = "numericUpDown_OF_Noise"; + numericUpDown_OF_Noise.Size = new Size(40, 23); + numericUpDown_OF_Noise.TabIndex = 17; + numericUpDown_OF_Noise.Value = new decimal(new int[] { 1, 0, 0, 0 }); // // label13 // label13.AutoSize = true; label13.Location = new Point(7, 53); label13.Name = "label13"; - label13.Size = new Size(59, 15); + label13.Size = new Size(40, 15); label13.TabIndex = 16; label13.Tag = "#accuracy"; - label13.Text = "Accuracy:"; + label13.Text = "Noise:"; // // numericUpDown_OF_Freq // @@ -843,13 +876,13 @@ groupBox_Barometer.Controls.Add(label18); groupBox_Barometer.Controls.Add(checkBox_Bar_Enable); groupBox_Barometer.Controls.Add(label9); - groupBox_Barometer.Controls.Add(numericUpDown_Bar_Accur); + groupBox_Barometer.Controls.Add(numericUpDown_Bar_Noise); groupBox_Barometer.Controls.Add(label8); groupBox_Barometer.Controls.Add(numericUpDown_Bar_Freq); groupBox_Barometer.Controls.Add(label5); groupBox_Barometer.Controls.Add(label7); groupBox_Barometer.Dock = DockStyle.Top; - groupBox_Barometer.Location = new Point(0, 389); + groupBox_Barometer.Location = new Point(0, 433); groupBox_Barometer.Name = "groupBox_Barometer"; groupBox_Barometer.Size = new Size(204, 130); groupBox_Barometer.TabIndex = 3; @@ -876,11 +909,12 @@ numericUpDown_Bar_Laten.Size = new Size(41, 23); numericUpDown_Bar_Laten.TabIndex = 22; numericUpDown_Bar_Laten.Value = new decimal(new int[] { 3, 0, 0, 65536 }); + numericUpDown_Bar_Laten.ValueChanged += numericUpDown_Bar_Update; // // label29 // label29.AutoSize = true; - label29.Location = new Point(5, 103); + label29.Location = new Point(9, 103); label29.Name = "label29"; label29.Size = new Size(55, 15); label29.TabIndex = 21; @@ -902,6 +936,7 @@ textBox_Bar_Pressure.Size = new Size(61, 23); textBox_Bar_Pressure.TabIndex = 19; textBox_Bar_Pressure.Text = "102258"; + textBox_Bar_Pressure.TextChanged += numericUpDown_Bar_Update; // // label18 // @@ -925,6 +960,7 @@ checkBox_Bar_Enable.Tag = "#en"; checkBox_Bar_Enable.Text = "En"; checkBox_Bar_Enable.UseVisualStyleBackColor = true; + checkBox_Bar_Enable.CheckedChanged += numericUpDown_Bar_Update; // // label9 // @@ -935,26 +971,26 @@ label9.TabIndex = 12; label9.Text = "Pa"; // - // numericUpDown_Bar_Accur + // numericUpDown_Bar_Noise // - numericUpDown_Bar_Accur.DecimalPlaces = 1; - numericUpDown_Bar_Accur.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); - numericUpDown_Bar_Accur.Location = new Point(68, 72); - numericUpDown_Bar_Accur.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); - numericUpDown_Bar_Accur.Name = "numericUpDown_Bar_Accur"; - numericUpDown_Bar_Accur.Size = new Size(40, 23); - numericUpDown_Bar_Accur.TabIndex = 11; - numericUpDown_Bar_Accur.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Bar_Noise.DecimalPlaces = 1; + numericUpDown_Bar_Noise.Location = new Point(68, 72); + numericUpDown_Bar_Noise.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_Bar_Noise.Name = "numericUpDown_Bar_Noise"; + numericUpDown_Bar_Noise.Size = new Size(40, 23); + numericUpDown_Bar_Noise.TabIndex = 11; + numericUpDown_Bar_Noise.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Bar_Noise.ValueChanged += numericUpDown_Bar_Update; // // label8 // label8.AutoSize = true; - label8.Location = new Point(6, 74); + label8.Location = new Point(23, 74); label8.Name = "label8"; - label8.Size = new Size(59, 15); + label8.Size = new Size(40, 15); label8.TabIndex = 10; label8.Tag = "#accuracy"; - label8.Text = "Accuracy:"; + label8.Text = "Noise:"; // // numericUpDown_Bar_Freq // @@ -965,6 +1001,7 @@ numericUpDown_Bar_Freq.Size = new Size(40, 23); numericUpDown_Bar_Freq.TabIndex = 7; numericUpDown_Bar_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 }); + numericUpDown_Bar_Freq.ValueChanged += numericUpDown_Bar_Update; // // label5 // @@ -978,7 +1015,7 @@ // label7 // label7.AutoSize = true; - label7.Location = new Point(5, 45); + label7.Location = new Point(1, 45); label7.Name = "label7"; label7.Size = new Size(65, 15); label7.TabIndex = 8; @@ -991,7 +1028,7 @@ groupBox_GPS.Controls.Add(numericUpDown12); groupBox_GPS.Controls.Add(label4); groupBox_GPS.Controls.Add(label6); - groupBox_GPS.Controls.Add(numericUpDown13); + groupBox_GPS.Controls.Add(numericUpDown_Pos_Noise); groupBox_GPS.Controls.Add(label10); groupBox_GPS.Controls.Add(numericUpDown14); groupBox_GPS.Controls.Add(label11); @@ -999,7 +1036,7 @@ groupBox_GPS.Controls.Add(checkBox_GPS_Enable); groupBox_GPS.Dock = DockStyle.Top; groupBox_GPS.Enabled = false; - groupBox_GPS.Location = new Point(0, 283); + groupBox_GPS.Location = new Point(0, 327); groupBox_GPS.Name = "groupBox_GPS"; groupBox_GPS.Size = new Size(204, 106); groupBox_GPS.TabIndex = 2; @@ -1044,26 +1081,26 @@ label6.TabIndex = 29; label6.Text = "m"; // - // numericUpDown13 + // numericUpDown_Pos_Noise // - numericUpDown13.DecimalPlaces = 1; - numericUpDown13.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); - numericUpDown13.Location = new Point(69, 46); - numericUpDown13.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); - numericUpDown13.Name = "numericUpDown13"; - numericUpDown13.Size = new Size(40, 23); - numericUpDown13.TabIndex = 28; - numericUpDown13.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Pos_Noise.DecimalPlaces = 1; + numericUpDown_Pos_Noise.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_Pos_Noise.Location = new Point(69, 46); + numericUpDown_Pos_Noise.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_Pos_Noise.Name = "numericUpDown_Pos_Noise"; + numericUpDown_Pos_Noise.Size = new Size(40, 23); + numericUpDown_Pos_Noise.TabIndex = 28; + numericUpDown_Pos_Noise.Value = new decimal(new int[] { 1, 0, 0, 65536 }); // // label10 // label10.AutoSize = true; - label10.Location = new Point(7, 48); + label10.Location = new Point(23, 48); label10.Name = "label10"; - label10.Size = new Size(59, 15); + label10.Size = new Size(40, 15); label10.TabIndex = 27; label10.Tag = "#accuracy"; - label10.Text = "Accuracy:"; + label10.Text = "Noise:"; // // numericUpDown14 // @@ -1087,7 +1124,7 @@ // label48 // label48.AutoSize = true; - label48.Location = new Point(6, 19); + label48.Location = new Point(2, 19); label48.Name = "label48"; label48.Size = new Size(65, 15); label48.TabIndex = 25; @@ -1116,7 +1153,7 @@ groupBox_Mag.Controls.Add(label22); groupBox_Mag.Dock = DockStyle.Top; groupBox_Mag.Enabled = false; - groupBox_Mag.Location = new Point(0, 232); + groupBox_Mag.Location = new Point(0, 276); groupBox_Mag.Name = "groupBox_Mag"; groupBox_Mag.Size = new Size(204, 51); groupBox_Mag.TabIndex = 5; @@ -1187,7 +1224,7 @@ groupBox_Gyr.Controls.Add(label21); groupBox_Gyr.Dock = DockStyle.Top; groupBox_Gyr.Enabled = false; - groupBox_Gyr.Location = new Point(0, 110); + groupBox_Gyr.Location = new Point(0, 154); groupBox_Gyr.Name = "groupBox_Gyr"; groupBox_Gyr.Size = new Size(204, 122); groupBox_Gyr.TabIndex = 7; @@ -1359,7 +1396,7 @@ groupBox_Acc.Controls.Add(label20); groupBox_Acc.Dock = DockStyle.Top; groupBox_Acc.Enabled = false; - groupBox_Acc.Location = new Point(0, 0); + groupBox_Acc.Location = new Point(0, 44); groupBox_Acc.Name = "groupBox_Acc"; groupBox_Acc.Size = new Size(204, 110); groupBox_Acc.TabIndex = 6; @@ -1485,11 +1522,33 @@ label20.TabIndex = 2; label20.Text = "Freq:"; // + // groupBox2 + // + groupBox2.Controls.Add(checkBox_Mode_Real); + groupBox2.Dock = DockStyle.Top; + groupBox2.Location = new Point(0, 0); + groupBox2.Name = "groupBox2"; + groupBox2.Size = new Size(204, 44); + groupBox2.TabIndex = 9; + groupBox2.TabStop = false; + groupBox2.Text = "Mode"; + // + // checkBox_Mode_Real + // + checkBox_Mode_Real.AutoSize = true; + checkBox_Mode_Real.Location = new Point(10, 22); + checkBox_Mode_Real.Name = "checkBox_Mode_Real"; + checkBox_Mode_Real.Size = new Size(107, 19); + checkBox_Mode_Real.TabIndex = 0; + checkBox_Mode_Real.Text = "Real simulation"; + checkBox_Mode_Real.UseVisualStyleBackColor = true; + checkBox_Mode_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged; + // // tabPage_Area // tabPage_Area.Location = new Point(4, 24); tabPage_Area.Name = "tabPage_Area"; - tabPage_Area.Size = new Size(210, 849); + tabPage_Area.Size = new Size(210, 889); tabPage_Area.TabIndex = 2; tabPage_Area.Tag = "#area"; tabPage_Area.Text = "Area"; @@ -1502,7 +1561,7 @@ groupBox_Navi.Dock = DockStyle.Right; groupBox_Navi.Location = new Point(684, 24); groupBox_Navi.Name = "groupBox_Navi"; - groupBox_Navi.Size = new Size(200, 877); + groupBox_Navi.Size = new Size(200, 917); groupBox_Navi.TabIndex = 3; groupBox_Navi.TabStop = false; groupBox_Navi.Tag = "#navigation"; @@ -1515,7 +1574,7 @@ panel1.Dock = DockStyle.Fill; panel1.Location = new Point(3, 42); panel1.Name = "panel1"; - panel1.Size = new Size(194, 832); + panel1.Size = new Size(194, 872); panel1.TabIndex = 3; // // button_Drone_Color @@ -1552,47 +1611,18 @@ timer_Test.Interval = 10; timer_Test.Tick += timer_Test_Tick; // - // label55 - // - label55.AutoSize = true; - label55.Location = new Point(115, 105); - label55.Name = "label55"; - label55.Size = new Size(24, 15); - label55.TabIndex = 35; - label55.Text = "sec"; - // - // numericUpDown18 - // - numericUpDown18.DecimalPlaces = 2; - numericUpDown18.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); - numericUpDown18.Location = new Point(68, 103); - numericUpDown18.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); - numericUpDown18.Name = "numericUpDown18"; - numericUpDown18.Size = new Size(41, 23); - numericUpDown18.TabIndex = 34; - numericUpDown18.Value = new decimal(new int[] { 1, 0, 0, 65536 }); - // - // label56 - // - label56.AutoSize = true; - label56.Location = new Point(5, 105); - label56.Name = "label56"; - label56.Size = new Size(55, 15); - label56.TabIndex = 33; - label56.Text = "Lateness:"; - // // Form_Main // AutoScaleDimensions = new SizeF(7F, 15F); AutoScaleMode = AutoScaleMode.Font; - ClientSize = new Size(884, 901); + ClientSize = new Size(884, 941); Controls.Add(groupBox_Screen); Controls.Add(groupBox_Navi); Controls.Add(tabControl_Menu); Controls.Add(menuStrip_Menu); Icon = (Icon)resources.GetObject("$this.Icon"); MainMenuStrip = menuStrip_Menu; - MinimumSize = new Size(900, 940); + MinimumSize = new Size(900, 980); Name = "Form_Main"; Text = "Drone Simulator V1.0"; FormClosing += Form_Main_FormClosing; @@ -1614,8 +1644,9 @@ panel_Menu_Model.ResumeLayout(false); groupBox1.ResumeLayout(false); groupBox1.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown18).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown9).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown10).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown11).EndInit(); groupBox_OF.ResumeLayout(false); groupBox_OF.PerformLayout(); @@ -1623,17 +1654,17 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown8).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown7).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown1).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Accur).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit(); groupBox_Barometer.ResumeLayout(false); groupBox_Barometer.PerformLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Laten).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Accur).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Freq).EndInit(); groupBox_GPS.ResumeLayout(false); groupBox_GPS.PerformLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown12).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown13).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown14).EndInit(); groupBox_Mag.ResumeLayout(false); groupBox_Mag.PerformLayout(); @@ -1654,9 +1685,10 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown3).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit(); + groupBox2.ResumeLayout(false); + groupBox2.PerformLayout(); groupBox_Navi.ResumeLayout(false); panel1.ResumeLayout(false); - ((System.ComponentModel.ISupportInitialize)numericUpDown18).EndInit(); ResumeLayout(false); PerformLayout(); } @@ -1700,11 +1732,11 @@ private Label label5; private Label label7; private Label label9; - private NumericUpDown numericUpDown_Bar_Accur; + private NumericUpDown numericUpDown_Bar_Noise; private Label label8; private GroupBox groupBox_OF; private Label label12; - private NumericUpDown numericUpDown_OF_Accur; + private NumericUpDown numericUpDown_OF_Noise; private Label label13; private NumericUpDown numericUpDown_OF_Freq; private Label label14; @@ -1764,7 +1796,7 @@ private Label label42; private Label label43; private Label label44; - private NumericUpDown numericUpDown10; + private NumericUpDown numericUpDown_Range_Noise; private Label label45; private NumericUpDown numericUpDown11; private Label label46; @@ -1773,7 +1805,7 @@ private NumericUpDown numericUpDown12; private Label label4; private Label label6; - private NumericUpDown numericUpDown13; + private NumericUpDown numericUpDown_Pos_Noise; private Label label10; private NumericUpDown numericUpDown14; private Label label11; @@ -1790,5 +1822,7 @@ private Label label55; private NumericUpDown numericUpDown18; private Label label56; + private GroupBox groupBox2; + private CheckBox checkBox_Mode_Real; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 68e3587..111f3eb 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -1,4 +1,4 @@ -using System.Text; +using System.Text; using System.Numerics; using System.Windows.Forms; using static System.Net.Mime.MediaTypeNames; @@ -22,6 +22,9 @@ namespace DroneSimulator public Form_Main() { InitializeComponent(); + + RealMode.RealSimulation = checkBox_Mode_Real.Checked; + numericUpDown_Bar_Update(null, null); } private void ClientConnectionCallback(object o) @@ -38,15 +41,6 @@ namespace DroneSimulator Drone drone = new Drone(data.ID); drone.Create(1.0f, 0.5f, 1.0f); - // - drone.dataBarometer.Pressure = 102258; - drone.dataBarometer.Accuracy = (float)numericUpDown_Bar_Accur.Value; - drone.dataBarometer.Frequency = (int)numericUpDown_Bar_Freq.Value; - try { drone.dataBarometer.Pressure = int.Parse(textBox_Bar_Pressure.Text); } - catch { MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error); } - - // - screen2D.CreateDrone(Color.Red, data.ID); AllDrones.Add(drone); @@ -84,7 +78,7 @@ namespace DroneSimulator List? send = drone.DataStream(data.Buffer, data.Size); if (send == null) return; - try + try { foreach (byte[]? b in send) { @@ -108,7 +102,6 @@ namespace DroneSimulator { button_Client_Start.Text = "Stop"; button_Client_Start.BackColor = Color.LimeGreen; - panel_Menu_Model.Enabled = false; break; } case NetServerClients.ServerState.Stop: @@ -116,7 +109,6 @@ namespace DroneSimulator label_Clients_Num.Text = "0"; button_Client_Start.Text = "Start"; button_Client_Start.BackColor = Color.Transparent; - panel_Menu_Model.Enabled = true; break; } } @@ -215,5 +207,26 @@ namespace DroneSimulator } } } + + private void numericUpDown_Bar_Update(object sender, EventArgs e) + { + try { RealMode.Barometer.Pressure = uint.Parse(textBox_Bar_Pressure.Text); } + catch + { + RealMode.Barometer.Pressure = 102258; + MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error); + } + + + RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value; + RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value; + RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value; + RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked; + } + + private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e) + { + RealMode.RealSimulation = checkBox_Mode_Real.Checked; + } } } diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs new file mode 100644 index 0000000..8b0a6e9 --- /dev/null +++ b/DroneSimulator/RealMode.cs @@ -0,0 +1,317 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Numerics; +using System.Reflection; + +namespace DroneSimulator +{ + internal class RealMode + { + public static bool RealSimulation; + + internal class Accelerometer + { + public static uint Freq; + public static float Noise; + public static float ScaleLeft; + public static float ScaleRight; + public static float Lateness; + + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private Vector3[] laten = new Vector3[count]; + private uint index = 0; + + private uint timer = 0; + private Vector3 result; + + public void Update(ref Vector3 value, ref uint time) + { + if (!RealSimulation) return; + + float scale = (ScaleRight - ScaleLeft) / 2; + float shift = scale + ScaleLeft; + + value.X = (value.X * scale) + shift; + value.Y = (value.Y * scale) + shift; + value.Z = (value.Z * scale) + shift; + + int noise = (int)(Noise * 1000); + value.X += ((float)rand.Next(-noise, noise)) / 1000; + value.Y += ((float)rand.Next(-noise, noise)) / 1000; + value.Z += ((float)rand.Next(-noise, noise)) / 1000; + + uint clock = (uint)(Lateness * 1000); + + uint tick = time - last; + last = time; + while (tick != 0) + { + tick--; + laten[index++] = value; + if (index >= clock) index = 0; + } + + value = laten[index]; + + uint freq = 1000 / Freq; + + if (timer + freq < time) + { + result = value; + timer = time; + } + else + { + value = result; + time = timer; + } + } + } + + internal class Gyroscope + { + public static uint Freq; + public static float Noise; + public static Vector3 Shift; + public static float Lateness; + + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private Vector3[] laten = new Vector3[count]; + private uint index = 0; + + private uint timer = 0; + private Vector3 result; + + public void Update(ref Vector3 value, ref uint time) + { + if (!RealSimulation) return; + + value.X += Shift.X; + value.Y += Shift.Y; + value.Z += Shift.Z; + + int noise = (int)(Noise * 1000); + value.X += ((float)rand.Next(-noise, noise)) / 1000; + value.Y += ((float)rand.Next(-noise, noise)) / 1000; + value.Z += ((float)rand.Next(-noise, noise)) / 1000; + + uint clock = (uint)(Lateness * 1000); + + uint tick = time - last; + last = time; + while (tick != 0) + { + tick--; + laten[index++] = value; + if (index >= clock) index = 0; + } + + value = laten[index]; + + uint freq = 1000 / Freq; + + if (timer + freq < time) + { + result = value; + timer = time; + } + else + { + value = result; + time = timer; + } + } + } + + internal class Magnetometer + { + + } + + internal class Position + { + public static bool Enable; + public static uint Freq; + public static float Noise; + public static float Lateness; + + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private Vector3[] laten = new Vector3[count]; + private uint index = 0; + + private uint timer = 0; + private Vector3 result; + + public void Update(ref Vector3 value, ref uint time) + { + if (!RealSimulation) return; + if (!Enable) { value = result; time = timer; return; } + + int noise = (int)(Noise * 1000); + value.X += ((float)rand.Next(-noise, noise)) / 1000; + value.Y += ((float)rand.Next(-noise, noise)) / 1000; + value.Z += ((float)rand.Next(-noise, noise)) / 1000; + + uint clock = (uint)(Lateness * 1000); + + uint tick = time - last; + last = time; + while (tick != 0) + { + tick--; + laten[index++] = value; + if (index >= clock) index = 0; + } + + value = laten[index]; + + uint freq = 1000 / Freq; + + if (timer + freq < time) + { + result = value; + timer = time; + } + else + { + value = result; + time = timer; + } + } + } + + internal class Barometer + { + public static bool Enable; + public static float Pressure; + public static uint Freq; + public static float Noise; + public static float Lateness; + + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private float[] laten = new float[count]; + private uint index = 0; + + private uint timer = 0; + private float result; + + public void Update(ref float value, ref uint time) + { + if (!Enable) { value = result; time = timer; return; } + + value = Pressure - value; + + if (!RealSimulation) return; + + int noise = (int)(Noise * 1000); + value += ((float)rand.Next(-noise, noise)) / 1000; + + uint clock = (uint)(Lateness * 1000); + + uint tick = time - last; + last = time; + while (tick != 0) + { + tick--; + laten[index++] = value; + if (index >= clock) index = 0; + } + + value = laten[index]; + + uint freq = 1000 / Freq; + + if (timer + freq < time) + { + result = value; + timer = time; + } + else + { + value = result; + time = timer; + } + } + } + + internal class OpticalFlow + { + + } + + internal class Range + { + public static bool Enable; + public static float MaxHeight; + public static uint Freq; + public static float Noise; + public static float Lateness; + + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private float[] laten = new float[count]; + private uint index = 0; + + private uint timer = 0; + private float result; + + public void Update(ref float value, ref uint time) + { + if (!RealSimulation) return; + if (!Enable) { value = result; time = timer; return; } + + if (value > MaxHeight) value = -1; + + int noise = (int)(Noise * 1000); + value += ((float)rand.Next(-noise, noise)) / 1000; + + uint clock = (uint)(Lateness * 1000); + + uint tick = time - last; + last = time; + while (tick != 0) + { + tick--; + laten[index++] = value; + if (index >= clock) index = 0; + } + + value = laten[index]; + + uint freq = 1000 / Freq; + + if (timer + freq < time) + { + result = value; + timer = time; + } + else + { + value = result; + time = timer; + } + } + } + } +}