From 90cec037eb4d03f1b63ee3f0acb7ffa267f7ab0b Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Thu, 22 May 2025 01:47:54 +0300 Subject: [PATCH] update --- DroneClient/Drone.cs | 38 +++- DroneClient/DroneData.cs | 61 +++-- DroneClient/FormMain.Designer.cs | 131 ++++++++--- DroneClient/FormMain.cs | 7 +- DroneSimulator/Drone.cs | 40 +++- DroneSimulator/DroneData.cs | 12 +- DroneSimulator/FormMain.Designer.cs | 338 ++++++++++++++++++---------- DroneSimulator/FormMain.cs | 25 +- DroneSimulator/RealMode.cs | 77 +++++++ 9 files changed, 553 insertions(+), 176 deletions(-) diff --git a/DroneClient/Drone.cs b/DroneClient/Drone.cs index b553e8b..324f4ab 100644 --- a/DroneClient/Drone.cs +++ b/DroneClient/Drone.cs @@ -1,5 +1,7 @@ -using System.Diagnostics.Metrics; +using DroneData; +using System.Diagnostics.Metrics; using System.Drawing; +using System.Numerics; using System.Runtime.InteropServices; namespace DroneClient @@ -15,6 +17,8 @@ namespace DroneClient public float LaserRange; public uint TimeRange; + public Vector2 OF = Vector2.Zero; + public float MotorUL, MotorUR, MotorDL, MotorDR; public static byte[] getBytes(object data) @@ -116,6 +120,16 @@ namespace DroneClient return new byte[0]; } + private byte[]? RecvDataOF(byte[] data) + { + DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF)); + + OF.X = of.X; + OF.Y = of.Y; + + return new byte[0]; + } + private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body) { byte[] zero = new byte[0]; @@ -186,6 +200,22 @@ namespace DroneClient } } + case DroneData.DataType.DataOF: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataOF(body); + } + } + case DroneData.DataType.DataMotor4: { if (head.Mode == DroneData.DataMode.Request) @@ -271,12 +301,18 @@ namespace DroneClient local.Mode = DroneData.DataMode.Request; local.Type = DroneData.DataType.DataLocal; + DroneData.DataHead of = new DroneData.DataHead(); + of.Size = DroneData.DataHead.StrLen; + of.Mode = DroneData.DataMode.Request; + of.Type = DroneData.DataType.DataOF; + List list = new List(); list.Add(getBytes(acc)); list.Add(getBytes(gyr)); list.Add(getBytes(range)); list.Add(getBytes(local)); + list.Add(getBytes(of)); list.Add(SendDataMotor4()); int count = 0; diff --git a/DroneClient/DroneData.cs b/DroneClient/DroneData.cs index b713c0e..d58814b 100644 --- a/DroneClient/DroneData.cs +++ b/DroneClient/DroneData.cs @@ -1,4 +1,4 @@ -using System.Runtime.InteropServices; +using System.Runtime.InteropServices; namespace DroneData { @@ -12,10 +12,13 @@ namespace DroneData None = 0, Head = 1, // Output - DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, + DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, // Input - DataMotor4 = 2001, DataMotor6 = 2002 + DataMotor4 = 2001, DataMotor6 = 2002, + + // State + DataQuat = 3001, }; public struct DataHead @@ -25,7 +28,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 +40,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 +50,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,31 +60,51 @@ namespace DroneData public DataHead Head; public XYZ Mag; - public uint Time; // + public uint Time; // Последнее время изменения данных static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag)); } - public struct DataLocal - { - public DataHead Head; - public XYZ Local; // - - public uint Time; // - - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal)); - } - 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)); } + public struct DataLocal + { + public DataHead Head; + public XYZ Local; // Локальные координаты + + public uint Time; // Последнее время изменения данных + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal)); + } + + public struct DataBar + { + public DataHead Head; + public float Pressure; // Давление + + public uint Time; // Последнее время изменения данных + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); + } + + public struct DataOF + { + public DataHead Head; + public float X, Y; // Угловой сдвиг + + public uint Time; // Последнее время изменения данных + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF)); + } + public struct DataMotor4 { public DataHead Head; diff --git a/DroneClient/FormMain.Designer.cs b/DroneClient/FormMain.Designer.cs index 561daf9..3c57a3d 100644 --- a/DroneClient/FormMain.Designer.cs +++ b/DroneClient/FormMain.Designer.cs @@ -38,12 +38,14 @@ label3 = new Label(); label1 = new Label(); groupBox2 = new GroupBox(); + label_time_acc = new Label(); label_Acc_Z = new Label(); label7 = new Label(); label_Acc_Y = new Label(); label5 = new Label(); label_Acc_X = new Label(); groupBox3 = new GroupBox(); + label_time_gyr = new Label(); label_Gyr_Z = new Label(); label9 = new Label(); label_Gyr_Y = new Label(); @@ -51,6 +53,7 @@ label_Gyr_X = new Label(); label13 = new Label(); groupBox4 = new GroupBox(); + label_time_range = new Label(); label_Pos_L = new Label(); label6 = new Label(); label_Pos_Y = new Label(); @@ -65,15 +68,19 @@ label_Pow = new Label(); button_ML = new Button(); button_MR = new Button(); - label_time_acc = new Label(); - label_time_range = new Label(); - label_time_gyr = new Label(); + groupBox5 = new GroupBox(); + label_time_of = new Label(); + label_OF_Y = new Label(); + label17 = new Label(); + label_OF_X = new Label(); + label19 = new Label(); groupBox1.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit(); groupBox2.SuspendLayout(); groupBox3.SuspendLayout(); groupBox4.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit(); + groupBox5.SuspendLayout(); SuspendLayout(); // // timer_Test @@ -92,7 +99,7 @@ groupBox1.Dock = DockStyle.Top; groupBox1.Location = new Point(0, 0); groupBox1.Name = "groupBox1"; - groupBox1.Size = new Size(275, 80); + groupBox1.Size = new Size(357, 80); groupBox1.TabIndex = 3; groupBox1.TabStop = false; groupBox1.Tag = ""; @@ -173,6 +180,15 @@ groupBox2.TabStop = false; groupBox2.Text = "Acc"; // + // label_time_acc + // + label_time_acc.AutoSize = true; + label_time_acc.Location = new Point(6, 100); + label_time_acc.Name = "label_time_acc"; + label_time_acc.Size = new Size(13, 15); + label_time_acc.TabIndex = 25; + label_time_acc.Text = "0"; + // // label_Acc_Z // label_Acc_Z.AutoSize = true; @@ -227,13 +243,22 @@ groupBox3.Controls.Add(label11); groupBox3.Controls.Add(label_Gyr_X); groupBox3.Controls.Add(label13); - groupBox3.Location = new Point(95, 86); + groupBox3.Location = new Point(94, 86); groupBox3.Name = "groupBox3"; groupBox3.Size = new Size(78, 118); groupBox3.TabIndex = 6; groupBox3.TabStop = false; groupBox3.Text = "Gyr"; // + // label_time_gyr + // + label_time_gyr.AutoSize = true; + label_time_gyr.Location = new Point(3, 100); + label_time_gyr.Name = "label_time_gyr"; + label_time_gyr.Size = new Size(13, 15); + label_time_gyr.TabIndex = 26; + label_time_gyr.Text = "0"; + // // label_Gyr_Z // label_Gyr_Z.AutoSize = true; @@ -297,13 +322,22 @@ groupBox4.Controls.Add(label10); groupBox4.Controls.Add(label_Pos_X); groupBox4.Controls.Add(label14); - groupBox4.Location = new Point(188, 86); + groupBox4.Location = new Point(182, 86); groupBox4.Name = "groupBox4"; groupBox4.Size = new Size(78, 118); groupBox4.TabIndex = 7; groupBox4.TabStop = false; groupBox4.Text = "Pos"; // + // label_time_range + // + label_time_range.AutoSize = true; + label_time_range.Location = new Point(6, 100); + label_time_range.Name = "label_time_range"; + label_time_range.Size = new Size(13, 15); + label_time_range.TabIndex = 27; + label_time_range.Text = "0"; + // // label_Pos_L // label_Pos_L.AutoSize = true; @@ -443,38 +477,71 @@ button_MR.MouseDown += button_UU_MouseDown; button_MR.MouseUp += button_UU_MouseUp; // - // label_time_acc + // groupBox5 // - label_time_acc.AutoSize = true; - label_time_acc.Location = new Point(6, 100); - label_time_acc.Name = "label_time_acc"; - label_time_acc.Size = new Size(13, 15); - label_time_acc.TabIndex = 25; - label_time_acc.Text = "0"; + groupBox5.Controls.Add(label_time_of); + groupBox5.Controls.Add(label_OF_Y); + groupBox5.Controls.Add(label17); + groupBox5.Controls.Add(label_OF_X); + groupBox5.Controls.Add(label19); + groupBox5.Location = new Point(270, 86); + groupBox5.Name = "groupBox5"; + groupBox5.Size = new Size(78, 118); + groupBox5.TabIndex = 24; + groupBox5.TabStop = false; + groupBox5.Text = "OF"; // - // label_time_range + // label_time_of // - label_time_range.AutoSize = true; - label_time_range.Location = new Point(6, 100); - label_time_range.Name = "label_time_range"; - label_time_range.Size = new Size(13, 15); - label_time_range.TabIndex = 27; - label_time_range.Text = "0"; + label_time_of.AutoSize = true; + label_time_of.Location = new Point(6, 100); + label_time_of.Name = "label_time_of"; + label_time_of.Size = new Size(13, 15); + label_time_of.TabIndex = 27; + label_time_of.Text = "0"; // - // label_time_gyr + // label_OF_Y // - label_time_gyr.AutoSize = true; - label_time_gyr.Location = new Point(3, 100); - label_time_gyr.Name = "label_time_gyr"; - label_time_gyr.Size = new Size(13, 15); - label_time_gyr.TabIndex = 26; - label_time_gyr.Text = "0"; + label_OF_Y.AutoSize = true; + label_OF_Y.Location = new Point(19, 45); + label_OF_Y.Name = "label_OF_Y"; + label_OF_Y.Size = new Size(13, 15); + label_OF_Y.TabIndex = 7; + label_OF_Y.Text = "0"; + // + // label17 + // + label17.AutoSize = true; + label17.Location = new Point(6, 45); + label17.Name = "label17"; + label17.Size = new Size(17, 15); + label17.TabIndex = 6; + label17.Text = "Y:"; + // + // label_OF_X + // + label_OF_X.AutoSize = true; + label_OF_X.Location = new Point(19, 19); + label_OF_X.Name = "label_OF_X"; + label_OF_X.Size = new Size(13, 15); + label_OF_X.TabIndex = 5; + label_OF_X.Text = "0"; + // + // label19 + // + label19.AutoSize = true; + label19.Location = new Point(6, 19); + label19.Name = "label19"; + label19.Size = new Size(17, 15); + label19.TabIndex = 4; + label19.Text = "X:"; // // Form_Main // AutoScaleDimensions = new SizeF(7F, 15F); AutoScaleMode = AutoScaleMode.Font; - ClientSize = new Size(275, 447); + ClientSize = new Size(357, 447); + Controls.Add(groupBox5); Controls.Add(button_MR); Controls.Add(button_ML); Controls.Add(label_Pow); @@ -501,6 +568,8 @@ groupBox4.ResumeLayout(false); groupBox4.PerformLayout(); ((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit(); + groupBox5.ResumeLayout(false); + groupBox5.PerformLayout(); ResumeLayout(false); PerformLayout(); } @@ -546,5 +615,11 @@ private Label label_time_acc; private Label label_time_range; private Label label_time_gyr; + private GroupBox groupBox5; + private Label label_time_of; + private Label label_OF_Y; + private Label label17; + private Label label_OF_X; + private Label label19; } } diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index 70361f4..5d16e3d 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -1,4 +1,4 @@ -using System.Text; +using System.Text; using System.Numerics; using System.Windows.Forms; using static DroneSimulator.NetClient; @@ -109,6 +109,11 @@ namespace DroneSimulator label_time_range.Text = dataDrone.TimeRange.ToString(); + label_OF_X.Text = dataDrone.OF.X.ToString(); + label_OF_Y.Text = dataDrone.OF.Y.ToString(); + + label_time_of.Text = dataDrone.TimeRange.ToString(); + netClient.SendData(dataDrone.SendReqest()); } diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 25fa9a7..aa344f5 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -26,6 +26,8 @@ namespace DroneSimulator public Vector4 Orientation; + public static bool[] Freeze = new bool[3]; + private uint DataTimer; private const float Gravity = 9.8f; @@ -36,13 +38,14 @@ namespace DroneSimulator private Thread DroneThread; private int Timer; - private Random MainRandom = new Random(); + private Vector2 MoveOF = Vector2.Zero; 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(); + RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow(); public static byte[] getBytes(object data) { @@ -169,7 +172,7 @@ namespace DroneSimulator { Quaternion grav = new Quaternion(0, 0, 1, 0); - grav = (Quat * grav) * Quaternion.Inverse(Quat); + grav = Quat * grav * Quaternion.Inverse(Quat); float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD; if (yaw < 0.0f) yaw = 360.0f + yaw; @@ -201,6 +204,10 @@ namespace DroneSimulator AccXYZ /= Gravity; // Вернуть измерения в G + if (Freeze[0]) { SpdXYZ.X = 0; PosXYZ.X = 0; } + if (Freeze[1]) { SpdXYZ.Y = 0; PosXYZ.Y = 0; } + if (Freeze[2]) { SpdXYZ.Z = 0; PosXYZ.Z = 5; } + if (PosXYZ.Z < 0) { SpdPRY = Vector3.Zero; @@ -241,23 +248,26 @@ namespace DroneSimulator //Active = false; // Перевернулся вверх ногами } - Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0); - //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест) + Quaternion grav = Quat * new Quaternion(0, 0, flow/Mass, 0) * Quaternion.Inverse(Quat); Acc = new Vector3(grav.X, grav.Y, grav.Z); Gyr = SpdPRY; - float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI; + 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); else LaserRange = float.MaxValue; } + 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 * 11, (uint)tick); RealPos.Update(PosXYZ, (uint)tick); + RealOF.Update(MoveOF, LaserRange, (uint)tick); DataTimer = (uint)tick; } @@ -378,6 +388,24 @@ namespace DroneSimulator return getBytes(bar); } + private byte[] SendDataOF() + { + DroneData.DataOF of = new DroneData.DataOF(); + + 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.X = RealOF.result.X; + of.Y = RealOF.result.Y; + of.Time = RealBar.timer; + + MoveOF = Vector2.Zero; + + return getBytes(of); + } + private byte[] SendDataQuaternion() { DroneData.DataQuat quat = new DroneData.DataQuat(); @@ -424,6 +452,8 @@ namespace DroneSimulator case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero; + case DroneData.DataType.DataOF: if (head.Mode == DroneData.DataMode.Request) return SendDataOF(); else return zero; + 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; diff --git a/DroneSimulator/DroneData.cs b/DroneSimulator/DroneData.cs index 9e7961b..8b10216 100644 --- a/DroneSimulator/DroneData.cs +++ b/DroneSimulator/DroneData.cs @@ -12,7 +12,7 @@ namespace DroneData None = 0, Head = 1, // Output - DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, + DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, // Input DataMotor4 = 2001, DataMotor6 = 2002, @@ -95,6 +95,16 @@ namespace DroneData static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); } + public struct DataOF + { + public DataHead Head; + public float X, Y; // Угловой сдвиг + + public uint Time; // Последнее время изменения данных + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF)); + } + public struct DataMotor4 { public DataHead Head; diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index a91a96b..0f5d347 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -70,17 +70,17 @@ label46 = new Label(); label47 = new Label(); groupBox_OF = new GroupBox(); - label53 = new Label(); - numericUpDown17 = new NumericUpDown(); - label54 = new Label(); - numericUpDown8 = new NumericUpDown(); - numericUpDown7 = new NumericUpDown(); - label41 = new Label(); 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(); - numericUpDown1 = new NumericUpDown(); + numericUpDown_OF_Len = new NumericUpDown(); checkBox_OF_Enable = new CheckBox(); label17 = new Label(); label16 = new Label(); @@ -151,15 +151,21 @@ numericUpDown_Acc_Freq = new NumericUpDown(); label20 = new Label(); groupBox2 = new GroupBox(); - checkBox_Mode_Real = new CheckBox(); + radioButton1 = new RadioButton(); + radioButton_Real = new RadioButton(); tabPage_Area = new TabPage(); groupBox_Navi = new GroupBox(); panel1 = new Panel(); + listBox_Drones = new ListBox(); button_Drone_Color = new Button(); comboBox_Drone_Rotor = new ComboBox(); comboBox_Drone = new ComboBox(); timer_Test = new System.Windows.Forms.Timer(components); - listBox_Drones = new ListBox(); + groupBox3 = new GroupBox(); + checkBox_Area_Freeze_X = new CheckBox(); + label41 = new Label(); + checkBox_Area_Freeze_Y = new CheckBox(); + checkBox_Area_Freeze_Z = new CheckBox(); menuStrip_Menu.SuspendLayout(); groupBox_Screen.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit(); @@ -179,10 +185,11 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).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_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(); groupBox_Barometer.SuspendLayout(); @@ -210,8 +217,10 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit(); groupBox2.SuspendLayout(); + tabPage_Area.SuspendLayout(); groupBox_Navi.SuspendLayout(); panel1.SuspendLayout(); + groupBox3.SuspendLayout(); SuspendLayout(); // // menuStrip_Menu @@ -506,7 +515,7 @@ groupBox1.Controls.Add(label46); groupBox1.Controls.Add(label47); groupBox1.Dock = DockStyle.Top; - groupBox1.Location = new Point(0, 749); + groupBox1.Location = new Point(0, 732); groupBox1.Name = "groupBox1"; groupBox1.Size = new Size(204, 133); groupBox1.TabIndex = 8; @@ -648,17 +657,17 @@ // // groupBox_OF // - groupBox_OF.Controls.Add(label53); - groupBox_OF.Controls.Add(numericUpDown17); - groupBox_OF.Controls.Add(label54); - groupBox_OF.Controls.Add(numericUpDown8); - groupBox_OF.Controls.Add(numericUpDown7); - groupBox_OF.Controls.Add(label41); 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(numericUpDown1); + groupBox_OF.Controls.Add(numericUpDown_OF_Len); groupBox_OF.Controls.Add(checkBox_OF_Enable); groupBox_OF.Controls.Add(label17); groupBox_OF.Controls.Add(label16); @@ -669,95 +678,101 @@ 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, 563); groupBox_OF.Name = "groupBox_OF"; - groupBox_OF.Size = new Size(204, 186); + groupBox_OF.Size = new Size(204, 169); groupBox_OF.TabIndex = 4; groupBox_OF.TabStop = false; groupBox_OF.Text = "Optical flow"; // + // label40 + // + label40.AutoSize = true; + label40.Location = new Point(158, 18); + label40.Name = "label40"; + label40.Size = new Size(28, 15); + label40.TabIndex = 36; + label40.Text = "lens"; + // + // numericUpDown_OF_Lens + // + numericUpDown_OF_Lens.DecimalPlaces = 1; + numericUpDown_OF_Lens.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_OF_Lens.Location = new Point(153, 36); + numericUpDown_OF_Lens.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_OF_Lens.Minimum = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_OF_Lens.Name = "numericUpDown_OF_Lens"; + numericUpDown_OF_Lens.Size = new Size(40, 23); + numericUpDown_OF_Lens.TabIndex = 35; + 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, 111); + 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, 109); + 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, 155); + label53.Location = new Point(113, 139); label53.Name = "label53"; label53.Size = new Size(24, 15); label53.TabIndex = 32; label53.Text = "sec"; // - // numericUpDown17 + // numericUpDown_OF_Laten // - numericUpDown17.DecimalPlaces = 2; - numericUpDown17.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); - numericUpDown17.Location = new Point(66, 153); - numericUpDown17.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); - numericUpDown17.Name = "numericUpDown17"; - numericUpDown17.Size = new Size(41, 23); - numericUpDown17.TabIndex = 31; - numericUpDown17.Value = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_OF_Laten.DecimalPlaces = 2; + numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); + numericUpDown_OF_Laten.Location = new Point(66, 137); + 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); + numericUpDown_OF_Laten.TabIndex = 31; + numericUpDown_OF_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_OF_Laten.ValueChanged += numericUpDown_OF_Update; // // label54 // label54.AutoSize = true; - label54.Location = new Point(3, 155); + label54.Location = new Point(3, 139); label54.Name = "label54"; label54.Size = new Size(55, 15); label54.TabIndex = 30; label54.Text = "Lateness:"; // - // numericUpDown8 + // numericUpDown_OF_Error // - numericUpDown8.DecimalPlaces = 2; - numericUpDown8.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); - numericUpDown8.Location = new Point(126, 125); - numericUpDown8.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); - numericUpDown8.Name = "numericUpDown8"; - numericUpDown8.Size = new Size(46, 23); - numericUpDown8.TabIndex = 26; - numericUpDown8.Value = new decimal(new int[] { 1, 0, 0, 65536 }); - // - // numericUpDown7 - // - numericUpDown7.DecimalPlaces = 1; - numericUpDown7.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); - numericUpDown7.Location = new Point(45, 125); - numericUpDown7.Name = "numericUpDown7"; - numericUpDown7.Size = new Size(40, 23); - numericUpDown7.TabIndex = 24; - numericUpDown7.Value = new decimal(new int[] { 3, 0, 0, 0 }); - // - // label41 - // - label41.AutoSize = true; - label41.Location = new Point(171, 127); - label41.Name = "label41"; - label41.Size = new Size(24, 15); - label41.TabIndex = 29; - label41.Text = "sec"; - // - // label40 - // - label40.AutoSize = true; - label40.Location = new Point(37, 107); - label40.Name = "label40"; - label40.Size = new Size(64, 15); - label40.TabIndex = 28; - label40.Text = "probability"; - // - // label39 - // - label39.AutoSize = true; - label39.Location = new Point(135, 107); - label39.Name = "label39"; - label39.Size = new Size(31, 15); - label39.TabIndex = 27; - label39.Text = "time"; + numericUpDown_OF_Error.DecimalPlaces = 1; + numericUpDown_OF_Error.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_OF_Error.Location = new Point(38, 109); + 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(84, 127); + label38.Location = new Point(83, 111); label38.Name = "label38"; label38.Size = new Size(17, 15); label38.TabIndex = 25; @@ -766,33 +781,35 @@ // label37 // label37.AutoSize = true; - label37.Location = new Point(1, 127); + label37.Location = new Point(1, 111); label37.Name = "label37"; label37.Size = new Size(35, 15); label37.TabIndex = 23; label37.Text = "Error:"; // - // numericUpDown1 + // numericUpDown_OF_Len // - numericUpDown1.Location = new Point(68, 80); - numericUpDown1.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); - numericUpDown1.Name = "numericUpDown1"; - numericUpDown1.Size = new Size(40, 23); - numericUpDown1.TabIndex = 20; - numericUpDown1.Value = new decimal(new int[] { 8, 0, 0, 0 }); + numericUpDown_OF_Len.Location = new Point(68, 80); + numericUpDown_OF_Len.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_OF_Len.Name = "numericUpDown_OF_Len"; + numericUpDown_OF_Len.Size = new Size(40, 23); + numericUpDown_OF_Len.TabIndex = 20; + numericUpDown_OF_Len.Value = new decimal(new int[] { 8, 0, 0, 0 }); + numericUpDown_OF_Len.ValueChanged += numericUpDown_OF_Update; // // checkBox_OF_Enable // checkBox_OF_Enable.AutoSize = true; checkBox_OF_Enable.Checked = true; checkBox_OF_Enable.CheckState = CheckState.Checked; - checkBox_OF_Enable.Location = new Point(159, 157); + checkBox_OF_Enable.Location = new Point(159, 141); checkBox_OF_Enable.Name = "checkBox_OF_Enable"; checkBox_OF_Enable.Size = new Size(39, 19); checkBox_OF_Enable.TabIndex = 22; checkBox_OF_Enable.Tag = "#en"; checkBox_OF_Enable.Text = "En"; checkBox_OF_Enable.UseVisualStyleBackColor = true; + checkBox_OF_Enable.CheckedChanged += numericUpDown_OF_Update; // // label17 // @@ -811,7 +828,7 @@ label16.Size = new Size(70, 15); label16.TabIndex = 19; label16.Tag = "#max_height"; - label16.Text = "Max height:"; + label16.Text = "Max length:"; // // label12 // @@ -831,6 +848,7 @@ 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 }); + numericUpDown_OF_Noise.ValueChanged += numericUpDown_OF_Update; // // label13 // @@ -851,6 +869,7 @@ numericUpDown_OF_Freq.Size = new Size(40, 23); numericUpDown_OF_Freq.TabIndex = 13; numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 }); + numericUpDown_OF_Freq.ValueChanged += numericUpDown_OF_Update; // // label14 // @@ -1541,7 +1560,8 @@ // // groupBox2 // - groupBox2.Controls.Add(checkBox_Mode_Real); + groupBox2.Controls.Add(radioButton1); + groupBox2.Controls.Add(radioButton_Real); groupBox2.Dock = DockStyle.Top; groupBox2.Location = new Point(0, 0); groupBox2.Name = "groupBox2"; @@ -1550,19 +1570,33 @@ groupBox2.TabStop = false; groupBox2.Text = "Mode"; // - // checkBox_Mode_Real + // radioButton1 // - 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; + radioButton1.AutoSize = true; + radioButton1.Checked = true; + radioButton1.Location = new Point(9, 19); + radioButton1.Name = "radioButton1"; + radioButton1.Size = new Size(50, 19); + radioButton1.TabIndex = 2; + radioButton1.TabStop = true; + radioButton1.Text = "Ideal"; + radioButton1.UseVisualStyleBackColor = true; + radioButton1.CheckedChanged += checkBox_Mode_Real_CheckedChanged; + // + // radioButton_Real + // + radioButton_Real.AutoSize = true; + radioButton_Real.Location = new Point(150, 19); + radioButton_Real.Name = "radioButton_Real"; + radioButton_Real.Size = new Size(47, 19); + radioButton_Real.TabIndex = 1; + radioButton_Real.Text = "Real"; + radioButton_Real.UseVisualStyleBackColor = true; + radioButton_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged; // // tabPage_Area // + tabPage_Area.Controls.Add(groupBox3); tabPage_Area.Location = new Point(4, 24); tabPage_Area.Name = "tabPage_Area"; tabPage_Area.Size = new Size(210, 889); @@ -1595,6 +1629,14 @@ panel1.Size = new Size(194, 872); panel1.TabIndex = 3; // + // listBox_Drones + // + listBox_Drones.FormattingEnabled = true; + listBox_Drones.Location = new Point(3, 45); + listBox_Drones.Name = "listBox_Drones"; + listBox_Drones.Size = new Size(188, 214); + listBox_Drones.TabIndex = 3; + // // button_Drone_Color // button_Drone_Color.BackColor = Color.Tomato; @@ -1629,13 +1671,61 @@ timer_Test.Interval = 10; timer_Test.Tick += timer_Test_Tick; // - // listBox_Drones + // groupBox3 // - listBox_Drones.FormattingEnabled = true; - listBox_Drones.Location = new Point(3, 45); - listBox_Drones.Name = "listBox_Drones"; - listBox_Drones.Size = new Size(188, 214); - listBox_Drones.TabIndex = 3; + groupBox3.Controls.Add(checkBox_Area_Freeze_Z); + groupBox3.Controls.Add(checkBox_Area_Freeze_Y); + groupBox3.Controls.Add(label41); + groupBox3.Controls.Add(checkBox_Area_Freeze_X); + groupBox3.Dock = DockStyle.Top; + groupBox3.Location = new Point(0, 0); + groupBox3.Name = "groupBox3"; + groupBox3.Size = new Size(210, 59); + groupBox3.TabIndex = 0; + groupBox3.TabStop = false; + groupBox3.Text = "Position"; + // + // checkBox_Area_Freeze_X + // + checkBox_Area_Freeze_X.AutoSize = true; + checkBox_Area_Freeze_X.Location = new Point(55, 23); + checkBox_Area_Freeze_X.Name = "checkBox_Area_Freeze_X"; + checkBox_Area_Freeze_X.Size = new Size(33, 19); + checkBox_Area_Freeze_X.TabIndex = 0; + checkBox_Area_Freeze_X.Text = "X"; + checkBox_Area_Freeze_X.UseVisualStyleBackColor = true; + checkBox_Area_Freeze_X.CheckedChanged += checkBox_Area_Freeze_CheckedChanged; + // + // label41 + // + label41.AutoSize = true; + label41.Location = new Point(6, 24); + label41.Name = "label41"; + label41.Size = new Size(43, 15); + label41.TabIndex = 1; + label41.Text = "Freeze:"; + // + // checkBox_Area_Freeze_Y + // + checkBox_Area_Freeze_Y.AutoSize = true; + checkBox_Area_Freeze_Y.Location = new Point(103, 23); + checkBox_Area_Freeze_Y.Name = "checkBox_Area_Freeze_Y"; + checkBox_Area_Freeze_Y.Size = new Size(33, 19); + checkBox_Area_Freeze_Y.TabIndex = 2; + checkBox_Area_Freeze_Y.Text = "Y"; + checkBox_Area_Freeze_Y.UseVisualStyleBackColor = true; + checkBox_Area_Freeze_Y.CheckedChanged += checkBox_Area_Freeze_CheckedChanged; + // + // checkBox_Area_Freeze_Z + // + checkBox_Area_Freeze_Z.AutoSize = true; + checkBox_Area_Freeze_Z.Location = new Point(151, 23); + checkBox_Area_Freeze_Z.Name = "checkBox_Area_Freeze_Z"; + checkBox_Area_Freeze_Z.Size = new Size(33, 19); + checkBox_Area_Freeze_Z.TabIndex = 3; + checkBox_Area_Freeze_Z.Text = "Z"; + checkBox_Area_Freeze_Z.UseVisualStyleBackColor = true; + checkBox_Area_Freeze_Z.CheckedChanged += checkBox_Area_Freeze_CheckedChanged; // // Form_Main // @@ -1676,10 +1766,11 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).EndInit(); groupBox_OF.ResumeLayout(false); groupBox_OF.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)numericUpDown17).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown8).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown7).EndInit(); - ((System.ComponentModel.ISupportInitialize)numericUpDown1).EndInit(); + ((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(); groupBox_Barometer.ResumeLayout(false); @@ -1713,8 +1804,11 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit(); groupBox2.ResumeLayout(false); groupBox2.PerformLayout(); + tabPage_Area.ResumeLayout(false); groupBox_Navi.ResumeLayout(false); panel1.ResumeLayout(false); + groupBox3.ResumeLayout(false); + groupBox3.PerformLayout(); ResumeLayout(false); PerformLayout(); } @@ -1769,7 +1863,7 @@ private Label label15; private CheckBox checkBox_Pos_Enable; private Label label17; - private NumericUpDown numericUpDown1; + private NumericUpDown numericUpDown_OF_Len; private Label label16; private CheckBox checkBox_OF_Enable; private CheckBox checkBox_Bar_Enable; @@ -1809,12 +1903,8 @@ private Label label34; private Label label36; private Label label35; - private NumericUpDown numericUpDown7; + private NumericUpDown numericUpDown_OF_Error; private Label label37; - private Label label41; - private Label label40; - private Label label39; - private NumericUpDown numericUpDown8; private Label label38; private GroupBox groupBox1; private CheckBox checkBox_Range_Enable; @@ -1843,13 +1933,23 @@ private NumericUpDown numericUpDown_Acc_Laten; private Label label50; private Label label53; - private NumericUpDown numericUpDown17; + private NumericUpDown numericUpDown_OF_Laten; private Label label54; private Label label55; private NumericUpDown numericUpDown_Range_Laten; private Label label56; private GroupBox groupBox2; - private CheckBox checkBox_Mode_Real; private ListBox listBox_Drones; + private Label label39; + private NumericUpDown numericUpDown_OF_Wait; + private Label label40; + private NumericUpDown numericUpDown_OF_Lens; + private RadioButton radioButton1; + private RadioButton radioButton_Real; + private GroupBox groupBox3; + private CheckBox checkBox_Area_Freeze_X; + private CheckBox checkBox_Area_Freeze_Z; + private CheckBox checkBox_Area_Freeze_Y; + private Label label41; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 00a0f35..3598d25 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -23,7 +23,7 @@ namespace DroneSimulator { InitializeComponent(); - RealMode.RealSimulation = checkBox_Mode_Real.Checked; + RealMode.RealSimulation = radioButton_Real.Checked; numericUpDown_Acc_Update(null, null); numericUpDown_Gyr_Update(null, null); numericUpDown_Pos_Update(null, null); @@ -236,7 +236,7 @@ namespace DroneSimulator private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e) { - RealMode.RealSimulation = checkBox_Mode_Real.Checked; + RealMode.RealSimulation = radioButton_Real.Checked; } private void numericUpDown_Acc_Update(object sender, EventArgs e) @@ -277,5 +277,26 @@ namespace DroneSimulator RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value; } + + private void numericUpDown_OF_Update(object sender, EventArgs e) + { + RealMode.OpticalFlow.Freq = (uint)numericUpDown_OF_Freq.Value; + RealMode.OpticalFlow.Noise = (float)numericUpDown_OF_Noise.Value; + 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.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) + { + Drone.Freeze[0] = checkBox_Area_Freeze_X.Checked; + Drone.Freeze[1] = checkBox_Area_Freeze_Y.Checked; + Drone.Freeze[2] = checkBox_Area_Freeze_Z.Checked; + } } } diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs index 342e33f..f9c2c47 100644 --- a/DroneSimulator/RealMode.cs +++ b/DroneSimulator/RealMode.cs @@ -4,6 +4,7 @@ using System.Linq; using System.Text; using System.Numerics; using System.Reflection; +using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar; namespace DroneSimulator { @@ -263,7 +264,83 @@ namespace DroneSimulator internal class OpticalFlow { + public static bool Enable; + public static float MaxHeight; + public static uint Freq; + public static float Noise; + public static float Lateness; + public static float Error; + public static uint Wait; + public static float Lens; + private uint last = 0; + + private Random rand = new Random(); + + private const int count = 1000; + private Vector2[] laten = new Vector2[count]; + private uint index = 0; + + public uint delay = 0; + + public uint timer = 0; + public Vector2 result; + public void Update(Vector2 value, float Range, uint time) + { + if (!RealSimulation) + { + result = value; + timer = time; + return; + } + + if (!Enable) + { + result = Vector2.NaN; + 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; + else + { + int noise = (int)(Noise * 1000); + value.X += ((float)rand.Next(-noise, noise)) / 1000; + value.Y += ((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; + } + } } internal class Range