Дофига исправлений

DroneClient/FormMain.cs

@@ -112,37 +112,39 @@ namespace DroneSimulator
 
     private void button_UU_MouseDown(object sender, MouseEventArgs e)
     {
+      const float pow = 0.1f;
+
       if (sender == button_UU)
       {
-        sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR -= 0.1f;
+        sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow;
-        sendDrone.MotorDL += 0.1f; sendDrone.MotorDR += 0.1f;
+        sendDrone.MotorDL += pow; sendDrone.MotorDR += pow;
       }
       if (sender == button_DD)
       {
-        sendDrone.MotorUL += 0.1f; sendDrone.MotorUR += 0.1f;
+        sendDrone.MotorUL += pow; sendDrone.MotorUR += pow;
-        sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR -= 0.1f;
+        sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow;
       }
       if (sender == button_LL)
       {
-        sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f;
+        sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
-        sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f;
+        sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
       }
       if (sender == button_RR)
       {
-        sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f;
+        sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
-        sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f;
+        sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
       }
 
       if (sender == button_ML)
       {
-        sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f;
+        sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
-        sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f;
+        sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
       }
 
       if (sender == button_MR)
       {
-        sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f;
+        sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
-        sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f;
+        sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
       }
     }
 

DroneSimulator/Drone.cs

@@ -9,6 +9,7 @@ namespace DroneSimulator
     public float Mass; // Масса
     public bool Active; // Живой?
     public float Length; // Длинна лучей
+    public float Dynamic; // Физика поведения
     public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
     public Quaternion Quat; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей
@@ -76,6 +77,20 @@ namespace DroneSimulator
       public float LaserRange;
     }
 
+    public DataOut GetDataOut()
+    {
+      DataOut data = new DataOut();
+
+      data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
+      data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
+
+      data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
+
+      data.LaserRange = LaserRange;
+
+      return data;
+    }
+
     public struct DataIn
     {
       public float MotorUL, MotorUR, MotorDL, MotorDR;
@@ -99,7 +114,7 @@ namespace DroneSimulator
       {
         float time = Environment.TickCount - Timer;
         Timer = Environment.TickCount;
-        Action(time / 100);
+        Action(time / 1000);
         Thread.Sleep(1);
       }
     }
@@ -108,7 +123,7 @@ namespace DroneSimulator
     {
       ID = id;
       Active = false;
-      PosXYZ = new Vector3(2000, 1000, 0);
+      PosXYZ = Vector3.Zero;
       SpdXYZ = Vector3.Zero;
       AccXYZ = Vector3.Zero;
       Quat = Quaternion.Identity;
@@ -118,9 +133,10 @@ namespace DroneSimulator
       DroneThread.Start();
     }
 
-    public int Create(float mass, float len)
+    public int Create(float mass, float len, float dynamic)
     {
       Mass = Range(mass);
+      Dynamic = dynamic * 100;
       Length = len;
 
       Active = true;
@@ -180,19 +196,23 @@ namespace DroneSimulator
 
       float flow = Power;
 
-      if (PosXYZ.Z < 100)
+      if (PosXYZ.Z < 0.3f)
       {
         flow += flow * 0.1f; // Воздушная подушка
       }
 
+      SpdPRY += AccPRY * ((time * Dynamic) / (Mass * Length));
+
+      float dyn = Dynamic / 10;
+
       Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
-      AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) / Mass;
+      AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (dyn / Mass);
       
-      SpdPRY += AccPRY * time / (Mass * Length);
+      SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity * dyn)) * time;
-
-      SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
       PosXYZ += SpdXYZ * time;
 
+      AccXYZ /= dyn;
+
       if (PosXYZ.Z < 0)
       {
         SpdPRY = Vector3.Zero;
@@ -200,7 +220,7 @@ namespace DroneSimulator
         SpdXYZ.Y = 0;
         Quat = Quaternion.Identity;
       }
-      else Rotate(SpdPRY.X, SpdPRY.Y, SpdPRY.Z);
+      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
 
       Vector4 ori = GetOrientation();
 
@@ -213,7 +233,7 @@ namespace DroneSimulator
           Active = false; // Сильно ударился о землю
         }*/
 
-        /*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45)
+        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
         {
           Active = false; // Повредил винты при посадке
         }*/
@@ -226,13 +246,13 @@ namespace DroneSimulator
       }
       else
       {
-        /*if (ori.W < 0)
+        if (ori.W < 0)
         {
-          Active = false; // Перевернулся вверх ногами
+          //Active = false; // Перевернулся вверх ногами
-        }*/
+        }
 
         Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
-        grav = (Quat * grav) * Quaternion.Inverse(Quat);
+        //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
         Acc = new Vector3(grav.X, grav.Y, grav.Z);
 
         Gyr = SpdPRY;

DroneSimulator/FormMain.cs

@@ -33,7 +33,7 @@ namespace DroneSimulator
       if (data.Connect)
       {
         Drone drone = new Drone(data.ID);
-        drone.Create(0.5f, 10.0f);
+        drone.Create(0.5f, 1.0f, 1.0f);
 
         screen2D.CreateDrone(Color.Red, data.ID);
 
@@ -73,12 +73,7 @@ namespace DroneSimulator
 
       drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
 
-      Drone.DataOut od = new Drone.DataOut();
+      Drone.DataOut od = drone.GetDataOut();
-
-      od.AccX = drone.Acc.X; od.AccY = drone.Acc.Y; od.AccZ = drone.Acc.Z;
-      od.GyrX = drone.Gyr.X; od.GyrY = drone.Gyr.Y; od.GyrZ = drone.Gyr.Z;
-      od.PosX = drone.PosXYZ.X; od.PosY = drone.PosXYZ.Y;
-      od.LaserRange = drone.LaserRange;
 
       try { data.Client.Send(Drone.getBytes(od)); }
       catch { }

DroneSimulator/Screen2D.cs

@@ -24,6 +24,7 @@ namespace DroneSimulator
       public int Azimuth = 0;
     }
 
+    private float Scale = 100;
     private DrawCallback drawCallback;
     private Bitmap MainArea = new Bitmap(4096, 2560);
     private List<DroneInfo> DroneList = new List<DroneInfo>();
@@ -161,6 +162,11 @@ namespace DroneSimulator
       const float TO_GRAD = 180 / MathF.PI;
       const float TO_RADI = MathF.PI / 180;
 
+      pos *= Scale;
+
+      pos.X += MainArea.Width / 2;
+      pos.Y += MainArea.Height / 2;
+
       foreach (var d in DroneList)
       {
         if (d.ID != id) continue;