Mag active

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

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<byte[]> list = new List<byte[]>();
 
       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;

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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
+    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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
   }
 
-  public struct DataLocal
-  {
-    public DataHead Head;
-    public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-    public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
-  }
-
   public struct DataRange
   {
     public DataHead Head;
-    public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public float LiDAR; // Датчик посадки
 
-    public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    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;

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,22 @@
       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();
+      trackBar_Value = new TrackBar();
+      label4 = 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();
+      ((System.ComponentModel.ISupportInitialize)trackBar_Value).BeginInit();
       SuspendLayout();
       // 
       // timer_Test
@@ -92,7 +102,7 @@
       groupBox1.Dock = DockStyle.Top;
       groupBox1.Location = new Point(0, 0);
       groupBox1.Name = "groupBox1";
-      groupBox1.Size = new Size(275, 80);
+      groupBox1.Size = new Size(446, 80);
       groupBox1.TabIndex = 3;
       groupBox1.TabStop = false;
       groupBox1.Tag = "";
@@ -168,11 +178,20 @@
       groupBox2.Controls.Add(label1);
       groupBox2.Location = new Point(6, 86);
       groupBox2.Name = "groupBox2";
-      groupBox2.Size = new Size(78, 118);
+      groupBox2.Size = new Size(100, 118);
       groupBox2.TabIndex = 5;
       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 +246,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(112, 86);
       groupBox3.Name = "groupBox3";
-      groupBox3.Size = new Size(78, 118);
+      groupBox3.Size = new Size(103, 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 +325,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(221, 86);
       groupBox4.Name = "groupBox4";
-      groupBox4.Size = new Size(78, 118);
+      groupBox4.Size = new Size(103, 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 +480,93 @@
       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(330, 86);
+      groupBox5.Name = "groupBox5";
+      groupBox5.Size = new Size(104, 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:";
+      // 
+      // trackBar_Value
+      // 
+      trackBar_Value.Location = new Point(349, 240);
+      trackBar_Value.Maximum = 100;
+      trackBar_Value.Minimum = 1;
+      trackBar_Value.Name = "trackBar_Value";
+      trackBar_Value.Orientation = Orientation.Vertical;
+      trackBar_Value.Size = new Size(45, 195);
+      trackBar_Value.TabIndex = 25;
+      trackBar_Value.Value = 1;
+      // 
+      // label4
+      // 
+      label4.AutoSize = true;
+      label4.Location = new Point(347, 219);
+      label4.Name = "label4";
+      label4.Size = new Size(47, 15);
+      label4.TabIndex = 26;
+      label4.Text = "POWER";
       // 
       // Form_Main
       // 
       AutoScaleDimensions = new SizeF(7F, 15F);
       AutoScaleMode = AutoScaleMode.Font;
-      ClientSize = new Size(275, 447);
+      ClientSize = new Size(446, 447);
+      Controls.Add(label4);
+      Controls.Add(trackBar_Value);
+      Controls.Add(groupBox5);
       Controls.Add(button_MR);
       Controls.Add(button_ML);
       Controls.Add(label_Pow);
@@ -501,6 +593,9 @@
       groupBox4.ResumeLayout(false);
       groupBox4.PerformLayout();
       ((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit();
+      groupBox5.ResumeLayout(false);
+      groupBox5.PerformLayout();
+      ((System.ComponentModel.ISupportInitialize)trackBar_Value).EndInit();
       ResumeLayout(false);
       PerformLayout();
     }
@@ -546,5 +641,12 @@
     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;
+    private TrackBar trackBar_Value;
   }
 }

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());
     }
 
@@ -123,7 +128,7 @@ namespace DroneSimulator
 
     private void button_UU_MouseDown(object sender, MouseEventArgs e)
     {
-      const float pow = 0.1f;
+      float pow = ((float)trackBar_Value.Value) / 10.0f;
 
       if (sender == button_UU)
       {

DroneSimulator/Area.cs

@@ -0,0 +1,27 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace DroneSimulator
+{
+  internal class Area
+  {
+    public struct Poisition
+    {
+      public struct Freeze{ public static bool X, Y, Z; }
+    }
+
+    public struct Wind
+    {
+      public static bool Enable;
+      public struct Speed { public static float From, To; }
+      public static float Direction;
+      public static float Density;
+      public static float PosResist;
+      public static float RotResist;
+    }
+  }
+}

DroneSimulator/Drone.cs

@@ -1,37 +1,68 @@
-using System.CodeDom;
+using System.Diagnostics;
+using System.Net.Sockets;
 using System.Numerics;
 using System.Runtime.InteropServices;
-using System.Security.Cryptography;
+using static VisualData.VisualDrone;
 
 namespace DroneSimulator
 {
   internal class Drone
   {
     public int ID;
-    public float Mass; // Масса
-    public bool Active; // Живой?
-    public float Length; // Длинна лучей
+
+    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 float MaxPower; // Максимальная Тяга всех двигателей (КГ)
+    
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
 
     public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
     public float LaserRange; // Имитация: Дальномер
 
-    private uint DataTimer;
+    public Vector4 Orientation;
 
     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;
 
-    private static int CounterID = 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
+    {
+      static public float Mass; // Масса
+      static public float Length; // Длинна лучей
+      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)
     {
@@ -74,57 +105,86 @@ namespace DroneSimulator
       return mem;
     }
 
-    public struct DataVisual
+    public VisualData.VisualDrone GetVisual(int count, int index)
     {
-      public int ID; // Идентификатор
-      public float W, X, Y, Z; // Кватернион вращения
-      public float PosX, PosY, PosZ; // Координаты в пространстве
+      VisualData.VisualDrone drone = new VisualData.VisualDrone();
+
+      drone.Head.Size = Marshal.SizeOf(typeof(VisualData.VisualDrone));
+      drone.Head.Type = VisualData.VisualHead.VisualType.Drone;
+
+      drone.Count = count;
+      drone.Index = index;
+
+      drone.ID = ID;
+      drone.Color.A = 255; drone.Color.R = 255; drone.Color.G = 0; drone.Color.B = 0;
+
+      drone.Rotate.X = Quat.X; drone.Rotate.Y = Quat.Y; drone.Rotate.Z = Quat.Z; drone.Rotate.W = Quat.W;
+      drone.Position.X = PosXYZ.X; drone.Position.Y = PosXYZ.Y; drone.Position.Z = PosXYZ.Z;
+      drone.Scale = 1.0f;
+
+      drone.State = VisualData.VisualDrone.DroneState.Active;
+
+      drone.Power = Power;
+
+      return drone;
     }
 
-    public DataVisual GetVisual()
+    public static void StartThread()
     {
-      return new DataVisual() { ID = this.ID, W = this.Quat.W, X = this.Quat.X, Y = this.Quat.Y, Z = this.Quat.Z, PosX = this.PosXYZ.X, PosY = this.PosXYZ.Y, PosZ = this.PosXYZ.Z };
-    }
+      if (DroneThread != null) return;
 
-    private void ThreadFunction()
-    {
-      while (DroneThread != null)
-      {
-        Action(Environment.TickCount);
-        Thread.Sleep(1);
-      }
-    }
-
-    public Drone(int id)
-    {
-      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 = new Thread(new ThreadStart(Drone.ThreadFunction));
+      DroneThread.Priority = ThreadPriority.Highest;
       DroneThread.Start();
     }
 
-    public int Create(float power, float mass, float len)
+    public static void StopThread()
     {
-      Mass = mass;
-      Length = len;
-      MaxPower = power;
+      if (DroneThread == null) return;
 
+      DroneThread = null;
+    }
+
+    private static void ThreadFunction()
+    {
+      while (DroneThread != null)
+      {
+        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, Socket? client)
+    {
+      ID = id;
+      Timer = 0;
+      Client = client;
+    }
+
+    public int Create()
+    {
       Active = true;
 
       return ID;
     }
 
-    public void Close()
-    {
-      DroneThread = null;
-    }
-
     private float GetAngle(float a1, float a2, float az)
     {
       if (a2 == 0.0f && az == 0.0f)
@@ -154,11 +214,22 @@ 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);
 
-      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;
@@ -166,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;
@@ -180,67 +258,129 @@ namespace DroneSimulator
         flow += flow * 0.1f; // Воздушная подушка
       }
 
-      SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
+      float wind_x = 0, wind_y = 0, wind_z = 0;
+      float wind_p = 0, wind_r = 0, wind_w = 0;
+
+      if (Area.Wind.Enable)
+      {
+        Quaternion dir = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), Area.Wind.Direction * TO_RADI * 2);
+
+        Quaternion spd = new Quaternion(0, Area.Wind.Speed.From, 0, 0) * dir;
+
+        float spd_x = spd.X - SpdXYZ.X;
+        float spd_y = spd.Y - SpdXYZ.Y;
+        float spd_z = spd.Z - SpdXYZ.Z;
+
+        wind_x = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_x * MathF.Abs(spd_x));
+        wind_y = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_y * MathF.Abs(spd_y));
+        wind_z = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_z * MathF.Abs(spd_z));
+
+        wind_p = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.X * MathF.Abs(SpdPRY.X));
+        wind_r = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Y * MathF.Abs(SpdPRY.Y));
+        wind_w = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Z * MathF.Abs(SpdPRY.Z));
+
+        AccPRY.X -= wind_p; AccPRY.Y -= wind_r; AccPRY.Z -= wind_w;
+      }
+
+      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, pow.Y, pow.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;
 
       AccXYZ /= Gravity; // Вернуть измерения в G
 
-      if (PosXYZ.Z < 0)
+      if (Area.Poisition.Freeze.X) { SpdXYZ.X = 0; PosXYZ.X = 0; }
+      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)
       {
         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();
 
-      if (PosXYZ.Z < 0)
+      Orientation = ori;
+      float range = 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; // Перевернулся вверх ногами
         }
 
-        Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
-        //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
+        Quaternion grav = Quat * new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 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);
+        range = PosXYZ.Z / MathF.Cos(tilt);
+
+        if (tilt < 90 && ori.W > 0) LaserRange = range;
         else LaserRange = float.MaxValue;
       }
 
-      DataTimer = (uint)tick;
+      RealAcc.Update(Acc, tick);
+      RealGyr.Update(Gyr, tick);
+      RealRange.Update(LaserRange, tick);
+      RealBar.Update(PosXYZ.Z, tick);
+      RealPos.Update(PosXYZ, tick);
+
+      Vector2 xy = new Vector2(SpdXYZ.X * TO_GRAD / range, SpdXYZ.Y * TO_GRAD / range);
+      xy = RotateToZ(xy, true);
+
+      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)
@@ -248,13 +388,20 @@ 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)
     {
       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));
@@ -262,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));
@@ -276,10 +449,10 @@ 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 = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
-      acc.Time = DataTimer;
+      acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
+      acc.Time = RealAcc.timer;
 
       return getBytes(acc);
     }
@@ -291,10 +464,10 @@ 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 = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
-      gyr.Time = DataTimer;
+      gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
+      gyr.Time = RealGyr.timer;
 
       return getBytes(gyr);
     }
@@ -306,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);
     }
@@ -321,10 +494,10 @@ 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 = LaserRange;
-      range.Time = DataTimer;
+      range.LiDAR = RealRange.result;
+      range.Time = RealRange.timer;
 
       return getBytes(range);
     }
@@ -336,20 +509,132 @@ 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 = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
-      local.Time = DataTimer;
+      local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
+      local.Time = RealPos.timer;
 
       return getBytes(local);
     }
 
+    private byte[] SendDataBarometer()
+    {
+      DroneData.DataBar bar = new DroneData.DataBar();
+
+      bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
+      bar.Head.Mode = DroneData.DataMode.Response;
+      bar.Head.Type = DroneData.DataType.DataBar;
+      bar.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+
+      bar.Pressure = RealBar.result;
+      bar.Time = RealBar.timer;
+
+      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)(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);
+    }
+
+    private byte[] SendDataGPS()
+    {
+      DroneData.DataGPS gps = new DroneData.DataGPS();
+
+      gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
+      gps.Head.Mode = DroneData.DataMode.Response;
+      gps.Head.Type = DroneData.DataType.DataGPS;
+      gps.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+
+      GPS.Point p = new GPS.Point();
+      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;
+
+      g=GPS.localToGlobal(p, g);
+
+      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 + RealPos.result.Z;
+
+      DateTime tim = DateTime.Now;
+      gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
+
+      gps.Fix = GPS.State.Fix;
+      gps.SatVisible = GPS.State.SatVisible;
+      gps.SatUsed = GPS.State.SatUsed;
+      gps.Noise = GPS.State.Noise;
+      gps.Hdop = GPS.State.Hdop;
+      gps.Vdop = GPS.State.Vdop;
+      gps.Pdop = GPS.State.Pdop;
+
+      gps.Time = RealPos.timer;
+
+      return getBytes(gps);
+    }
+
+    private byte[] SendDataQuaternion()
+    {
+      DroneData.DataQuat quat = new DroneData.DataQuat();
+
+      quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
+      quat.Head.Mode = DroneData.DataMode.Response;
+      quat.Head.Type = DroneData.DataType.DataQuat;
+      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 = new byte[0];
+      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)
@@ -366,86 +651,25 @@ namespace DroneSimulator
             }
           }
 
-        case DroneData.DataType.DataGyr:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataGyr();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
+        case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;
 
-        case DroneData.DataType.DataMag:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataMag();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
+        case DroneData.DataType.DataMag: if (head.Mode == DroneData.DataMode.Request) return SendDataMag(); else return zero;
 
-        case DroneData.DataType.DataRange:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataRange();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
+        case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero;
 
-        case DroneData.DataType.DataLocal:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataLocal();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
+        case DroneData.DataType.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else return zero;
 
-        case DroneData.DataType.DataMotor4:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              // ... //
-              //
-              return zero;
-            }
-            else
-            {
-              // Пришли данные
-              RecvDataMotor4(body);
-              return zero;
-            }
-          }
+        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.DataGPS: if (head.Mode == DroneData.DataMode.Request) return SendDataGPS(); 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;
+
+        case DroneData.DataType.Ping: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       return zero;
@@ -454,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)
     {
@@ -475,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,4 +1,4 @@
-using System.Runtime.InteropServices;
+using System.Runtime.InteropServices;
 
 namespace DroneData
 {
@@ -9,13 +9,16 @@ 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, 
+    DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, DataGPS = 1008,
 
     // Input
-    DataMotor4 = 2001, DataMotor6 = 2002
+    DataMotor4 = 2001, DataMotor6 = 2002,
+
+    // State
+    DataQuat = 3001,
   };
 
   public struct DataHead
@@ -25,19 +28,29 @@ namespace DroneData
     public DataMode Mode;
     public DataType Type;
 
-    public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
+    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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
   }
@@ -45,9 +58,9 @@ namespace DroneData
   public struct DataGyr
   {
     public DataHead Head;
-    public XYZ Gyr;
+    public XYZ Gyr; // dps, угловые скорости
 
-    public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
   }
@@ -57,35 +70,75 @@ namespace DroneData
     public DataHead Head;
     public XYZ Mag;
 
-    public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
   }
 
-  public struct DataLocal
-  {
-    public DataHead Head;
-    public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-    public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
-  }
-
   public struct DataRange
   {
     public DataHead Head;
-    public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public float LiDAR; // m, Датчик посадки
 
-    public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
   }
 
+  public struct DataLocal
+  {
+    public DataHead Head;
+    public XYZ Local; // m, Локальные координаты
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
+  }
+
+  public struct DataBar
+  {
+    public DataHead Head;
+    public float Pressure; // Pa, Давление
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
+  }
+
+  public struct DataOF
+  {
+    public DataHead Head;
+    public float X, Y; // degree, Угловой сдвиг
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
+  }
+
+  public struct DataGPS
+  {
+    public DataHead Head;
+
+    public double Lat, Lon; // Координаты (градусы)
+    public float Alt; // Высота (метры)
+    public float Speed; // Скорость (м/с)
+    public int UTC; // Время UTC hhmmss
+
+    public byte Fix; // Тип решения 0-8 (NMEA Fix type)
+    public byte SatVisible; // Количество видимых спутников
+    public byte SatUsed; // Количество используемых спутников
+    public float Hdop, Vdop, Pdop; // Геометрический фактор
+    public float Noise; // Шум (db)
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
+  }
+
   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));
   }
@@ -93,8 +146,16 @@ 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));
   }
+
+  public struct DataQuat
+  {
+    public DataHead Head;
+    public float X, Y, Z, W; // Кватернион дрона
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat));
+  }
 }

DroneSimulator/FormMain.cs

@@ -1,4 +1,4 @@
-using System.Text;
+using System.Text;
 using System.Numerics;
 using System.Windows.Forms;
 using static System.Net.Mime.MediaTypeNames;
@@ -17,11 +17,20 @@ namespace DroneSimulator
     NetServerClients netServerClient = new NetServerClients();
     NetServerVisual netServerVisual = new NetServerVisual();
 
-    List<Drone> AllDrones = new List<Drone>();
-
     public Form_Main()
     {
       InitializeComponent();
+
+      numericUpDown_Acc_Update(null, null);
+      numericUpDown_Gyr_Update(null, null);
+      numericUpDown_Pos_Update(null, null);
+      numericUpDown_Bar_Update(null, null);
+      numericUpDown_Range_Update(null, null);
+      numericUpDown_OF_Update(null, null);
+      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)
@@ -35,25 +44,29 @@ namespace DroneSimulator
 
       if (data.Connect)
       {
-        Drone drone = new Drone(data.ID);
-        drone.Create(1.0f, 0.5f, 1.0f);
+        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)
         {
-          if (drone.ID != data.ID) continue;
-          drone.Close();
-
-          screen2D.RemoveDrone(data.ID);
-
-          AllDrones.Remove(drone);
-          break;
+          foreach (Drone drone in Drone.AllDrones)
+          {
+            if (drone.ID != data.ID) continue;
+            d = drone;
+            Drone.AllDrones.Remove(drone);
+            break;
+          }
         }
+
+        if (d != null) screen2D.RemoveDrone(d.ID);
       }
     }
 
@@ -63,11 +76,14 @@ namespace DroneSimulator
 
       Drone? drone = null;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
       {
-        if (d.ID != data.ID) continue;
-        drone = d;
-        break;
+        foreach (Drone d in Drone.AllDrones)
+        {
+          if (d.ID != data.ID) continue;
+          drone = d;
+          break;
+        }
       }
 
       if (drone == null) return;
@@ -99,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:
@@ -106,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);
@@ -133,19 +157,32 @@ namespace DroneSimulator
 
     private void timer_Test_Tick(object sender, EventArgs e)
     {
+      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;
 
-      foreach (Drone d in AllDrones)
+      listBox_Drones.Items.Clear();
+
+      try
       {
-        screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+        lock (Drone.AllDrones)
+        {
+          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)
     {
@@ -153,7 +190,7 @@ namespace DroneSimulator
 
       Invoke((MethodInvoker)delegate
       {
-        label_Clients_Num.Text = data.Count.ToString();
+        label_Visual_Num.Text = data.Count.ToString();
       });
 
       if (data.Connect)
@@ -170,12 +207,17 @@ namespace DroneSimulator
     {
       NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
 
-      foreach (Drone d in AllDrones)
-      {
-        Drone.DataVisual v = d.GetVisual();
+      int index = 0;
 
-        try { data.Client.Send(Drone.getBytes(v)); }
-        catch { }
+      lock (Drone.AllDrones)
+      {
+        foreach (Drone d in Drone.AllDrones)
+        {
+          VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++);
+
+          try { data.Client.Send(Drone.getBytes(v)); }
+          catch { }
+        }
       }
     }
 
@@ -204,5 +246,138 @@ namespace DroneSimulator
           }
       }
     }
+
+    private void numericUpDown_Bar_Update(object sender, EventArgs e)
+    {
+      RealMode.Barometer.RealSimulation = checkBox_Model_Bar_Real.Checked;
+
+      try { RealMode.Barometer.Pressure = uint.Parse(textBox_Bar_Pressure.Text); }
+      catch
+      {
+        RealMode.Barometer.Pressure = 102258;
+        MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error);
+      }
+
+      RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value;
+      RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value;
+      RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value;
+      RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked;
+    }
+
+    private void numericUpDown_Acc_Update(object sender, EventArgs e)
+    {
+      RealMode.Accelerometer.RealSimulation = checkBox_Model_Acc_Real.Checked;
+
+      RealMode.Accelerometer.Freq = (uint)numericUpDown_Acc_Freq.Value;
+      RealMode.Accelerometer.Noise = (float)numericUpDown_Acc_Noise.Value;
+      RealMode.Accelerometer.Lateness = (float)numericUpDown_Acc_Laten.Value;
+
+      RealMode.Accelerometer.ScaleLeft = (float)numericUpDown_Acc_Scale_Left.Value;
+      RealMode.Accelerometer.ScaleRight = (float)numericUpDown_Acc_Scale_Rigth.Value;
+    }
+
+    private void numericUpDown_Gyr_Update(object sender, EventArgs e)
+    {
+      RealMode.Gyroscope.RealSimulation = checkBox_Model_Gyr_Real.Checked;
+
+      RealMode.Gyroscope.Freq = (uint)numericUpDown_Gyr_Freq.Value;
+      RealMode.Gyroscope.Noise = (float)numericUpDown_Gyr_Noise.Value;
+      RealMode.Gyroscope.Lateness = (float)numericUpDown_Gyr_Laten.Value;
+
+      RealMode.Gyroscope.Shift.X = (float)numericUpDown_Gyr_Shift_X.Value;
+      RealMode.Gyroscope.Shift.Y = (float)numericUpDown_Gyr_Shift_Y.Value;
+      RealMode.Gyroscope.Shift.Z = (float)numericUpDown_Gyr_Shift_Z.Value;
+    }
+
+    private void numericUpDown_Pos_Update(object sender, EventArgs e)
+    {
+      RealMode.Position.RealSimulation = checkBox_Model_Pos_Real.Checked;
+
+      RealMode.Position.Freq = (uint)numericUpDown_Pos_Freq.Value;
+      RealMode.Position.Noise = (float)numericUpDown_Pos_Noise.Value;
+      RealMode.Position.Lateness = (float)numericUpDown_Pos_Laten.Value;
+      RealMode.Position.Enable = checkBox_Pos_Enable.Checked;
+    }
+
+    private void numericUpDown_Range_Update(object sender, EventArgs e)
+    {
+      RealMode.Range.RealSimulation = checkBox_Model_Range_Real.Checked;
+
+      RealMode.Range.Freq = (uint)numericUpDown_Range_Freq.Value;
+      RealMode.Range.Noise = (float)numericUpDown_Range_Noise.Value;
+      RealMode.Range.Lateness = (float)numericUpDown_Range_Laten.Value;
+      RealMode.Range.Enable = checkBox_Range_Enable.Checked;
+
+      RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
+    }
+
+    private void numericUpDown_OF_Update(object sender, EventArgs e)
+    {
+      RealMode.OpticalFlow.RealSimulation = checkBox_Model_OF_Real.Checked;
+
+      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;
+      RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
+    }
+
+    private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
+    {
+      Area.Poisition.Freeze.X = checkBox_Area_Freeze_X.Checked;
+      Area.Poisition.Freeze.Y = checkBox_Area_Freeze_Y.Checked;
+      Area.Poisition.Freeze.Z = checkBox_Area_Freeze_Z.Checked;
+    }
+
+    private void numericUpDown_Area_Wind_Update(object sender, EventArgs e)
+    {
+      Area.Wind.Enable = checkBox_Area_Wind_Enable.Checked;
+      Area.Wind.Speed.From = (float)numericUpDown_Area_Wind_Speed_From.Value;
+      Area.Wind.Speed.To = (float)numericUpDown_Area_Wind_Speed_To.Value;
+      Area.Wind.Direction = (float)numericUpDown_Area_Wind_Direction.Value;
+      Area.Wind.Density = (float)numericUpDown_Area_Wind_Density.Value;
+      Area.Wind.PosResist = ((float)numericUpDown_Area_Wind_PosResist.Value) / 1000.0f;
+      Area.Wind.RotResist = ((float)numericUpDown_Area_Wind_RotResist.Value) / 1000.0f;
+    }
+
+    private void numericUpDown_GPS_ValueChanged(object sender, EventArgs e)
+    {
+      GPS.Home.Lat = (double)numericUpDown_GPS_Lat.Value;
+      GPS.Home.Lon = (double)numericUpDown_GPS_Lon.Value;
+      GPS.Home.Alt = (float)numericUpDown_GPS_Alt.Value;
+
+      GPS.State.Fix = (byte)numericUpDown_GPS_Fix.Value;
+      GPS.State.SatVisible = (byte)numericUpDown_GPS_Vis.Value;
+      GPS.State.SatUsed = (byte)numericUpDown_GPS_Use.Value;
+      GPS.State.Noise = (float)numericUpDown_GPS_Noise.Value;
+      GPS.State.Hdop = (float)numericUpDown_GPS_HDOP.Value;
+      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;
+    }
+
+    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/FormMain.resx

@@ -117,14 +117,11 @@
   <resheader name="writer">
     <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </resheader>
-  <metadata name="menuStrip_Menu.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
-    <value>17, 17</value>
-  </metadata>
   <metadata name="timer_Test.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
     <value>162, 5</value>
   </metadata>
   <metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
-    <value>25</value>
+    <value>78</value>
   </metadata>
   <assembly alias="System.Drawing" name="System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" />
   <data name="$this.Icon" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">

DroneSimulator/GPS.cs

@@ -0,0 +1,74 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace DroneSimulator
+{
+  internal class GPS
+  {
+    static double PI = 3.14159265358979323846;
+    public struct Home
+    {
+      public static double Lat, Lon;
+      public static float Alt;
+    }
+
+    public struct State
+    {
+      public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
+      public static byte SatVisible; // Количество видимых спутников
+      public static byte SatUsed; // Количество используемых спутников
+      public static float Hdop, Vdop, Pdop; // Геометрический фактор
+      public static float Noise; // Шум (db)
+    }
+
+    public struct GlobalCoords
+    {
+      public double latitude, longitude;
+    }
+
+    public struct Point
+    {
+      public double x, y;
+    }
+
+    // Конвертация градусов в радианы
+    static double deg2rad(double deg)
+    {
+      return deg * PI / 180.0;
+    }
+
+    // Конвертация радиан в градусы
+    static double rad2deg(double rad)
+    {
+      return rad * 180.0 / PI;
+    }
+
+    // Перевод локальных координат в глобальные
+    public static GlobalCoords localToGlobal(Point local, GlobalCoords origin)
+    {
+      const double er = 6371000; // Radius of the earth in m
+
+      // Преобразование приращений координат
+      double dLat = local.x / er; // В радианах
+      double originLatRad = deg2rad(origin.latitude);
+
+      // Вычисление новой широты
+      double newLatRad = originLatRad + dLat;
+      double newLat = rad2deg(newLatRad);
+
+      // Вычисление новой долготы (с использованием средней широты для точности)
+      double avgLatRad = (originLatRad + newLatRad) / 2.0;
+      double dLon = local.y / (er * Math.Cos(avgLatRad)); // В радианах
+      double newLon = origin.longitude + rad2deg(dLon);
+      GlobalCoords coord = new GlobalCoords();
+      coord.latitude = newLat;
+      coord.longitude = newLon;
+      return coord;
+    }
+  }
+}

DroneSimulator/RealMode.cs

@@ -0,0 +1,534 @@
+using Microsoft.VisualBasic.Devices;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+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
+{
+  internal class RealMode
+  {
+
+
+    internal class Accelerometer
+    {
+      public static uint Freq;
+      public static float Noise;
+      public static float ScaleLeft;
+      public static float ScaleRight;
+      public static float Lateness;
+      public static bool RealSimulation;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      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(Vector3 value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        float scale = (ScaleRight - ScaleLeft) / 2;
+        float shift = scale + ScaleLeft;
+
+        value.X = (value.X * scale) + shift;
+        value.Y = (value.Y * scale) + shift;
+        value.Z = (value.Z * scale) + shift;
+
+        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 = time - last;
+        while (true)
+        {
+          laten[index] = value;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        value = laten[move];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq <= time)
+        {
+          result = value;
+          timer = time;
+        }
+      }
+    }
+
+    internal class Gyroscope
+    {
+      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();
+
+      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(Vector3 value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        value.X += Shift.X;
+        value.Y += Shift.Y;
+        value.Z += Shift.Z;
+
+        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 = time - last;
+        while (true)
+        {
+          laten[index] = value;
+          clock--;
+          if (clock == 0) break;
+          index++;
+          if (index >= count) index = 0;
+        }
+        last = time;
+
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        value = laten[move];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq <= time)
+        {
+          result = value;
+          timer = time;
+        }
+      }
+    }
+
+    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
+    {
+      public static bool Enable;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+      public static bool RealSimulation;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      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(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;
+        }
+
+        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 Barometer
+    {
+      public static bool Enable;
+      public static float Pressure;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+      public static bool RealSimulation;
+      public static float Temperature = 25.0f;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private float[] laten = new float[count];
+      private int index = 0;
+
+      public uint timer = 0;
+      public float result;
+
+      public void Update(float value, uint time)
+      {
+        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;
+        }
+
+        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 OpticalFlow
+    {
+      public static bool Enable;
+      public static float MaxHeight;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+      public static float Lens;
+      public static bool RealSimulation;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private Vector2[] laten = new Vector2[count];
+      private int index = 0;
+
+      public uint delay = 0;
+
+      public uint timer = 0;
+      public Vector2 result;
+      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;
+          timer = time;
+          return true;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return true;
+        }
+
+        int move = (int)(Lateness * count);
+        move = index - move;
+        while (move < 0) move += count;
+        result = laten[move];
+
+        uint freq = count / Freq;
+        if (timer + freq <= time)
+        {
+          timer = time;
+          return true;
+        }
+
+        return false;
+      }
+    }
+
+    internal class Range
+    {
+      public static bool Enable;
+      public static float MaxHeight;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+      public static bool RealSimulation;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private float[] laten = new float[count];
+      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;
+        }
+
+        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;
+        }
+      }
+    }
+  }
+}

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))
@@ -106,78 +149,45 @@ namespace DroneSimulator
 
         foreach (var d in DroneList)
         {
-          if (d.Azimuth >= 360) d.Azimuth -= 360;
-          var bmp = RotateImage(d.Drone, d.Azimuth);
-
-          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));
-
-          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)
+          try
           {
-            x1 = (int)(Math.Sin(tilt.Y) * 130);
-            x3 = (int)(Math.Sin(tilt.Y) * 130);
-          }
-          else
-          {
-            x2 = (int)(Math.Cos(tilt.Y) * 130);
-          }
+            if (d.Azimuth >= 360) d.Azimuth -= 360;
 
-          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);
-          }
+            var bmp = DrawImageByQuaternion(d.Drone, d.Quaternion);
 
-          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.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
 
-          g.DrawImage(bmp, dest);
+            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, d.PosXY.X - d.Drone.Width / 2, d.PosXY.Y - d.Drone.Height / 2); // Draw the transformed image
+
+          }
+          catch { }
         }
       }
 
       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;
       }

DroneSimulator/VisualData.cs

@@ -0,0 +1,39 @@
+using System.Numerics;
+
+namespace VisualData
+{
+  public struct VisualHead
+  {
+    public enum VisualType : int { None = 0, Drone = 1 } // Тип объекта
+
+    public int Size; // Размер данных этой структуры в байтах (проверка для соответствия передачи структуры)
+
+    public VisualType Type; // Тип передоваемого объекта
+  }
+
+  public struct VisualDrone
+  {
+    public VisualHead Head;
+    public enum DroneState : int { Dead = 0, Disabled = 1, Waiting = 2, Active = 3 } // Переключения типа 3D модели
+
+    public int Count; // Всего дронов на полигоне
+    public int Index; // Номер дрона
+
+    public int ID; // Идентификатор (для привязки камеры)
+
+    public struct ARGB { public byte A, R, G, B; }
+    public struct Quat { public float X, Y, Z, W; }
+    public struct Vect3 { public float X, Y, Z; }
+
+    public ARGB Color; // Цвет корпуса
+    public Quat Rotate; // Кватернион вращения
+    public Vect3 Position; // Координаты в пространстве
+
+    public float Scale; // Масштаб модельки (1=оригинальный)
+
+    public DroneState State; // Тип прорисовываемой модели
+
+    public float Power; // Скорость всех двигателей
+  }
+
+}