diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index e9d0295..f5b3186 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -7,14 +7,13 @@ namespace DroneSimulator internal class Drone { public int ID; - public float Mass; // Масса + public bool Active; // Живой? - public float Length; // Длинна лучей public const float Dynamic = 10; // Динамика вращения public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение public Quaternion Quat; // Основной кватернион public float Power = 0; // Тяга всех двигателей (0-1) - public float MaxPower; // Максимальная Тяга всех двигателей (КГ) + public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп @@ -34,12 +33,19 @@ namespace DroneSimulator 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 struct Physics + { + static public float Mass; // Масса + static public float Length; // Длинна лучей + static public float MaxPower; // Максимальная Тяга всех двигателей (КГ) + } + + private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer(); + private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope(); + private RealMode.Position RealPos = new RealMode.Position(); + private RealMode.Barometer RealBar = new RealMode.Barometer(); + private RealMode.Range RealRange = new RealMode.Range(); + private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow(); public static byte[] getBytes(object data) { @@ -129,12 +135,8 @@ namespace DroneSimulator DroneThread.Start(); } - public int Create(float power, float mass, float len) + public int Create() { - Mass = mass; - Length = len; - MaxPower = power; - Active = true; return ID; @@ -226,10 +228,10 @@ namespace DroneSimulator AccPRY.X -= wind_p; AccPRY.Y -= wind_r; AccPRY.Z -= wind_w; } - SpdPRY += AccPRY * (Dynamic * time / (Mass * Length)); + SpdPRY += AccPRY * (Dynamic * time / (Physics.Mass * Physics.Length)); Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat; - AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Mass); + AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Physics.Mass); SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time; PosXYZ += SpdXYZ * time; @@ -308,7 +310,7 @@ namespace DroneSimulator if (pow > 1) pow = 1; if (pow < 0) pow = 0; - return pow * MaxPower; + return pow * Physics.MaxPower; } public void SetQadroPow(float ul, float ur, float dl, float dr) diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index 97a0f04..0b9eac1 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -199,6 +199,17 @@ numericUpDown_GPS_Lat = new NumericUpDown(); label67 = new Label(); label66 = new Label(); + tabPage_Drone = new TabPage(); + groupBox_Physics = new GroupBox(); + label78 = new Label(); + numericUpDown_Physics_Power = new NumericUpDown(); + label77 = new Label(); + numericUpDown_Physics_Length = new NumericUpDown(); + label76 = new Label(); + numericUpDown_Physics_Mass = new NumericUpDown(); + label75 = new Label(); + label74 = new Label(); + label27 = new Label(); groupBox_Navi = new GroupBox(); panel1 = new Panel(); listBox_Drones = new ListBox(); @@ -277,6 +288,11 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Alt).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Lon).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Lat).BeginInit(); + tabPage_Drone.SuspendLayout(); + groupBox_Physics.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Power).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Length).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Mass).BeginInit(); groupBox_Navi.SuspendLayout(); panel1.SuspendLayout(); SuspendLayout(); @@ -308,6 +324,7 @@ tabControl_Menu.Controls.Add(tabPage_Model); tabControl_Menu.Controls.Add(tabPage_Area); tabControl_Menu.Controls.Add(tabPage_GPS); + tabControl_Menu.Controls.Add(tabPage_Drone); tabControl_Menu.Dock = DockStyle.Left; tabControl_Menu.Location = new Point(0, 0); tabControl_Menu.Name = "tabControl_Menu"; @@ -2167,6 +2184,126 @@ label66.TabIndex = 0; label66.Text = "Lat:"; // + // tabPage_Drone + // + tabPage_Drone.Controls.Add(groupBox_Physics); + tabPage_Drone.Location = new Point(4, 24); + tabPage_Drone.Name = "tabPage_Drone"; + tabPage_Drone.Padding = new Padding(3); + tabPage_Drone.Size = new Size(210, 788); + tabPage_Drone.TabIndex = 4; + tabPage_Drone.Text = "Drone"; + tabPage_Drone.UseVisualStyleBackColor = true; + // + // groupBox_Physics + // + groupBox_Physics.Controls.Add(label78); + groupBox_Physics.Controls.Add(numericUpDown_Physics_Power); + groupBox_Physics.Controls.Add(label77); + groupBox_Physics.Controls.Add(numericUpDown_Physics_Length); + groupBox_Physics.Controls.Add(label76); + groupBox_Physics.Controls.Add(numericUpDown_Physics_Mass); + groupBox_Physics.Controls.Add(label75); + groupBox_Physics.Controls.Add(label74); + groupBox_Physics.Controls.Add(label27); + groupBox_Physics.Dock = DockStyle.Top; + groupBox_Physics.Location = new Point(3, 3); + groupBox_Physics.Name = "groupBox_Physics"; + groupBox_Physics.Size = new Size(204, 120); + groupBox_Physics.TabIndex = 0; + groupBox_Physics.TabStop = false; + groupBox_Physics.Text = "Physics"; + // + // label78 + // + label78.AutoSize = true; + label78.Location = new Point(128, 87); + label78.Name = "label78"; + label78.Size = new Size(20, 15); + label78.TabIndex = 8; + label78.Text = "kg"; + // + // numericUpDown_Physics_Power + // + numericUpDown_Physics_Power.DecimalPlaces = 2; + numericUpDown_Physics_Power.Location = new Point(57, 85); + numericUpDown_Physics_Power.Maximum = new decimal(new int[] { 5000, 0, 0, 0 }); + numericUpDown_Physics_Power.Minimum = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_Physics_Power.Name = "numericUpDown_Physics_Power"; + numericUpDown_Physics_Power.Size = new Size(65, 23); + numericUpDown_Physics_Power.TabIndex = 7; + numericUpDown_Physics_Power.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Physics_Power.ValueChanged += numericUpDown_Physics_ValueChanged; + // + // label77 + // + label77.AutoSize = true; + label77.Location = new Point(128, 58); + label77.Name = "label77"; + label77.Size = new Size(18, 15); + label77.TabIndex = 6; + label77.Text = "m"; + // + // numericUpDown_Physics_Length + // + numericUpDown_Physics_Length.DecimalPlaces = 2; + numericUpDown_Physics_Length.Location = new Point(57, 56); + numericUpDown_Physics_Length.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_Physics_Length.Minimum = new decimal(new int[] { 1, 0, 0, 65536 }); + numericUpDown_Physics_Length.Name = "numericUpDown_Physics_Length"; + numericUpDown_Physics_Length.Size = new Size(65, 23); + numericUpDown_Physics_Length.TabIndex = 5; + numericUpDown_Physics_Length.Value = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Physics_Length.ValueChanged += numericUpDown_Physics_ValueChanged; + // + // label76 + // + label76.AutoSize = true; + label76.Location = new Point(128, 29); + label76.Name = "label76"; + label76.Size = new Size(20, 15); + label76.TabIndex = 4; + label76.Text = "kg"; + // + // numericUpDown_Physics_Mass + // + numericUpDown_Physics_Mass.DecimalPlaces = 2; + numericUpDown_Physics_Mass.Location = new Point(57, 27); + numericUpDown_Physics_Mass.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); + numericUpDown_Physics_Mass.Minimum = new decimal(new int[] { 1, 0, 0, 131072 }); + numericUpDown_Physics_Mass.Name = "numericUpDown_Physics_Mass"; + numericUpDown_Physics_Mass.Size = new Size(65, 23); + numericUpDown_Physics_Mass.TabIndex = 3; + numericUpDown_Physics_Mass.Value = new decimal(new int[] { 5, 0, 0, 65536 }); + numericUpDown_Physics_Mass.ValueChanged += numericUpDown_Physics_ValueChanged; + // + // label75 + // + label75.AutoSize = true; + label75.Location = new Point(8, 87); + label75.Name = "label75"; + label75.Size = new Size(43, 15); + label75.TabIndex = 2; + label75.Text = "Power:"; + // + // label74 + // + label74.AutoSize = true; + label74.Location = new Point(7, 58); + label74.Name = "label74"; + label74.Size = new Size(47, 15); + label74.TabIndex = 1; + label74.Text = "Length:"; + // + // label27 + // + label27.AutoSize = true; + label27.Location = new Point(17, 29); + label27.Name = "label27"; + label27.Size = new Size(37, 15); + label27.TabIndex = 0; + label27.Text = "Mass:"; + // // groupBox_Navi // groupBox_Navi.Controls.Add(panel1); @@ -2330,6 +2467,12 @@ ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Alt).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Lon).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Lat).EndInit(); + tabPage_Drone.ResumeLayout(false); + groupBox_Physics.ResumeLayout(false); + groupBox_Physics.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Power).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Length).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Physics_Mass).EndInit(); groupBox_Navi.ResumeLayout(false); panel1.ResumeLayout(false); ResumeLayout(false); @@ -2512,5 +2655,16 @@ private Label label82; private Label label81; private Label label80; + private TabPage tabPage_Drone; + private GroupBox groupBox_Physics; + private Label label27; + private NumericUpDown numericUpDown_Physics_Length; + private Label label76; + private NumericUpDown numericUpDown_Physics_Mass; + private Label label75; + private Label label74; + private Label label78; + private NumericUpDown numericUpDown_Physics_Power; + private Label label77; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index aaf2483..90cd72f 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -32,6 +32,7 @@ namespace DroneSimulator checkBox_Area_Freeze_CheckedChanged(null, null); numericUpDown_Area_Wind_Update(null, null); numericUpDown_GPS_ValueChanged(null, null); + numericUpDown_Physics_ValueChanged(null, null); } private void ClientConnectionCallback(object o) @@ -46,7 +47,7 @@ namespace DroneSimulator if (data.Connect) { Drone drone = new Drone(data.ID); - drone.Create(1.0f, 0.5f, 1.0f); + drone.Create(); screen2D.CreateDrone(Color.Red, data.ID); @@ -178,7 +179,7 @@ namespace DroneSimulator Invoke((MethodInvoker)delegate { - label_Clients_Num.Text = data.Count.ToString(); + label_Visual_Num.Text = data.Count.ToString(); }); if (data.Connect) @@ -344,5 +345,12 @@ namespace DroneSimulator GPS.State.Vdop = (float)numericUpDown_GPS_VDOP.Value; GPS.State.Pdop = (float)numericUpDown_GPS_PDOP.Value; } + + private void numericUpDown_Physics_ValueChanged(object sender, EventArgs e) + { + Drone.Physics.Mass = (float)numericUpDown_Physics_Mass.Value; + Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value; + Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value; + } } }