Mag active

Димончик нашёл ошибочку, которую поправили. Хвала всемогущему Димончику!!!

DroneSimulator/Drone.cs

@@ -1,6 +1,8 @@
-using System.Numerics;
+using System.Diagnostics;
+using System.Net.Sockets;
+using System.Numerics;
 using System.Runtime.InteropServices;
-using static System.Net.Mime.MediaTypeNames;
+using static VisualData.VisualDrone;
 
 namespace DroneSimulator
 {
@@ -8,10 +10,12 @@ namespace DroneSimulator
   {
     public int ID;
 
-    public bool Active; // Живой?
+    private Socket? Client = null;
+
+    public bool Active = false; // Живой?
     public const float Dynamic = 10; // Динамика вращения
-    public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
-    public Quaternion Quat; // Основной кватернион
+    public Vector3 PosXYZ = Vector3.Zero, SpdXYZ = Vector3.Zero, AccXYZ = Vector3.Zero; // Положение в пространстве: Позиция, Скорость, Ускорение
+    public Quaternion Quat = Quaternion.Identity; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей (0-1)
     
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
@@ -21,17 +25,23 @@ namespace DroneSimulator
 
     public Vector4 Orientation;
 
-    private uint DataTimer;
-
     private const float Gravity = 9.8f;
 
     private const float TO_GRAD = 180 / MathF.PI;
     private const float TO_RADI = MathF.PI / 180;
 
-    private Thread DroneThread;
-    private int Timer;
+    public static List<Drone> AllDrones = new List<Drone>();
+    private static Thread? DroneThread = null;
+    private uint StepTime = 0;
+
+    public static Semaphore StepSemaphore = new Semaphore(0, 10);
+
+    private uint Timer;
+
+    public static bool TimeLimit = false;
 
     private Vector2 MoveOF = Vector2.Zero;
+    private bool ReadyOF = false;
 
     public struct Physics
     {
@@ -40,12 +50,19 @@ namespace DroneSimulator
       static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
     }
 
+    public struct Propeller
+    {
+      static public float Diameter; // Диаметр лопостей
+      static public float MaxRotate; // Максимальнные обороты в секунду
+    }
+
     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();
+    private RealMode.Magnetometer RealMag = new RealMode.Magnetometer();
 
     public static byte[] getBytes(object data)
     {
@@ -112,27 +129,53 @@ namespace DroneSimulator
       return drone;
     }
 
-    private void ThreadFunction()
+    public static void StartThread()
+    {
+      if (DroneThread != null) return;
+
+      DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
+      DroneThread.Priority = ThreadPriority.Highest;
+      DroneThread.Start();
+    }
+
+    public static void StopThread()
+    {
+      if (DroneThread == null) return;
+
+      DroneThread = null;
+    }
+
+    private static void ThreadFunction()
     {
       while (DroneThread != null)
       {
-        Action(Environment.TickCount);
-        Thread.Sleep(1);
+        if (StepSemaphore.WaitOne(1))
+        {
+          lock (AllDrones)
+          {
+            foreach (Drone drone in AllDrones)
+            {
+              if (drone.StepTime > 0)
+              {
+                uint prev = drone.Timer;
+                uint next = prev + drone.StepTime;
+
+                drone.Action(next);
+                drone.StepTime = 0;
+                drone.SendStep(next, prev);
+                break;
+              }
+            }
+          }
+        }
       }
     }
 
-    public Drone(int id)
+    public Drone(int id, Socket? client)
     {
       ID = id;
-      Active = false;
-      PosXYZ = Vector3.Zero;
-      SpdXYZ = Vector3.Zero;
-      AccXYZ = Vector3.Zero;
-      Quat = Quaternion.Identity;
-
-      DroneThread = new Thread(new ThreadStart(ThreadFunction));
-      Timer = Environment.TickCount;
-      DroneThread.Start();
+      Timer = 0;
+      Client = client;
     }
 
     public int Create()
@@ -142,11 +185,6 @@ namespace DroneSimulator
       return ID;
     }
 
-    public void Close()
-    {
-      DroneThread = null;
-    }
-
     private float GetAngle(float a1, float a2, float az)
     {
       if (a2 == 0.0f && az == 0.0f)
@@ -176,6 +214,17 @@ namespace DroneSimulator
       Quat = Quaternion.Normalize(map);
     }
 
+    Vector2 RotateToZ(Vector2 vec, bool Rev = false)
+    {
+      Quaternion v = new Quaternion(vec.X, vec.Y, 0, 0);
+      Quaternion q = new Quaternion(0, 0, Rev ? -Quat.Z : Quat.Z, Quat.W);
+      q = Quaternion.Normalize(q);
+
+      q = (v * q) * q;
+
+      return new Vector2(q.X, q.Y);
+    }
+
     public Vector4 GetOrientation()
     {
       Quaternion grav = new Quaternion(0, 0, 1, 0);
@@ -188,9 +237,16 @@ namespace DroneSimulator
       return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
     }
 
-    public void Action(int tick)
+    int TestGyr = 0;
+    int TestDir = 1;
+
+    public void Action(uint tick)
     {
-      float time = (tick - Timer) / 1000.0f;
+      uint period = tick - Timer;
+
+      if (period == 0) return;
+
+      float time = period / 1000.0f;
       Timer = tick;
 
       if (!Active) return;
@@ -202,8 +258,6 @@ namespace DroneSimulator
         flow += flow * 0.1f; // Воздушная подушка
       }
 
-      
-
       float wind_x = 0, wind_y = 0, wind_z = 0;
       float wind_p = 0, wind_r = 0, wind_w = 0;
 
@@ -241,42 +295,52 @@ namespace DroneSimulator
       if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
       if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
 
-      if (PosXYZ.Z < 0)
+      /*if (PosXYZ.Z < 0)
       {
         SpdPRY = Vector3.Zero;
         SpdXYZ.X = 0;
         SpdXYZ.Y = 0;
         Quat = Quaternion.Identity;
       }
-      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
+      else */
+      Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
 
       Vector4 ori = GetOrientation();
 
       Orientation = ori;
+      float range = 0;
 
-      if (PosXYZ.Z < 0)
+      if (PosXYZ.Z <= 0)
+      {
+        PosXYZ.Z = 0;
+        SpdXYZ.Z = 0;
+        LaserRange = 0;
+        Acc = new Vector3(0, 0, 1);
+      }
+
+      /*if (PosXYZ.Z < 0)
       {
         PosXYZ.Z = 0;
 
-        /*if (SpdXYZ.Z < -5)
-        {
-          Active = false; // Сильно ударился о землю
-        }*/
+        //if (SpdXYZ.Z < -5)
+        //{
+        //  Active = false; // Сильно ударился о землю
+        //}
 
-        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
-        {
-          Active = false; // Повредил винты при посадке
-        }*/
+        //if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
+        //{
+        //  Active = false; // Повредил винты при посадке
+        //}
 
         SpdXYZ.Z = 0;
 
         Acc = new Vector3(0, 0, 1);
         Gyr = Vector3.Zero;
         LaserRange = 0;
-      }
+      }*/
       else
       {
-        if (ori.W < 0)
+        if (ori.W < 0) // это обман
         {
           //Active = false; // Перевернулся вверх ногами
         }
@@ -288,21 +352,35 @@ namespace DroneSimulator
 
         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);
+        range = PosXYZ.Z / MathF.Cos(tilt);
+
+        if (tilt < 90 && ori.W > 0) LaserRange = range;
         else LaserRange = float.MaxValue;
       }
 
-      MoveOF.X += SpdXYZ.X - Gyr.Y;
-      MoveOF.Y += SpdXYZ.Y + Gyr.X;
+      RealAcc.Update(Acc, tick);
+      RealGyr.Update(Gyr, tick);
+      RealRange.Update(LaserRange, tick);
+      RealBar.Update(PosXYZ.Z, tick);
+      RealPos.Update(PosXYZ, tick);
 
-      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);
+      Vector2 xy = new Vector2(SpdXYZ.X * TO_GRAD / range, SpdXYZ.Y * TO_GRAD / range);
+      xy = RotateToZ(xy, true);
 
-      DataTimer = (uint)tick;
+      Vector2 of_xy;
+      if (range > 0.1) of_xy = new Vector2(xy.X - Gyr.Y, xy.Y + Gyr.X);
+      else of_xy = Vector2.Zero;
+
+      bool of = RealOF.Update(of_xy, LaserRange, tick);
+
+      RealMag.Update(Quat, tick);
+
+      lock (this)
+      {
+        MoveOF += RealOF.result * time;
+
+        if (of) ReadyOF = true;
+      }
     }
 
     private float Range(float pow)
@@ -317,6 +395,13 @@ namespace DroneSimulator
     {
       ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);
 
+      float coef = Area.Wind.Density * MathF.Pow(Propeller.Diameter, 4);
+
+      //ul = MathF.Pow(ul * Propeller.MaxRotate, 2) * coef; // я хз как делать
+      //ur = MathF.Pow(ur * Propeller.MaxRotate, 2) * coef;
+      //dl = MathF.Pow(dl * Propeller.MaxRotate, 2) * coef;
+      //dr = MathF.Pow(dr * Propeller.MaxRotate, 2) * coef;
+
       Power = (ul + ur + dl + dr) / 4;
 
       AccPRY.Y = ((ul + dl) - (ur + dr));
@@ -324,6 +409,32 @@ namespace DroneSimulator
       AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
     }
 
+    private void SendStep(uint time, uint prev)
+    {
+      DroneData.Step step = new DroneData.Step();
+
+      step.Head.Size = Marshal.SizeOf(typeof(DroneData.Step));
+      step.Head.Mode = DroneData.DataMode.Response;
+      step.Head.Type = DroneData.DataType.Step;
+      step.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+
+      step.StepTime = time;
+      step.PrevTime = prev;
+
+      byte[] bytes = getBytes(step);
+
+      try { Client?.Send(bytes); } catch { }
+    }
+
+    private void RecvStep(byte[] data)
+    {
+      DroneData.Step step = (DroneData.Step)fromBytes(data, typeof(DroneData.Step));
+      
+      StepTime = step.StepTime;
+
+      StepSemaphore.Release();
+    }
+
     private void RecvDataMotor4(byte[] data)
     {
       DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
@@ -338,7 +449,7 @@ namespace DroneSimulator
       acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
       acc.Head.Mode = DroneData.DataMode.Response;
       acc.Head.Type = DroneData.DataType.DataAcc;
-      acc.Head.Time = (uint)Environment.TickCount;
+      acc.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
       acc.Time = RealAcc.timer;
@@ -353,7 +464,7 @@ namespace DroneSimulator
       gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
       gyr.Head.Mode = DroneData.DataMode.Response;
       gyr.Head.Type = DroneData.DataType.DataGyr;
-      gyr.Head.Time = (uint)Environment.TickCount;
+      gyr.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
       gyr.Time = RealGyr.timer;
@@ -368,10 +479,10 @@ namespace DroneSimulator
       mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
       mag.Head.Mode = DroneData.DataMode.Response;
       mag.Head.Type = DroneData.DataType.DataMag;
-      mag.Head.Time = (uint)Environment.TickCount;
+      mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
-      mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
-      mag.Time = DataTimer;
+      mag.Mag.X = RealMag.result.X; mag.Mag.Y = RealMag.result.Y; mag.Mag.Z = RealMag.result.Z;
+      mag.Time = RealMag.timer;
 
       return getBytes(mag);
     }
@@ -383,7 +494,7 @@ namespace DroneSimulator
       range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
       range.Head.Mode = DroneData.DataMode.Response;
       range.Head.Type = DroneData.DataType.DataRange;
-      range.Head.Time = (uint)Environment.TickCount;
+      range.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       range.LiDAR = RealRange.result;
       range.Time = RealRange.timer;
@@ -398,7 +509,7 @@ namespace DroneSimulator
       local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
       local.Head.Mode = DroneData.DataMode.Response;
       local.Head.Type = DroneData.DataType.DataLocal;
-      local.Head.Time = (uint)Environment.TickCount;
+      local.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
       local.Time = RealPos.timer;
@@ -413,7 +524,7 @@ namespace DroneSimulator
       bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
       bar.Head.Mode = DroneData.DataMode.Response;
       bar.Head.Type = DroneData.DataType.DataBar;
-      bar.Head.Time = (uint)Environment.TickCount;
+      bar.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       bar.Pressure = RealBar.result;
       bar.Time = RealBar.timer;
@@ -428,13 +539,26 @@ namespace DroneSimulator
       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;
+      of.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
+      lock (this)
+      {
+        if (ReadyOF)
+        {
+          of.X = MoveOF.X;
+          of.Y = MoveOF.Y;
           MoveOF = Vector2.Zero;
+        }
+        else
+        {
+          of.X = 0;
+          of.Y = 0;
+        }
+
+        of.Time = RealOF.timer;
+
+        ReadyOF = false;
+      }
 
       return getBytes(of);
     }
@@ -446,10 +570,10 @@ namespace DroneSimulator
       gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
       gps.Head.Mode = DroneData.DataMode.Response;
       gps.Head.Type = DroneData.DataType.DataGPS;
-      gps.Head.Time = (uint)Environment.TickCount;
+      gps.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       GPS.Point p = new GPS.Point();
-      p.x = PosXYZ.Y; p.y= PosXYZ.X;
+      p.x = RealPos.result.Y; p.y= RealPos.result.X;
 
       GPS.GlobalCoords g = new GPS.GlobalCoords();
       g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
@@ -459,7 +583,7 @@ namespace DroneSimulator
       gps.Lat = g.latitude; gps.Lon = g.longitude;
 
       gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
-      gps.Alt = GPS.Home.Alt + PosXYZ.Z;
+      gps.Alt = GPS.Home.Alt + RealPos.result.Z;
 
       DateTime tim = DateTime.Now;
       gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
@@ -472,6 +596,8 @@ namespace DroneSimulator
       gps.Vdop = GPS.State.Vdop;
       gps.Pdop = GPS.State.Pdop;
 
+      gps.Time = RealPos.timer;
+
       return getBytes(gps);
     }
 
@@ -482,19 +608,33 @@ namespace DroneSimulator
       quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
       quat.Head.Mode = DroneData.DataMode.Response;
       quat.Head.Type = DroneData.DataType.DataQuat;
-      quat.Head.Time = (uint)Environment.TickCount;
+      quat.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
       quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
 
       return getBytes(quat);
     }
 
+    private byte[] SendPingPong()
+    {
+      DroneData.DataHead head = new DroneData.DataHead();
+
+      head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
+      head.Mode = DroneData.DataMode.Response;
+      head.Type = DroneData.DataType.Ping;
+      head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+
+      return getBytes(head);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
       byte[] zero = Array.Empty<byte>();
 
       switch (head.Type)
       {
+        case DroneData.DataType.Step: if (head.Mode == DroneData.DataMode.Request) RecvStep(body); return zero;
+
         case DroneData.DataType.DataAcc:
           {
             if (head.Mode == DroneData.DataMode.Request)
@@ -528,6 +668,8 @@ namespace DroneSimulator
         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;
+
+        case DroneData.DataType.Ping: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       return zero;
@@ -536,7 +678,7 @@ namespace DroneSimulator
     private const int DroneStreamCount = 512;
     private byte[] DroneStreamData = new byte[DroneStreamCount];
     private int DroneStreamIndex = 0;
-    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
+    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.Ping };
 
     public List<byte[]?>? DataStream(byte[]? data, int size)
     {
@@ -557,6 +699,8 @@ namespace DroneSimulator
           DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
         }
 
+        if (DroneStreamHead.Size == 0) return null; // Поток сломан (конец)
+
         if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный
 
         byte[] body = new byte[DroneStreamHead.Size];

DroneSimulator/DroneData.cs

@@ -1,5 +1,4 @@
-using System.Net;
-using System.Runtime.InteropServices;
+using System.Runtime.InteropServices;
 
 namespace DroneData
 {
@@ -10,7 +9,7 @@ namespace DroneData
 
   public enum DataType : ushort
   {
-    None = 0, Head = 1,
+    None = 0, Ping = 1, Step = 2,
 
     // Output
     DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, DataGPS = 1008,
@@ -29,17 +28,27 @@ namespace DroneData
     public DataMode Mode;
     public DataType Type;
 
-    public uint Time; // Общее время
+    public uint Time; // ответ: Общее время на симуляторе
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
   }
 
+  public struct Step
+  {
+    public DataHead Head;
+
+    public uint StepTime; // ms, запрос: шаг работы симулятора; ответ: Последнее время изменения данных
+    public uint PrevTime; // ms, ответ: Предпоследнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.Step));
+  }
+
   public struct XYZ { public float X, Y, Z; }
 
   public struct DataAcc
   {
     public DataHead Head;
-    public XYZ Acc;
+    public XYZ Acc; // G, ускорения по осям
 
     public uint Time; // Последнее время изменения данных
 
@@ -49,7 +58,7 @@ namespace DroneData
   public struct DataGyr
   {
     public DataHead Head;
-    public XYZ Gyr;
+    public XYZ Gyr; // dps, угловые скорости
 
     public uint Time; // Последнее время изменения данных
 
@@ -69,7 +78,7 @@ namespace DroneData
   public struct DataRange
   {
     public DataHead Head;
-    public float LiDAR; // Датчик посадки
+    public float LiDAR; // m, Датчик посадки
 
     public uint Time; // Последнее время изменения данных
 
@@ -79,7 +88,7 @@ namespace DroneData
   public struct DataLocal
   {
     public DataHead Head;
-    public XYZ Local; // Локальные координаты
+    public XYZ Local; // m, Локальные координаты
 
     public uint Time; // Последнее время изменения данных
 
@@ -89,7 +98,7 @@ namespace DroneData
   public struct DataBar
   {
     public DataHead Head;
-    public float Pressure; // Давление
+    public float Pressure; // Pa, Давление
 
     public uint Time; // Последнее время изменения данных
 
@@ -99,7 +108,7 @@ namespace DroneData
   public struct DataOF
   {
     public DataHead Head;
-    public float X, Y; // Угловой сдвиг
+    public float X, Y; // degree, Угловой сдвиг
 
     public uint Time; // Последнее время изменения данных
 
@@ -129,7 +138,7 @@ namespace DroneData
   public struct DataMotor4
   {
     public DataHead Head;
-    public float UL, UR, DL, DR;
+    public float UL, UR, DL, DR; // тяга 0.0 - 1.0
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
   }
@@ -137,7 +146,7 @@ namespace DroneData
   public struct DataMotor6
   {
     public DataHead Head;
-    public float UL, UR, LL, RR, DL, DR;
+    public float UL, UR, LL, RR, DL, DR; // тяга 0.0 - 1.0
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
   }
@@ -145,7 +154,7 @@ namespace DroneData
   public struct DataQuat
   {
     public DataHead Head;
-    public float X, Y, Z, W;
+    public float X, Y, Z, W; // Кватернион дрона
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat));
   }

DroneSimulator/FormMain.cs

@@ -17,8 +17,6 @@ namespace DroneSimulator
     NetServerClients netServerClient = new NetServerClients();
     NetServerVisual netServerVisual = new NetServerVisual();
 
-    List<Drone> AllDrones = new List<Drone>();
-
     public Form_Main()
     {
       InitializeComponent();
@@ -46,26 +44,30 @@ namespace DroneSimulator
 
       if (data.Connect)
       {
-        Drone drone = new Drone(data.ID);
+        Drone drone = new Drone(data.ID, data.Client);
         drone.Create();
 
         screen2D.CreateDrone(Color.Red, data.ID);
 
-        AllDrones.Add(drone);
+        lock (Drone.AllDrones) Drone.AllDrones.Add(drone);
       }
       else
       {
-        foreach (Drone drone in AllDrones)
+        Drone? d = null;
+
+        lock (Drone.AllDrones)
+        {
+          foreach (Drone drone in Drone.AllDrones)
           {
             if (drone.ID != data.ID) continue;
-          drone.Close();
-
-          screen2D.RemoveDrone(data.ID);
-
-          AllDrones.Remove(drone);
+            d = drone;
+            Drone.AllDrones.Remove(drone);
             break;
           }
         }
+
+        if (d != null) screen2D.RemoveDrone(d.ID);
+      }
     }
 
     private void ClientReceiveCallback(object o)
@@ -74,12 +76,15 @@ namespace DroneSimulator
 
       Drone? drone = null;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
+      {
+        foreach (Drone d in Drone.AllDrones)
         {
           if (d.ID != data.ID) continue;
           drone = d;
           break;
         }
+      }
 
       if (drone == null) return;
 
@@ -110,6 +115,9 @@ namespace DroneSimulator
           {
             button_Client_Start.Text = "Stop";
             button_Client_Start.BackColor = Color.LimeGreen;
+            numericUpDown_Clients_Limit.Enabled = false;
+            numericUpDown_Clients_Port.Enabled = false;
+            checkBox_Lockstep_Limit.Enabled = false;
             break;
           }
         case NetServerClients.ServerState.Stop:
@@ -117,12 +125,17 @@ namespace DroneSimulator
             label_Clients_Num.Text = "0";
             button_Client_Start.Text = "Start";
             button_Client_Start.BackColor = Color.Transparent;
+            numericUpDown_Clients_Limit.Enabled = true;
+            numericUpDown_Clients_Port.Enabled = true;
+            checkBox_Lockstep_Limit.Enabled = true;
             break;
           }
       }
 
       if (done != NetServerClients.ServerState.Start) return;
 
+      Drone.StartThread();
+
       pictureBox_2D.Image = null;
 
       screen2D = new Screen2D(DrawCallback);
@@ -144,10 +157,9 @@ namespace DroneSimulator
 
     private void timer_Test_Tick(object sender, EventArgs e)
     {
-      DateTime test = DateTime.Now;
-
-      int tim = test.Second + test.Minute * 100 + test.Hour * 10000;
+      DateTime time = DateTime.Now;
 
+      label_Lockstep_Time.Text = time.Hour.ToString("D2") + ":" + time.Minute.ToString("D2") + ":" + time.Second.ToString("D2") + "." + time.Millisecond.ToString("D3");
 
       if (screen2D == null) return;
 
@@ -155,23 +167,22 @@ namespace DroneSimulator
 
       try
       {
-        foreach (Drone d in AllDrones)
+        lock (Drone.AllDrones)
         {
-          screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+          foreach (Drone d in Drone.AllDrones)
+          {
+            screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation(), d.Quat);
 
             string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
 
             listBox_Drones.Items.Add(line);
           }
         }
+      }
       catch { }
 
       screen2D.DrawScene();
     }
-    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
-    {
-      foreach (Drone d in AllDrones) d.Close();
-    }
 
     private void VisualConnectionCallback(object o)
     {
@@ -198,14 +209,17 @@ namespace DroneSimulator
 
       int index = 0;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
       {
-        VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
+        foreach (Drone d in Drone.AllDrones)
+        {
+          VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++);
 
           try { data.Client.Send(Drone.getBytes(v)); }
           catch { }
         }
       }
+    }
 
     private void button_Visual_Start_Click(object sender, EventArgs e)
     {
@@ -306,11 +320,8 @@ namespace DroneSimulator
       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.Lens = (uint)numericUpDown_OF_Lens.Value;
       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)
@@ -352,5 +363,21 @@ namespace DroneSimulator
       Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value;
       Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value;
     }
+
+    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+    {
+      Drone.StopThread();
+    }
+
+    private void checkBox_Lockstep_Limit_CheckedChanged(object sender, EventArgs e)
+    {
+      Drone.TimeLimit = checkBox_Lockstep_Limit.Checked;
+    }
+
+    private void numericUpDown_Propeller_ValueChanged(object sender, EventArgs e)
+    {
+      Drone.Propeller.Diameter = (float)numericUpDown_Propeller_Diameter.Value;
+      Drone.Propeller.MaxRotate = ((float)numericUpDown_Propeller_Rotation.Value) / 60;
+    }
   }
 }

DroneSimulator/RealMode.cs

@@ -1,10 +1,12 @@
-using System;
+using Microsoft.VisualBasic.Devices;
+using System;
 using System.Collections.Generic;
 using System.Linq;
-using System.Text;
 using System.Numerics;
 using System.Reflection;
+using System.Text;
 using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;
 
 namespace DroneSimulator
 {
@@ -27,7 +29,7 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private Vector3[] laten = new Vector3[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint timer = 0;
       public Vector3 result;
@@ -53,22 +55,25 @@ namespace DroneSimulator
         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)
+        uint clock = time - last;
+        while (true)
         {
-          tick--;
-          laten[index++] = value;
-          if (index >= clock) index = 0;
+          laten[index] = value;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
         }
+        last = time;
 
-        value = laten[index];
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        value = laten[move];
 
         uint freq = 1000 / Freq;
 
-        if (timer + freq < time)
+        if (timer + freq <= time)
         {
           result = value;
           timer = time;
@@ -90,7 +95,7 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private Vector3[] laten = new Vector3[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint timer = 0;
       public Vector3 result;
@@ -113,22 +118,25 @@ namespace DroneSimulator
         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)
+        uint clock = time - last;
+        while (true)
         {
-          tick--;
-          laten[index++] = value;
-          if (index >= clock) index = 0;
+          laten[index] = value;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
         }
+        last = time;
 
-        value = laten[index];
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        value = laten[move];
 
         uint freq = 1000 / Freq;
 
-        if (timer + freq < time)
+        if (timer + freq <= time)
         {
           result = value;
           timer = time;
@@ -138,7 +146,106 @@ namespace DroneSimulator
 
     internal class Magnetometer
     {
+      /**
+       * The model is produced by the United States’ National Geospatial-Intelligence Agency (NGA)
+       * and the United Kingdom’s Defence Geographic Centre (DGC)
+       * NCEI and the British Geological Survey (BGS) jointly developed the WMM.
+       */
+      /* Taganrog
+       * 47° 12' 32" N
+       * 38° 56' 10" E
+       * Declination: 8° 32' 28"
+       * Inclination: 65° 34' 9"
+       * Total Field: 51,120.8 nT
+       */
+      public static bool Enable;
+      public static uint Freq;
+      public static float Noise;
+      public static Vector3 Shift;
+      public static float Lateness;
+      public static bool RealSimulation;
 
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      public static float fieldStrength = 51.1208F; // uT
+      public static float fieldDeclination = (8 + 32 / 60 + 28 / 3600) * (MathF.PI / 180);
+      public static float fieldInclination = (65 + 34 / 60 + 9 / 3600) * (MathF.PI / 180);
+
+      private static Vector3 InitializeMagneticField()
+      {
+        float horizontalComponent = fieldStrength * MathF.Cos(fieldInclination);
+
+        float northComponent = horizontalComponent * MathF.Cos(fieldDeclination); // X
+        float eastComponent = horizontalComponent * MathF.Sin(fieldDeclination);  // Y
+        float downComponent = fieldStrength * MathF.Sin(fieldInclination);        // Z
+
+        return new Vector3(northComponent, eastComponent, downComponent);
+      }
+
+      private static Vector3 magneticField = InitializeMagneticField();
+
+      private const int count = 1000;
+      private Vector3[] laten = new Vector3[count];
+      private int index = 0;
+
+      public uint timer = 0;
+      public Vector3 result;
+
+      public void Update(Quaternion oreintantion, uint time)
+      {
+        Vector3 value = Vector3.Transform(magneticField, oreintantion);
+
+        Vector3 v = value;
+
+        v.X += Shift.X;
+        v.Y += Shift.Y;
+        v.Z += Shift.Z;
+
+        int noise = (int)(Noise * 1000);
+        v.X += ((float)rand.Next(-noise, noise)) / 1000;
+        v.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        v.Z += ((float)rand.Next(-noise, noise)) / 1000;
+
+        uint clock = time - last;
+        while (true)
+        {
+          laten[index] = v;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
+        if (!Enable)
+        {
+          result = Vector3.NaN;
+          timer = time;
+          return;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        v = laten[move];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq <= time)
+        {
+          result = v;
+          timer = time;
+        }
+      }
     }
 
     internal class Position
@@ -155,16 +262,35 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private Vector3[] laten = new Vector3[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint timer = 0;
       public Vector3 result;
 
       public void Update(Vector3 value, uint time)
       {
+        Vector3 v = value;
+
+        int noise = (int)(Noise * 1000);
+        v.X += ((float)rand.Next(-noise, noise)) / 1000;
+        v.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        v.Z += ((float)rand.Next(-noise, noise)) / 1000;
+
+        uint clock = time - last;
+        while (true)
+        {
+          laten[index] = v;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
         if (!Enable)
         {
           result = Vector3.NaN;
+          timer = time;
           return;
         }
 
@@ -175,29 +301,15 @@ namespace DroneSimulator
           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];
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        v = laten[move];
 
         uint freq = 1000 / Freq;
-
-        if (timer + freq < time)
+        if (timer + freq <= time)
         {
-          result = value;
+          result = v;
           timer = time;
         }
       }
@@ -211,6 +323,7 @@ namespace DroneSimulator
       public static float Noise;
       public static float Lateness;
       public static bool RealSimulation;
+      public static float Temperature = 25.0f;
 
       private uint last = 0;
 
@@ -218,18 +331,35 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private float[] laten = new float[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint timer = 0;
       public float result;
 
       public void Update(float value, uint time)
       {
-        value = Pressure - value;
+        value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));
+
+        float v = value;
+
+        int noise = (int)(Noise * 1000);
+        v += ((float)rand.Next(-noise, noise)) / 1000;
+
+        uint clock = time - last;
+        while (true)
+        {
+          laten[index] = v;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
 
         if (!Enable)
         {
           result = float.NaN;
+          timer = time;
           return;
         }
 
@@ -240,27 +370,15 @@ namespace DroneSimulator
           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];
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        v = laten[move];
 
         uint freq = 1000 / Freq;
-
-        if (timer + freq < time)
+        if (timer + freq <= time)
         {
-          result = value;
+          result = v;
           timer = time;
         }
       }
@@ -273,8 +391,6 @@ namespace DroneSimulator
       public static uint Freq;
       public static float Noise;
       public static float Lateness;
-      public static float Error;
-      public static uint Wait;
       public static float Lens;
       public static bool RealSimulation;
 
@@ -284,67 +400,64 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private Vector2[] laten = new Vector2[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint delay = 0;
 
       public uint timer = 0;
       public Vector2 result;
-      public void Update(Vector2 value, float Range, uint time)
+      public bool Update(Vector2 value, float Range, uint time)
       {
+        value *= Lens;
+
+        Vector2 v = value;
+
+        if (Range > MaxHeight) v = Vector2.Zero;
+        else
+        {
+          int noise = (int)(Noise * 1000);
+          v.X += ((float)rand.Next(-noise, noise)) / 1000;
+          v.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        }
+
+        uint clock = time - last;
+        while (true)
+        {
+          laten[index] = v;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
         if (!Enable)
         {
           result = Vector2.NaN;
-          return;
+          timer = time;
+          return true;
         }
 
         if (!RealSimulation)
         {
           result = value;
           timer = time;
-          return;
+          return true;
         }
 
-        value *= Lens;
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        result = laten[move];
 
-        if (rand.Next(0, 1000) < (Error * 10))
+        uint freq = count / Freq;
+        if (timer + freq <= time)
         {
-          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;
+          return true;
         }
+
+        return false;
       }
     }
 
@@ -363,16 +476,37 @@ namespace DroneSimulator
 
       private const int count = 1000;
       private float[] laten = new float[count];
-      private uint index = 0;
+      private int index = 0;
 
       public uint timer = 0;
       public float result;
 
       public void Update(float value, uint time)
       {
+        float v = value;
+
+        if (v > MaxHeight) v = -1;
+        else
+        {
+          int noise = (int)(Noise * 1000);
+          v += ((float)rand.Next(-noise, noise)) / 1000;
+        }
+
+        uint clock = time - last;
+        while (true)
+        {
+          laten[index] = v;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
         if (!Enable)
         {
           result = float.NaN;
+          timer = time;
           return;
         }
 
@@ -383,31 +517,15 @@ namespace DroneSimulator
           return;
         }
 
-        if (value > MaxHeight) value = MaxHeight;
-        else
-        {
-          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];
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        v = laten[move];
 
         uint freq = 1000 / Freq;
-
-        if (timer + freq < time)
+        if (timer + freq <= time)
         {
-          result = value;
+          result = v;
           timer = time;
         }
       }

DroneSimulator/Screen2D.cs

@@ -1,4 +1,5 @@
-using System.Numerics;
+using System.Drawing.Drawing2D;
+using System.Numerics;
 
 namespace DroneSimulator
 {
@@ -22,6 +23,7 @@ namespace DroneSimulator
 
       public PointF TiltXY = new Point(0, 0);
       public int Azimuth = 0;
+      public Quaternion Quaternion;
     }
 
     private float Scale = 100;
@@ -96,6 +98,47 @@ namespace DroneSimulator
       }
     }
 
+    private static Bitmap DrawImageByQuaternion(Bitmap bmp, Quaternion orientation)
+    {
+      if (bmp == null) return null;
+      orientation.X = -orientation.X;
+
+      int canvasSize = (int)System.Math.Sqrt(bmp.Width * bmp.Width + bmp.Height * bmp.Height);
+      Bitmap result = new Bitmap(canvasSize, canvasSize);
+
+      float halfWidth = bmp.Width / 2f;
+      float halfHeight = bmp.Height / 2f;
+
+
+      Vector3[] sourceCorners = new Vector3[]
+      {
+                new Vector3(-halfWidth, -halfHeight, 0), // верхний левый
+                new Vector3( halfWidth, -halfHeight, 0), // верхний правый
+                new Vector3(-halfWidth,  halfHeight, 0), // нижний левый
+      };
+
+      PointF[] destPoints = new PointF[3];
+
+      for (int i = 0; i < 3; i++)
+      {
+        Vector3 rotatedPoint = Vector3.Transform(sourceCorners[i], orientation);
+
+        destPoints[i] = new PointF(
+            rotatedPoint.X + canvasSize / 2f,
+            rotatedPoint.Y + canvasSize / 2f
+        );
+      }
+
+      using (Graphics g = Graphics.FromImage(result))
+      {
+        g.InterpolationMode = InterpolationMode.HighQualityBicubic;
+        g.SmoothingMode = SmoothingMode.AntiAlias;
+        g.DrawImage(bmp, destPoints);
+      }
+
+
+      return result;
+    }
     public void DrawScene()
     {
       using (Graphics g = Graphics.FromImage(MainArea))
@@ -109,50 +152,15 @@ namespace DroneSimulator
           try
           {
             if (d.Azimuth >= 360) d.Azimuth -= 360;
-            var bmp = RotateImage(d.Drone, d.Azimuth);
+
+            var bmp = DrawImageByQuaternion(d.Drone, d.Quaternion);
 
             g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
 
             g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
 
-            //g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
+            g.DrawImage(bmp, d.PosXY.X - d.Drone.Width / 2, d.PosXY.Y - d.Drone.Height / 2); // Draw the transformed image
 
-            float x1 = 0, y1 = 0;
-            float x2 = 130, y2 = 0;
-            float x3 = 0, y3 = 130;
-
-            const float TO_RADI = MathF.PI / 180;
-
-            Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
-            Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
-
-            tilt = tilt * rotate * rotate;
-
-            if (tilt.Y > 0)
-            {
-              x1 = (int)(Math.Sin(tilt.Y) * 130);
-              x3 = (int)(Math.Sin(tilt.Y) * 130);
-            }
-            else
-            {
-              x2 = (int)(Math.Cos(tilt.Y) * 130);
-            }
-
-            if (tilt.X > 0)
-            {
-              y1 = (int)(Math.Sin(tilt.X) * 130);
-              y2 = (int)(Math.Sin(tilt.X) * 130);
-            }
-            else
-            {
-              y3 = (int)(Math.Cos(tilt.X) * 130);
-            }
-
-            PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
-            PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
-            PointF[] dest = { ul, ur, dl };
-
-            g.DrawImage(bmp, dest);
           }
           catch { }
         }
@@ -161,27 +169,25 @@ namespace DroneSimulator
       drawCallback(MainArea);
     }
 
-    public void Move(int id, Vector3 pos, Vector4 tilt)
+    public void Move(int id, Vector3 pos, Vector4 tilt, Quaternion quaternion)
     {
       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;
 
-        d.PosXY.X = (int)pos.X;
-        d.PosXY.Y = MainArea.Height - (int)pos.Y;
+        d.PosXY.X = MainArea.Width / 2 + (int)pos.Y;
+        d.PosXY.Y = MainArea.Height / 2 - (int)pos.X;
         d.Height = (int)pos.Z;
 
         d.TiltXY.X = tilt.X * TO_RADI;
         d.TiltXY.Y = tilt.Y * TO_RADI;
         d.Azimuth = (int)tilt.Z;
+        d.Quaternion = quaternion;
 
         break;
       }