Physics

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();
+      groupBox5 = new GroupBox();
-      label_time_range = new Label();
+      label_time_of = new Label();
-      label_time_gyr = 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;
+      groupBox5.Controls.Add(label_time_of);
-      label_time_acc.Location = new Point(6, 100);
+      groupBox5.Controls.Add(label_OF_Y);
-      label_time_acc.Name = "label_time_acc";
+      groupBox5.Controls.Add(label17);
-      label_time_acc.Size = new Size(13, 15);
+      groupBox5.Controls.Add(label_OF_X);
-      label_time_acc.TabIndex = 25;
+      groupBox5.Controls.Add(label19);
-      label_time_acc.Text = "0";
+      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_of.AutoSize = true;
-      label_time_range.Location = new Point(6, 100);
+      label_time_of.Location = new Point(6, 100);
-      label_time_range.Name = "label_time_range";
+      label_time_of.Name = "label_time_of";
-      label_time_range.Size = new Size(13, 15);
+      label_time_of.Size = new Size(13, 15);
-      label_time_range.TabIndex = 27;
+      label_time_of.TabIndex = 27;
-      label_time_range.Text = "0";
+      label_time_of.Text = "0";
       // 
-      // label_time_gyr
+      // label_OF_Y
       // 
-      label_time_gyr.AutoSize = true;
+      label_OF_Y.AutoSize = true;
-      label_time_gyr.Location = new Point(3, 100);
+      label_OF_Y.Location = new Point(19, 45);
-      label_time_gyr.Name = "label_time_gyr";
+      label_OF_Y.Name = "label_OF_Y";
-      label_time_gyr.Size = new Size(13, 15);
+      label_OF_Y.Size = new Size(13, 15);
-      label_time_gyr.TabIndex = 26;
+      label_OF_Y.TabIndex = 7;
-      label_time_gyr.Text = "0";
+      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)
       {

DroneClientCpp/Drone.cpp

@@ -45,14 +45,12 @@ namespace DroneClient {
     // Реализация приватного метода SendDataMotor4
     array<Byte>^ Drone::SendDataMotor4()
     {
-
         DroneData::DataMotor4 mot4;
 
         mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
         mot4.Head.Mode = DroneData::DataMode::Response;
         mot4.Head.Type = DroneData::DataType::DataMotor4;
-        updateData();
+
-        setMotors();
         mot4.UL = MotorUL;
         mot4.UR = MotorUR;
         mot4.DL = MotorDL;
@@ -160,92 +158,6 @@ namespace DroneClient {
         return zero;
     }
 
-
-
-    //void Drone::setMotors()
-    //{
-    //    array<UInt16>^ ch = joy->Channels;   // ссылка на тот же массив
-    //    UInt16 pow = (ch[2] - 1000) / 10;
-    //    float fpow = float(pow)/ 100.0f;
-
-    //   const float maxAngle = 20.0f;
-    //  
-
-    //   desPitch = (ch[1] - 1500) * maxAngle / 500;
-    //   desRoll = (ch[0] - 1499) * maxAngle / 500;
-
-
-    //   float errorPitch, errorRoll, forceRoll, forcePitch;
-    //    errorPitch = desPitch -pitch;
-    //    errorRoll = desRoll - roll;
-
-    //    static float PRegulator = 0.001f;
- 
-    //    forcePitch = PRegulator * errorPitch;
-    //    forceRoll = PRegulator * errorRoll;
-
-
-    //   MotorUL = fpow-forcePitch + forceRoll;
-    //   MotorUR = fpow - forcePitch - forceRoll;
-    //   MotorDL = fpow + forcePitch + forceRoll;
-    //   MotorDR = fpow + forcePitch - forceRoll;
-    //}
-
-
-    /* ───────────── вспомогательная функция ───────────── */
-    static float Clamp01(float v)
-    {
-        if (v < 0.0f) return 0.0f;
-        if (v > 1.0f) return 1.0f;
-        return v;
-    }
-
-    /* ───────────── PD-регулятор и микширование ───────── */
-    void Drone::setMotors()
-    {
-        /* ---------- входные каналы --------------------- */
-        array<UInt16>^ ch = joy->Channels;
-        const float fpow = (ch[2] - 1000) * 0.001f;       // 0…1
-
-        /* ---------- желаемые углы ---------------------- */
-        const float maxAngle = 20.0f;
-         desPitch = (ch[1] - 1500) * maxAngle / 500.0f;
-         desRoll = (ch[0] - 1499) * maxAngle / 500.0f;
-
-         
-        /* ---------- PD-регулятор ----------------------- */
-        static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
-        const float errPitch = desPitch - pitch;
-        const float errRoll = desRoll - roll;
-
-        const float dt = 0.01f;                // период 10 мс (100 Гц)
-        const float dPitch = (errPitch - prevErrPitch) / dt;
-        const float dRoll = (errRoll - prevErrRoll) / dt;
-
-        const float Kp = 0.115f;               
-        const float Kd = 0.0f;
-
-        const float forcePitch = Kp * errPitch + Kd * dPitch;
-        const float forceRoll = Kp * errRoll + Kd * dRoll;
-
-        prevErrPitch = errPitch;
-        prevErrRoll = errRoll;
-
-        /* ---------- распределение на моторы ------------ */
-        MotorUL = fpow - forcePitch + forceRoll;
-        MotorUR = fpow - forcePitch - forceRoll;
-        MotorDL = fpow + forcePitch + forceRoll;
-        MotorDR = fpow + forcePitch - forceRoll;
-
-        //MotorUL = Clamp01(MotorUL);
-        //MotorUR = Clamp01(MotorUR);
-        //MotorDL = Clamp01(MotorDL);
-        //MotorDR = Clamp01(MotorDR);
-    }
-
-
-
-
     // Реализация конструктора
     Drone::Drone()
     {
@@ -254,11 +166,6 @@ namespace DroneClient {
         DroneStreamHead.Mode = DroneData::DataMode::None;
         DroneStreamHead.Size = 0;
         DroneStreamHead.Type = DroneData::DataType::None;
-
-        this->joy = gcnew Joypad();
-
-        this->joy->Start("COM7", 115200);
-          
     }
 
     // Реализация метода DataStream
@@ -345,16 +252,4 @@ namespace DroneClient {
 
       return send;
     }
-
-    void Drone::updateData(){
-        Vec3 acc{ this->AccX,this->AccY, this->AccZ };
-        Vec3 gyr{ this->GyrX,this->GyrY, this->GyrZ };
-        Vec3 mag{ 1,0, 0};
-        ORI result = WorkAccGyroMag(acc, gyr, mag, 0, 0.01);
-        this->pitch = result.Pitch;
-        this->roll = -result.Roll;
-        this->yaw = result.Yaw;
-
-
-    }
 }

DroneClientCpp/Drone.h

@@ -3,14 +3,10 @@
 #include "DroneData.h"
 #include <Windows.h>
 #include <vcclr.h>
-#include "Orientation.h"
-#include "joypad.h"
-
 
 #using <System.dll>
 #using <mscorlib.dll>
 
-
 using namespace System;
 using namespace System::Collections::Generic;
 using namespace System::Runtime::InteropServices;
@@ -24,18 +20,12 @@ namespace DroneClient {
         float GyrX, GyrY, GyrZ;
         unsigned int TimeAcc, TimeGyr;
 
-
         float PosX, PosY;
         float LaserRange;
         unsigned int TimeRange;
 
         float MotorUL, MotorUR, MotorDL, MotorDR;
 
-        float pitch, roll, yaw;
-
-        float desPitch, desRoll, desYaw;
-
-        Joypad^ joy;
         static array<Byte>^ GetBytes(Object^ data);
         static Object^ FromBytes(array<Byte>^ arr, Type^ type);
 
@@ -47,9 +37,6 @@ namespace DroneClient {
         array<Byte>^ RecvDataLocal(array<Byte>^ data);
         array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
 
-        void setMotors();
-
-
         literal int DroneStreamCount = 512;
         array<Byte>^ DroneStreamData;
         int DroneStreamIndex;
@@ -60,8 +47,5 @@ namespace DroneClient {
 
         System::Collections::Generic::List<array<Byte>^>^ DataStream(array<Byte>^ data, int size);
         array<Byte>^ SendRequest();
-        void updateData();
-
-
     };
 }

DroneClientCpp/DroneClientCpp.vcxproj

@@ -121,7 +121,6 @@
     <ClCompile Include="Drone.cpp" />
     <ClCompile Include="FormMain.cpp" />
     <ClCompile Include="NetClient.cpp" />
-    <ClCompile Include="Orientation.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Drone.h" />
@@ -129,10 +128,7 @@
     <ClInclude Include="FormMain.h">
       <FileType>CppForm</FileType>
     </ClInclude>
-    <ClInclude Include="joypad.h" />
     <ClInclude Include="NetClient.h" />
-    <ClInclude Include="Orientation.h" />
-    <ClInclude Include="WsServer.h" />
   </ItemGroup>
   <ItemGroup>
     <EmbeddedResource Include="FormMain.resx">

DroneClientCpp/DroneClientCpp.vcxproj.filters

@@ -24,9 +24,6 @@
     <ClCompile Include="Drone.cpp">
       <Filter>Исходные файлы</Filter>
     </ClCompile>
-    <ClCompile Include="Orientation.cpp">
-      <Filter>Исходные файлы</Filter>
-    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="FormMain.h">
@@ -41,14 +38,5 @@
     <ClInclude Include="Drone.h">
       <Filter>Файлы заголовков</Filter>
     </ClInclude>
-    <ClInclude Include="WsServer.h">
-      <Filter>Файлы заголовков</Filter>
-    </ClInclude>
-    <ClInclude Include="Orientation.h">
-      <Filter>Файлы заголовков</Filter>
-    </ClInclude>
-    <ClInclude Include="joypad.h">
-      <Filter>Файлы заголовков</Filter>
-    </ClInclude>
   </ItemGroup>
 </Project>

DroneClientCpp/FormMain.cpp

@@ -10,4 +10,3 @@ int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) {
     Application::Run(gcnew FormMain);
     return 0;
 }
-

DroneClientCpp/FormMain.resx

@@ -123,7 +123,4 @@
   <metadata name="timer1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
     <value>310, 17</value>
   </metadata>
-  <metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
-    <value>25</value>
-  </metadata>
 </root>

DroneClientCpp/Orientation.cpp

@@ -1,254 +0,0 @@
-#include "orientation.h"
-#include <math.h>
-
-static const float PI = 3.14159265359f;
-static const float TO_DEG = 180.0f / PI;
-static const float TO_RAD = PI / 180.0f;
-static const float R = 8.314f;
-static const float M = 0.029f;
-static const float g = 9.80665f;
-static const float ro = 1.189f;
-
-struct Quaternion
-{
-    float w, x, y, z;
-};
-
-static Quaternion qCurrent = { 1, 0, 0, 0 };
-
-static const float period = 0.005f;
-static bool isFirst = true;
-
-static void vecNormalize(Vec3& v)
-{
-    float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
-    if (n > 1e-9f)
-    {
-        v.x /= n;
-        v.y /= n;
-        v.z /= n;
-    }
-}
-
-static Vec3 vecCross(const Vec3& a, const Vec3& b)
-{
-    return
-    {
-      a.y * b.z - a.z * b.y,
-      a.z * b.x - a.x * b.z,
-      a.x * b.y - a.y * b.x
-    };
-}
-
-static void normalizeQuaternion(Quaternion& q)
-{
-    float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
-    if (norm > 1e-12f)
-    {
-        q.w /= norm;
-        q.x /= norm;
-        q.y /= norm;
-        q.z /= norm;
-    }
-}
-
-static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
-{
-    Quaternion r;
-    r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
-    r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
-    r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
-    r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
-    return r;
-}
-
-static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
-{
-    Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });
-
-    return r;
-}
-
-static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
-{
-    Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, Quaternion{ 0, In.x, In.y, In.z }), q);
-
-    return { r.x, r.y, r.z };
-}
-
-static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
-{
-    Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, In), q);
-
-    return r;
-}
-
-static Quaternion createYawQuaternion(float angle)
-{
-    Quaternion q;
-
-    q.w = cosf(angle);
-    q.x = 0.0f;
-    q.y = 0.0f;
-    q.z = sinf(angle);
-
-    return q;
-}
-
-static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity)
-{
-    float acc = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
-    if (acc > (1.0f + gravity) || acc < (1.0f - gravity)) return current;
-
-    vecNormalize(a);
-
-    Vec3 g{ 0, 0, 1 };
-    Vec3 axis = vecCross(g, a);
-    float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
-    Quaternion q = { w, axis.x, axis.y, axis.z };
-    normalizeQuaternion(q);
-
-    Quaternion qYaw{ current.w, 0, 0, current.z };
-
-    return quaternionMultiply(q, qYaw); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Z
-}
-
-static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
-{
-    Quaternion Mapp = current;
-    Quaternion Spd{ 0, wx, wy, wz };
-    Quaternion aq = quaternionMultiply(Spd, Mapp);
-
-    Mapp.w -= 0.5f * aq.w;
-    Mapp.x -= 0.5f * aq.x;
-    Mapp.y -= 0.5f * aq.y;
-    Mapp.z -= 0.5f * aq.z;
-
-    normalizeQuaternion(Mapp);
-    return Mapp;
-}
-
-static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
-{
-
-    float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
-
-    Quaternion q2_ = q2;
-    if (dot < 0)
-    {
-        q2_.w = -q2.w;
-        q2_.x = -q2.x;
-        q2_.y = -q2.y;
-        q2_.z = -q2.z;
-    }
-
-    Quaternion r;
-    r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
-    r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
-    r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
-    r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;
-
-    normalizeQuaternion(r);
-
-    return r;
-}
-
-inline float GetAngle(float a1, float a2, float az)
-{
-    if (a2 == 0.0f && az == 0.0f) return a1 > 0.0f ? 90.0f : -90.0f;
-    return atanf(a1 / sqrtf(a2 * a2 + az * az)) * TO_DEG;
-}
-
-static Vec3 quaternionToPitchRollYaw(const Quaternion& q, float& Upside)
-{
-    Quaternion pry = rotateVectorByQuaternion(q, { 0, 0, 1 });
-
-    Upside = (pry.z > 0.0f) ? 1.0f : -1.0f;
-
-    float yaw = 2 * atan2f(q.z, q.w) * TO_DEG;
-    if (yaw < 0.0f) yaw = 360.0f + yaw;
-
-    return // Sovereign orientation 
-    {
-      GetAngle(pry.y, pry.x, pry.z),  // Pitch
-      GetAngle(-pry.x, pry.y, pry.z), // Roll
-      yaw                             // Yaw
-    };
-}
-
-static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
-{
-    static Quaternion yq = createYawQuaternion(shift * TO_RAD);
-
-    vecNormalize(mag);
-
-    Quaternion mQ = { 0, mag.x, mag.y, mag.z };
-
-    Quaternion mW = backRotateVectorByQuaternion2(q, mQ);
-
-    mW = quaternionMultiply(mW, yq); // Shifting the axes to the true north
-
-    float gamma = mW.x * mW.x + mW.y * mW.y;
-    float beta = sqrtf(gamma + mW.x * sqrtf(gamma));
-
-    Quaternion mD
-    {
-      beta / (sqrtf(2.0f * gamma)),
-      0.0f,
-      0.0f,
-      mW.y / (sqrtf(2.0f) * beta),
-    };
-
-    mD.w = (1.0f - alpha) + alpha * mD.w;
-    mD.z = alpha * mD.z;
-
-    if (mD.w != mD.w || mD.x != mD.x || mD.y != mD.y || mD.z != mD.z) return;
-
-    q = quaternionMultiply(q, mD);
-    normalizeQuaternion(q);
-}
-
-// WorkAccGyro({DataIMU.Acc.X, DataIMU.Acc.Y, DataIMU.Acc.Z}, {DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z}, {-DataIMU.Mag.X, DataIMU.Mag.Y, DataIMU.Mag.Z}, 0.01);
-ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
-{
-    float wx = gyr.x * 500 / 32768 * 1.21f * (PI / 180) * period;
-    float wy = gyr.y * 500 / 32768 * 1.21f * (PI / 180) * period;
-    float wz = gyr.z * 500 / 32768 * 1.21f * (PI / 180) * period;
-
-    Vec3 aB = acc;
-
-    Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f); // Tolerance for gravity deviation 5%
-
-    qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
-
-    Quaternion qFused = nlerp(qCurrent, qAcc, alpha);
-
-    if (isFirst)
-    {
-        qFused = qAcc;
-        isFirst = false;
-    }
-
-    qCurrent = qFused;
-
-    addMagneto(qCurrent, mag, alpha, mag_shift);
-
-    float up;
-    Vec3 pry = quaternionToPitchRollYaw(qCurrent, up);
-
-    Vec3 ine = backRotateVectorByQuaternion(qCurrent, aB);
-    return
-    {
-      sqrtf(pry.x * pry.x + pry.y * pry.y) * up,
-      pry.x, pry.y, pry.z,
-      ine.x, ine.y, ine.z
-
-    };
-}
-
-float calculateHeight(float bar, float temp)
-{
-    static double firstBar = bar;
-    return (R * (temp + 273) / (M * g)) * log(firstBar / bar);
-
-}

DroneClientCpp/Orientation.h

@@ -1,15 +0,0 @@
-#pragma once
-struct Vec3 {
-	float x, y, z;
-};
-
-struct ORI
-{
-	float Tilt; // Earth's plane tilt
-	float Pitch, Roll, Yaw; // Sovereign orientation (not Euler)
-	float IneX, IneY, IneZ; // Inertial accelerations
-};
-
-ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha);
-
-float calculateHeight(float bar, float temp);

DroneClientCpp/WsServer.h

@@ -1,156 +0,0 @@
-#pragma once
-#include <Windows.h>
-#using <System.dll>
-#using <System.Net.Http.dll>
-
-#pragma pack(push, 1)
-struct angles_native { float pitch, roll, yaw; };
-#pragma pack(pop)
-
-using namespace System;
-using namespace System::Collections::Generic;
-using namespace System::Net;
-using namespace System::Net::WebSockets;
-using namespace System::Threading;
-using namespace System::Text;
-using namespace DroneClient;     // ← поправьте, если у вас другое
-
-
-
-public ref class WsServer      // имя можно оставить тем же
-{
-public:
-    WsServer() : _connected(false) {}
-
-    /* ---------- подключение --------------------------------------- */
-    bool Connect(String^ uri)
-    {
-        if (_connected) return true;           // уже подключены
-
-        _ws = gcnew ClientWebSocket();
-        _cts = gcnew CancellationTokenSource();
-
-        try
-        {
-            // 1) запускаем асинхронное соединение
-            System::Threading::Tasks::Task^ t =
-                _ws->ConnectAsync(gcnew Uri(uri), _cts->Token);
-
-            // 2) ждём завершения
-            t->Wait();                         // здесь AggregateException «обёртывает»
-            // все реальные ошибки
-            _connected = true;
-        }
-        catch (AggregateException^ ag)
-        {
-            // сообщений может быть несколько – выводим главное
-            Exception^ ex = ag->InnerExceptions->Count
-                ? ag->InnerExceptions[0] : ag;
-
-            System::String^ msg = ex->Message;
-
-            // типовые причины:
-            // • WebSocketException (ошибка DNS / connection refused / timeout)
-            // • NotSupportedException  (Windows 7: платформа без WebSocket‑клиента)
-            // • InvalidOperationException (неверный URI)
-            // → выводим в Debug и просто возвращаем false
-            System::Diagnostics::Debug::WriteLine("Connect error: " + msg);
-            _connected = false;
-        }
-        catch (Exception^ ex)          // на всякий случай – «прочие»
-        {
-            System::Diagnostics::Debug::WriteLine("Connect error: " + ex->Message);
-            _connected = false;
-        }
-        return _connected;
-    }
-
-    /* ---------- отключение ---------------------------------------- */
-    void Disconnect()
-    {
-        if (!_connected) return;
-
-        try
-        {
-            _ws->CloseAsync(WebSocketCloseStatus::NormalClosure,
-                "bye", CancellationToken::None)->Wait();
-        }
-        catch (Exception^) { /* игнор */ }
-
-        _ws = nullptr;
-        _cts = nullptr;
-        _connected = false;
-    }
-
-    /* ---------- быстрая отправка строки --------------------------- */
-    bool SendString(String^ msg)
-    {
-        if (!_connected) return false;
-
-        array<Byte>^ bytes = Encoding::UTF8->GetBytes(msg);
-        try
-        {
-            _ws->SendAsync(ArraySegment<Byte>(bytes),
-                WebSocketMessageType::Text,
-                true, CancellationToken::None)->Wait();
-            return true;
-        }
-        catch (Exception^) { return false; }
-    }
-
-    /* ---------- состояние ---------------------------------------- */
-    property bool IsConnected
-    {
-        bool get() { return _connected; }
-    }
-
-    void SendAnglesBinary(float p, float r, float y)
-    {
-        angles_native a = { p, r, y };
-
-        // безопасно копируем struct → byte[]
-        array<System::Byte>^ buf = gcnew array<System::Byte>(sizeof(a));
-        System::Runtime::InteropServices::GCHandle h =
-            System::Runtime::InteropServices::GCHandle::Alloc(buf,
-                System::Runtime::InteropServices::GCHandleType::Pinned);
-
-        memcpy(h.AddrOfPinnedObject().ToPointer(), &a, sizeof(a));
-        h.Free();
-
-        // шлём как Binary
-        _ws->SendAsync(System::ArraySegment<Byte>(buf),
-            System::Net::WebSockets::WebSocketMessageType::Binary,
-            true, System::Threading::CancellationToken::None)->Wait();
-    }
-
-    void TxLoop(System::Object^ param)
-    {
-        Drone^ d = safe_cast<Drone^>(param);
-        // 20 мс = 50 Гц
-        const int PERIOD_MS = 20;
-
-        // примерные значения, чтобы «шевелились»
-        float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
-
-        while (_runTx && _connected)
-        {
-            float p = System::Threading::Volatile::Read(d->pitch);
-            float r = System::Threading::Volatile::Read(d->roll);
-            float y = System::Threading::Volatile::Read(d->yaw);
-
-           SendAnglesBinary(p,r, y);
-           System::Threading::Thread::Sleep(PERIOD_MS);
-        }
-    }
-
-
-private:
-    ClientWebSocket^ _ws;
-    CancellationTokenSource^ _cts;
-    bool                           _connected;
-
-public:
-    // --- рядом с тем, где уже лежит wsClient ---
-    System::Threading::Thread^ _txThread = nullptr;  // поток передатчик
-    bool                        _runTx = false;    // признак «живого» цикла
-};

DroneClientCpp/joypad.h

@@ -1,137 +0,0 @@
-// joypad.h  ───────────────────────────────────────────────────────────
-#pragma once
-#include <iostream>
-#include <iomanip>
-#include <Windows.h>            //  <- для AllocConsole
-#using <System.dll>
-
-using namespace System;
-using namespace System::IO::Ports;
-using namespace System::Threading;
-
-/*---------------------------------------------------------------------
-  Joypad  (i-Bus → COM-порт)
----------------------------------------------------------------------*/
-public ref class Joypad
-{
-public:
-    /* событие – _не подписывайтесь_  ➜  ничего в форме не вызовется */
-    delegate void TickHandler(array<UInt16>^ ch);
-    event TickHandler^ TickEvent;
-
-    Joypad()
-    {
-        // -------- открываем консоль один раз ----------------------
-        static bool consoleReady = false;
-        if (!consoleReady && ::AllocConsole())
-        {
-            FILE* fp;
-            freopen_s(&fp, "CONOUT$", "w", stdout);
-            std::cout << "   CH1   CH2   CH3   CH4   CH5   CH6\n";
-            consoleReady = true;
-        }
-        // ----------------------------------------------------------
-
-        _sp = gcnew SerialPort();
-        _sp->ReadTimeout = 200;
-        _sp->ReadBufferSize = 256;
-    }
-
-    /* ---------- запуск ------------------------------------------- */
-    bool Start(String^ port, int baud )
-    {
-        if (_run) return true;
-        try
-        {
-            _sp->PortName = port;
-            _sp->BaudRate = baud;
-            _sp->Parity = Parity::None;
-            _sp->DataBits = 8;
-            _sp->StopBits = StopBits::One;
-            _sp->Open();
-        }
-        catch (Exception^ ex)
-        {
-            System::Diagnostics::Debug::WriteLine("Serial error: " + ex->Message);
-            return false;
-        }
-
-        _run = true;
-        _thr = gcnew Thread(gcnew ThreadStart(this, &Joypad::RxLoop));
-        _thr->IsBackground = true;
-        _thr->Start();
-        return true;
-    }
-
-    /* ---------- остановка ---------------------------------------- */
-    void Stop()
-    {
-        _run = false;
-        if (_thr && _thr->IsAlive) _thr->Join();
-        if (_sp->IsOpen) _sp->Close();
-    }
-
-    property array<UInt16>^ Channels { array<UInt16>^ get() { return _ch; } }
-
-private:
-    // ----------- поток приёма i-Bus ---------------------------------
-    void RxLoop()
-    {
-        array<Byte>^ buf = gcnew array<Byte>(64);
-        array<Byte>^ pkt = gcnew array<Byte>(32);
-
-        while (_run)
-        {
-            int read = 0;
-            try { read = _sp->Read(buf, 0, buf->Length); }
-            catch (TimeoutException^) { continue; }
-
-            for (int i = 0; i < read; ++i)
-            {
-                _fifo[_head++] = buf[i];
-                if (_head >= _fifo->Length) _head = 0;
-
-                if (_fifo[_head] == 0x20 && _fifo[(_head + 1) & 0x7F] == 0x40)
-                {
-                    for (int j = 0; j < 32; ++j)
-                        pkt[j] = _fifo[(_head + j) & 0x7F];
-
-                    if (!CheckPkt(pkt)) continue;
-                    ParseChannels(pkt);
-
-                    // -------- 1) печать в консоль ----------------
-                    std::cout << '\r';
-                    for (int k = 0; k < 6; ++k)
-                        std::cout << std::setw(6) << _ch[k] << ' ';
-                    std::cout << std::flush;
-                    // ---------------------------------------------
-
-                    // -------- 2) необязательный вызов события ----
-                        TickEvent(_ch);            // raise
-                }
-            }
-        }
-    }
-
-    bool CheckPkt(array<Byte>^ p)
-    {
-        UInt16 sum = 0; for (int i = 0; i < 30; ++i) sum += p[i];
-        sum = 0xFFFF - sum;
-        return sum == (p[30] | p[31] << 8);
-    }
-
-    void ParseChannels(array<Byte>^ p)
-    {
-        for (int i = 0; i < 14; ++i)
-            _ch[i] = (UInt16)(p[2 + i * 2] | p[3 + i * 2] << 8);
-    }
-
-    /* ---------- поля --------------------------------------------- */
-    SerialPort^ _sp;
-    Thread^ _thr;
-    bool        _run = false;
-
-    array<UInt16>^ _ch = gcnew array<UInt16>(14);
-    array<Byte>^ _fifo = gcnew array<Byte>(128);
-    int _head = 0;
-};

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,28 +1,26 @@
-using System.CodeDom;
+using System.Numerics;
-using System.Numerics;
 using System.Runtime.InteropServices;
-using System.Security.Cryptography;
+using static System.Net.Mime.MediaTypeNames;
-using static DroneSimulator.Drone;
-using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
 
 namespace DroneSimulator
 {
   internal class Drone
   {
     public int ID;
-    public float Mass; // Масса
+
     public bool Active; // Живой?
-    public float Length; // Длинна лучей
     public const float Dynamic = 10; // Динамика вращения
     public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
     public Quaternion Quat; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей (0-1)
-    public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
+    
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
 
     public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
     public float LaserRange; // Имитация: Дальномер
 
+    public Vector4 Orientation;
+
     private uint DataTimer;
 
     private const float Gravity = 9.8f;
@@ -33,17 +31,21 @@ namespace DroneSimulator
     private Thread DroneThread;
     private int Timer;
 
-    private Random MainRandom = new Random();
+    private Vector2 MoveOF = Vector2.Zero;
 
-    public struct DataBarometer
+    public struct Physics
     {
-      public int Value, Pressure; // Значение давления в Паскальях
+      static public float Mass; // Масса
-      public float Accuracy;
+      static public float Length; // Длинна лучей
-      public int Frequency;
+      static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
-      public uint Time;
     }
 
-    public DataBarometer dataBarometer;
+    private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
+    private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
+    private RealMode.Position RealPos = new RealMode.Position();
+    private RealMode.Barometer RealBar = new RealMode.Barometer();
+    private RealMode.Range RealRange = new RealMode.Range();
+    private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
 
     public static byte[] getBytes(object data)
     {
@@ -86,16 +88,28 @@ namespace DroneSimulator
       return mem;
     }
 
-    public struct DataVisual
+    public VisualData.VisualDrone GetVisual(int count, int index)
     {
-      public int ID; // Идентификатор
+      VisualData.VisualDrone drone = new VisualData.VisualDrone();
-      public float W, X, Y, Z; // Кватернион вращения
-      public float PosX, PosY, PosZ; // Координаты в пространстве
-    }
 
-    public DataVisual GetVisual()
+      drone.Head.Size = Marshal.SizeOf(typeof(VisualData.VisualDrone));
-    {
+      drone.Head.Type = VisualData.VisualHead.VisualType.Drone;
-      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 };
+
+      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;
     }
 
     private void ThreadFunction()
@@ -121,12 +135,8 @@ namespace DroneSimulator
       DroneThread.Start();
     }
 
-    public int Create(float power, float mass, float len)
+    public int Create()
     {
-      Mass = mass;
-      Length = len;
-      MaxPower = power;
-
       Active = true;
 
       return ID;
@@ -170,7 +180,7 @@ namespace DroneSimulator
     {
       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;
@@ -192,16 +202,45 @@ 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 (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;
@@ -213,6 +252,8 @@ namespace DroneSimulator
 
       Vector4 ori = GetOrientation();
 
+      Orientation = ori;
+
       if (PosXYZ.Z < 0)
       {
         PosXYZ.Z = 0;
@@ -240,25 +281,26 @@ namespace DroneSimulator
           //Active = false; // Перевернулся вверх ногами
         }
 
-        Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
+        Quaternion grav = Quat * new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0) * Quaternion.Inverse(Quat);
-        //grav = (Quat * grav) * 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);
         else LaserRange = float.MaxValue;
       }
 
-      if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer)
+      MoveOF.X += SpdXYZ.X - Gyr.Y;
-      {
+      MoveOF.Y += SpdXYZ.Y + Gyr.X;
-        float bar = dataBarometer.Pressure - PosXYZ.Z * 11;
+
-        int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10;
+      RealAcc.Update(Acc, (uint)tick);
-        dataBarometer.Value = (int)(bar + rnd);
+      RealGyr.Update(Gyr, (uint)tick);
-        dataBarometer.Time = DataTimer;
+      RealRange.Update(LaserRange, (uint)tick);
-      }
+      RealBar.Update(PosXYZ.Z * 11, (uint)tick);
+      RealPos.Update(PosXYZ, (uint)tick);
+      RealOF.Update(MoveOF, LaserRange, (uint)tick);
 
       DataTimer = (uint)tick;
     }
@@ -268,7 +310,7 @@ namespace DroneSimulator
       if (pow > 1) pow = 1;
       if (pow < 0) pow = 0;
 
-      return pow * MaxPower;
+      return pow * Physics.MaxPower;
     }
 
     public void SetQadroPow(float ul, float ur, float dl, float dr)
@@ -298,8 +340,8 @@ namespace DroneSimulator
       acc.Head.Type = DroneData.DataType.DataAcc;
       acc.Head.Time = (uint)Environment.TickCount;
 
-      acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
+      acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
-      acc.Time = DataTimer;
+      acc.Time = RealAcc.timer;
 
       return getBytes(acc);
     }
@@ -313,8 +355,8 @@ namespace DroneSimulator
       gyr.Head.Type = DroneData.DataType.DataGyr;
       gyr.Head.Time = (uint)Environment.TickCount;
 
-      gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
+      gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
-      gyr.Time = DataTimer;
+      gyr.Time = RealGyr.timer;
 
       return getBytes(gyr);
     }
@@ -343,8 +385,8 @@ namespace DroneSimulator
       range.Head.Type = DroneData.DataType.DataRange;
       range.Head.Time = (uint)Environment.TickCount;
 
-      range.LiDAR = LaserRange;
+      range.LiDAR = RealRange.result;
-      range.Time = DataTimer;
+      range.Time = RealRange.timer;
 
       return getBytes(range);
     }
@@ -358,8 +400,8 @@ namespace DroneSimulator
       local.Head.Type = DroneData.DataType.DataLocal;
       local.Head.Time = (uint)Environment.TickCount;
 
-      local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
+      local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
-      local.Time = DataTimer;
+      local.Time = RealPos.timer;
 
       return getBytes(local);
     }
@@ -373,15 +415,83 @@ namespace DroneSimulator
       bar.Head.Type = DroneData.DataType.DataBar;
       bar.Head.Time = (uint)Environment.TickCount;
 
-      bar.Pressure = dataBarometer.Value;
+      bar.Pressure = RealBar.result;
-      bar.Time = dataBarometer.Time;
+      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)Environment.TickCount;
+
+      of.X = RealOF.result.X;
+      of.Y = RealOF.result.Y;
+      of.Time = RealBar.timer;
+
+      MoveOF = Vector2.Zero;
+
+      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)Environment.TickCount;
+
+      GPS.Point p = new GPS.Point();
+      p.x = PosXYZ.Y; p.y= PosXYZ.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 + PosXYZ.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;
+
+      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)Environment.TickCount;
+
+      quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
+
+      return getBytes(quat);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
-      byte[] zero = new byte[0];
+      byte[] zero = Array.Empty<byte>();
 
       switch (head.Type)
       {
@@ -411,6 +521,12 @@ namespace DroneSimulator
 
         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;
       }
 

DroneSimulator/DroneData.cs

@@ -1,3 +1,4 @@
+using System.Net;
 using System.Runtime.InteropServices;
 
 namespace DroneData
@@ -12,10 +13,13 @@ namespace DroneData
     None = 0, Head = 1,
 
     // Output
-    DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
+    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,7 +29,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 +41,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 +51,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,7 +61,7 @@ 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));
   }
@@ -65,9 +69,9 @@ namespace DroneData
   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));
   }
@@ -75,9 +79,9 @@ namespace DroneData
   public struct DataLocal
   {
     public DataHead Head;
-    public XYZ Local; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public XYZ Local; // Локальные координаты
 
-    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.DataLocal));
   }
@@ -85,13 +89,43 @@ namespace DroneData
   public struct DataBar
   {
     public DataHead Head;
-    public int Pressure; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public float Pressure; // Давление
 
-    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.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 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;
@@ -107,4 +141,12 @@ namespace DroneData
 
     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;
@@ -8,11 +8,6 @@ using System.Runtime.InteropServices;
 using System.CodeDom;
 using System.Linq;
 
-using System.Net.WebSockets;
-using System.Threading;
-using System.Threading.Tasks;
-using System.Windows.Forms;
-
 namespace DroneSimulator
 {
   public partial class Form_Main : Form
@@ -22,35 +17,22 @@ namespace DroneSimulator
     NetServerClients netServerClient = new NetServerClients();
     NetServerVisual netServerVisual = new NetServerVisual();
 
-      Invoke((MethodInvoker)delegate
+    List<Drone> AllDrones = new List<Drone>();
-      {
-        label_Clients_Num.Text = data.Count.ToString();
-      });
-
-      if (data.Connect)
-      {
-        Drone drone = new Drone(data.ID);
-        drone.Create(1.0f, 0.5f, 1.0f);
-
-        //
-        drone.dataBarometer.Pressure = 102258;
-        drone.dataBarometer.Accuracy = (float)numericUpDown_Bar_Accur.Value;
-        drone.dataBarometer.Frequency = (int)numericUpDown_Bar_Freq.Value;
-        try { drone.dataBarometer.Pressure = int.Parse(textBox_Bar_Pressure.Text); }
-        catch { MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error); }
-
-        //
-
-        screen2D.CreateDrone(Color.Red, data.ID);
-
-        AllDrones.Add(drone);
-      }
-      else
-      {
-        foreach (Drone drone in AllDrones)
 
+    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)
@@ -65,7 +47,7 @@ namespace DroneSimulator
       if (data.Connect)
       {
         Drone drone = new Drone(data.ID);
-                drone.Create(1.0f, 0.5f, 1.0f);
+        drone.Create();
 
         screen2D.CreateDrone(Color.Red, data.ID);
 
@@ -162,12 +144,27 @@ 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;
+
+
       if (screen2D == null) return;
 
+      listBox_Drones.Items.Clear();
+
+      try
+      {
         foreach (Drone d in AllDrones)
         {
           screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+
+          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();
     }
@@ -182,7 +179,7 @@ namespace DroneSimulator
 
       Invoke((MethodInvoker)delegate
       {
-                label_Clients_Num.Text = data.Count.ToString();
+        label_Visual_Num.Text = data.Count.ToString();
       });
 
       if (data.Connect)
@@ -199,9 +196,11 @@ namespace DroneSimulator
     {
       NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
 
+      int index = 0;
+
       foreach (Drone d in AllDrones)
       {
-                Drone.DataVisual v = d.GetVisual();
+        VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
 
         try { data.Client.Send(Drone.getBytes(v)); }
         catch { }
@@ -234,130 +233,124 @@ 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);
       }
 
-    private void button_Client_Start_Click(object sender, EventArgs e)
+      RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value;
-    {
+      RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value;
-      var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ClientConnectionCallback, ClientReceiveCallback);
+      RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value;
-      switch (done)
+      RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked;
-      {
-        case NetServerClients.ServerState.Error:
-          {
-            MessageBox.Show("Error to start clients server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
-            break;
-          }
-        case NetServerClients.ServerState.Start:
-          {
-            button_Client_Start.Text = "Stop";
-            button_Client_Start.BackColor = Color.LimeGreen;
-            panel_Menu_Model.Enabled = false;
-            break;
-          }
-        case NetServerClients.ServerState.Stop:
-          {
-            label_Clients_Num.Text = "0";
-            button_Client_Start.Text = "Start";
-            button_Client_Start.BackColor = Color.Transparent;
-            panel_Menu_Model.Enabled = true;
-            break;
-          }
     }
 
-      if (done != NetServerClients.ServerState.Start) return;
+    private void numericUpDown_Acc_Update(object sender, EventArgs e)
+    {
+      RealMode.Accelerometer.RealSimulation = checkBox_Model_Acc_Real.Checked;
 
-      pictureBox_2D.Image = null;
+      RealMode.Accelerometer.Freq = (uint)numericUpDown_Acc_Freq.Value;
+      RealMode.Accelerometer.Noise = (float)numericUpDown_Acc_Noise.Value;
+      RealMode.Accelerometer.Lateness = (float)numericUpDown_Acc_Laten.Value;
 
-      screen2D = new Screen2D(DrawCallback);
+      RealMode.Accelerometer.ScaleLeft = (float)numericUpDown_Acc_Scale_Left.Value;
+      RealMode.Accelerometer.ScaleRight = (float)numericUpDown_Acc_Scale_Rigth.Value;
     }
 
-    private void exitToolStripMenuItem_Click(object sender, EventArgs e)
+    private void numericUpDown_Gyr_Update(object sender, EventArgs e)
     {
-      Close();
+      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 DrawCallback(Bitmap bmp)
+    private void numericUpDown_Pos_Update(object sender, EventArgs e)
     {
-      Invoke((MethodInvoker)delegate
+      RealMode.Position.RealSimulation = checkBox_Model_Pos_Real.Checked;
-      {
+
-        if (pictureBox_2D.Image == null) pictureBox_2D.Image = bmp;
+      RealMode.Position.Freq = (uint)numericUpDown_Pos_Freq.Value;
-        pictureBox_2D.Refresh();
+      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 timer_Test_Tick(object sender, EventArgs e)
+    private void numericUpDown_Range_Update(object sender, EventArgs e)
     {
-      if (screen2D == null) return;
+      RealMode.Range.RealSimulation = checkBox_Model_Range_Real.Checked;
 
-      foreach (Drone d in AllDrones)
+      RealMode.Range.Freq = (uint)numericUpDown_Range_Freq.Value;
-      {
+      RealMode.Range.Noise = (float)numericUpDown_Range_Noise.Value;
-        screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+      RealMode.Range.Lateness = (float)numericUpDown_Range_Laten.Value;
+      RealMode.Range.Enable = checkBox_Range_Enable.Checked;
+
+      RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
     }
 
-      screen2D.DrawScene();
+    private void numericUpDown_OF_Update(object sender, EventArgs e)
-    }
-    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
     {
-      foreach (Drone d in AllDrones) d.Close();
+      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 * 10;
+      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 VisualConnectionCallback(object o)
+    private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
     {
-      NetServerVisual.ConnectData data = (NetServerVisual.ConnectData)o;
+      Area.Poisition.Freeze.X = checkBox_Area_Freeze_X.Checked;
-
+      Area.Poisition.Freeze.Y = checkBox_Area_Freeze_Y.Checked;
-      Invoke((MethodInvoker)delegate
+      Area.Poisition.Freeze.Z = checkBox_Area_Freeze_Z.Checked;
-      {
-        label_Clients_Num.Text = data.Count.ToString();
-      });
-
-      if (data.Connect)
-      {
-        //---
-      }
-      else
-      {
-        //---
-      }
     }
 
-    private void VisualReceiveCallback(object o)
+    private void numericUpDown_Area_Wind_Update(object sender, EventArgs e)
     {
-      NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
+      Area.Wind.Enable = checkBox_Area_Wind_Enable.Checked;
-
+      Area.Wind.Speed.From = (float)numericUpDown_Area_Wind_Speed_From.Value;
-      foreach (Drone d in AllDrones)
+      Area.Wind.Speed.To = (float)numericUpDown_Area_Wind_Speed_To.Value;
-      {
+      Area.Wind.Direction = (float)numericUpDown_Area_Wind_Direction.Value;
-        Drone.DataVisual v = d.GetVisual();
+      Area.Wind.Density = (float)numericUpDown_Area_Wind_Density.Value;
-
+      Area.Wind.PosResist = ((float)numericUpDown_Area_Wind_PosResist.Value) / 1000.0f;
-        try { data.Client.Send(Drone.getBytes(v)); }
+      Area.Wind.RotResist = ((float)numericUpDown_Area_Wind_RotResist.Value) / 1000.0f;
-        catch { }
-      }
     }
 
-    private void button_Visual_Start_Click(object sender, EventArgs e)
+    private void numericUpDown_GPS_ValueChanged(object sender, EventArgs e)
     {
-      var done = netServerVisual.StartServer((int)numericUpDown_Visual_Port.Value, (int)numericUpDown_Visual_Limit.Value, VisualConnectionCallback, VisualReceiveCallback);
+      GPS.Home.Lat = (double)numericUpDown_GPS_Lat.Value;
-      switch (done)
+      GPS.Home.Lon = (double)numericUpDown_GPS_Lon.Value;
-      {
+      GPS.Home.Alt = (float)numericUpDown_GPS_Alt.Value;
-        case NetServerVisual.ServerState.Error:
+
-          {
+      GPS.State.Fix = (byte)numericUpDown_GPS_Fix.Value;
-            MessageBox.Show("Error to start visual server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
+      GPS.State.SatVisible = (byte)numericUpDown_GPS_Vis.Value;
-            break;
+      GPS.State.SatUsed = (byte)numericUpDown_GPS_Use.Value;
-          }
+      GPS.State.Noise = (float)numericUpDown_GPS_Noise.Value;
-        case NetServerVisual.ServerState.Start:
+      GPS.State.Hdop = (float)numericUpDown_GPS_HDOP.Value;
-          {
+      GPS.State.Vdop = (float)numericUpDown_GPS_VDOP.Value;
-            button_Visual_Start.Text = "Stop";
+      GPS.State.Pdop = (float)numericUpDown_GPS_PDOP.Value;
-            button_Visual_Start.BackColor = Color.LimeGreen;
-            break;
-          }
-        case NetServerVisual.ServerState.Stop:
-          {
-            label_Visual_Num.Text = "0";
-            button_Visual_Start.Text = "Start";
-            button_Visual_Start.BackColor = Color.Transparent;
-            break;
-          }
-      }
-    }
     }
 
+    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;
+    }
+  }
 }

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,416 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Numerics;
+using System.Reflection;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
+
+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 uint 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 = (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;
+        }
+      }
+    }
+
+    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 uint 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 = (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;
+        }
+      }
+    }
+
+    internal class Magnetometer
+    {
+
+    }
+
+    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 uint index = 0;
+
+      public uint timer = 0;
+      public Vector3 result;
+
+      public void Update(Vector3 value, uint time)
+      {
+        if (!Enable)
+        {
+          result = Vector3.NaN;
+          return;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          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];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq < time)
+        {
+          result = value;
+          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;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private float[] laten = new float[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public float result;
+
+      public void Update(float value, uint time)
+      {
+        value = Pressure - value;
+
+        if (!Enable)
+        {
+          result = float.NaN;
+          return;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          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];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq < time)
+        {
+          result = value;
+          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 Error;
+      public static uint Wait;
+      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 uint index = 0;
+
+      public uint delay = 0;
+
+      public uint timer = 0;
+      public Vector2 result;
+      public void Update(Vector2 value, float Range, uint time)
+      {
+        if (!Enable)
+        {
+          result = Vector2.NaN;
+          return;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        value *= Lens;
+
+        if (rand.Next(0, 1000) < (Error * 10))
+        {
+          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;
+        }
+      }
+    }
+
+    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 uint index = 0;
+
+      public uint timer = 0;
+      public float result;
+
+      public void Update(float value, uint time)
+      {
+        if (!Enable)
+        {
+          result = float.NaN;
+          return;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          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];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq < time)
+        {
+          result = value;
+          timer = time;
+        }
+      }
+    }
+  }
+}

DroneSimulator/Screen2D.cs

@@ -105,6 +105,8 @@ namespace DroneSimulator
         g.DrawRectangle(new Pen(Color.Black), new Rectangle { Width = MainArea.Width - 1, Height = MainArea.Height - 1 });
 
         foreach (var d in DroneList)
+        {
+          try
           {
             if (d.Azimuth >= 360) d.Azimuth -= 360;
             var bmp = RotateImage(d.Drone, d.Azimuth);
@@ -152,6 +154,8 @@ namespace DroneSimulator
 
             g.DrawImage(bmp, dest);
           }
+          catch { }
+        }
       }
 
       drawCallback(MainArea);

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; // Скорость всех двигателей
+  }
+
+}