Physics

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();
+    public struct Physics
-    RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
+    {
-    RealMode.Position RealPos = new RealMode.Position();
+      static public float Mass; // Масса
-    RealMode.Barometer RealBar = new RealMode.Barometer();
+      static public float Length; // Длинна лучей
-    RealMode.Range RealRange = new RealMode.Range();
+      static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
-    RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
+    }
+
+    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)

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;
   }
 }

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;
+    }
   }
 }