merge upstream

DroneClient/BaseCommHandler.cs

@@ -0,0 +1,478 @@
+using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+using TelemetryIO.Models;
+
+namespace TelemetryIO
+{
+    public interface iCommParams
+    {
+
+    }
+
+    internal class TCPCommParams : iCommParams
+    {
+        public string IP = "";
+        public int Port = 0;
+        public TCPCommParams(string addr, int port)
+        {
+            this.IP = addr;
+            this.Port = port;
+        }
+    }
+
+    public class SerialCommParams : iCommParams
+    {
+        public string PortName = "";
+        public int BaudRate = 9600;
+        public SerialCommParams(string portName, int baudRate)
+        {
+            PortName = portName;
+            BaudRate = baudRate;
+        }
+    }
+
+    public abstract class BaseCommHandler
+    {
+        public const int TELE_CMD_RD_ONCE = 1;
+        public const int TELE_CMD_RD_MON_ON = 2;
+        public const int TELE_CMD_RD_MON_OFF = 3;
+        public const int TELE_CMD_RD_MON_ADD = 4;
+        public const int TELE_CMD_RD_MON_REMOVE = 5;
+        public const int TELE_CMD_RD_MON_REMOVEALL = 6;
+        public const int TELE_CMD_WR = 10;
+        public const int TELE_CMD_MOTORS_CTRL = 100;
+        public const int TELE_CMD_ABORT = 999;
+        public const int TELE_CMD_HELLO = 9999;
+        public const byte CRC8_POLYNOMIAL = 0x07;
+        public const int CRC32_POLY = 0x04C11DB7;
+        public const byte ESCAPE_BEGIN = 0xBE;
+        public const byte ESCAPE_END = 0xED;
+        public const byte ESCAPE_CHAR = 0x0E;
+
+
+        private volatile bool _isReading = true;
+        private readonly ConcurrentQueue<byte> dataQueue = new ConcurrentQueue<byte>();
+        static private List<byte> rx_buf = new List<byte>();
+        private Task _readingTask;
+        private object lock_obj = new object();
+        private object lock_obj_put = new object();
+        private bool waitingForResponse = false;
+        private bool isTimeout = false;
+        private readonly int responseTimeout = 2000;
+        private System.Timers.Timer timeout = new System.Timers.Timer(3000);
+        private bool exitPending = false;
+        private float[] monitor = new float[32];
+        public string view_str = "";
+
+        protected string IP = "127.0.0.1";
+        protected int Port = 8888;
+
+        protected string PortName = "COM1";
+        protected int BaudRate = 9600;
+
+        private Queue<byte[]> req_buffer = new Queue<byte[]>();
+
+        //Generate event when answer is received
+        public delegate void AnswerEventHandler<SerialEventArgs>(object sender, SerialEventArgs e);
+        public event AnswerEventHandler<FeedbackEventArgs> AnswerReceived;//событие получены данные в ответ на запрос
+
+        //Generate event when handshake is occurred
+        public delegate void HandShakeHandler(object sender, EventArgs e);
+        public event HandShakeHandler HandShakeOccurred;//событие полетник ответил на запрос HELLO
+
+        //Generate event when monitoring telegram is received
+        public delegate void MonitoringEventHandler<MonitoringEventArgs>(object sender, MonitoringEventArgs e);
+        public event MonitoringEventHandler<MonitoringEventArgs> MonitoringItemsReceived;//событие получены данные мониторинга
+
+        Models.Telemetry telemetry = Models.Telemetry.Instance;
+        abstract public Task Open();
+        abstract public void Close();
+        abstract protected void sendData(byte[] data);
+        abstract public bool IsOpen();
+        abstract public void CloseConnection();
+        abstract public Task StartReadingAsync(object? client = null);
+
+        abstract public void setCommParams(iCommParams commParams);
+
+        abstract protected void ProcessCommand(int cmd, int slot, byte[] data, int offset, int len);
+
+        /// <summary>
+        /// Обрабатывает входящий поток данных, при обнаружении ECSAPE_END байта декодирует данные, проверяет контрольную сумму и запускает обработку команды
+        /// </summary>
+        protected void data_extract()
+        {
+            byte b = new byte();
+            while (dataQueue.TryDequeue(out b))
+            {
+                if (b == ESCAPE_END)
+                {//END BYTE IS RECEIVED
+                    isTimeout = false;
+                    byte[] unscape = EscapeSeqToBytes(rx_buf.ToArray());
+                    uint checksum = crc32(unscape, unscape.Length - 4);
+                    uint re_checksum = BitConverter.ToUInt32(unscape, unscape.Length - 4);
+                    if (re_checksum == checksum)
+                    {
+                        //Parse telegram
+                        int cmd = BitConverter.ToInt32(unscape, 0);
+                        int slot = BitConverter.ToInt32(unscape, 4);
+                        int len = BitConverter.ToInt32(unscape, 8);
+                        int offset = BitConverter.ToInt32(unscape, 12);
+                        byte[] data = new byte[len];
+                        Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** offset = {offset} *** len = {len}");
+                        if(cmd == TELE_CMD_WR)Array.Copy(unscape, 16, data, 0, len);
+                        ProcessCommand(cmd, slot, data, offset, len);
+
+                        waitingForResponse = false;
+                        timeout.Stop();
+                        sendNextRequest();
+                    }
+                }
+                else if (b == ESCAPE_BEGIN)
+                {//START BYTE IS RECEIVED
+                    rx_buf.Clear();
+                }
+                else
+                {//FILLING BUFFER
+                    rx_buf.Add(b);
+                }
+            }
+
+        }
+        //**********************************************************************************************
+        //*************************************** REQUESTS BLOCK ***************************************
+        //**********************************************************************************************
+
+        public void getPIDs()
+        {
+            //отправить наборы ПИДов
+            putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1000, new byte[0], 0, 4 * 9));
+            putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1001, new byte[0], 0, 4 * 9));
+            putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1002, new byte[0], 0, 4 * 9));
+            putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1003, new byte[0], 0, 4 * 9));
+        }
+
+        public void stopMonitoring()
+        {
+            //остановить мониторинг
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_OFF, 0, new byte[0], 0, 0));
+        }
+
+        public void startMonitoring()
+        {
+            //начать мониторинг
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_ON, 0, new byte[0], 0, 0));
+        }
+
+        public void AddMonitoringItem(int slot, int offset)
+        {
+            //добавить элемент из массива мониторинга по адресу
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, slot, new byte[0], offset));
+        }
+
+        public void AddMonitoringItem(string name)
+        {
+            //добавить элемент из массива мониторинга по имени
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, new byte[0], name));
+        }
+
+        public void RemoveMonitoringItem(int id)
+        {
+            //удалить элемент из массива мониторинга (len == id)
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVE, 0, new byte[0], id));
+        }
+
+        public void RemoveMonitoringItems()
+        {
+            //удалить все элементы из массива мониторинга
+            putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVEALL, 0, new byte[0], 0));
+        }
+
+        public void sendFloats(int slot, float[] sp)
+        {
+            //Записать массив чисел с плавающей точкой
+            putRequest(prepareTelegram(TELE_CMD_WR, slot, sp, 0, sp.Length * 4));
+        }
+
+        public void sendMotorsControl()
+        {
+            //Отправляет задание на моторы в полетник. Тот в ответ посылает актуальные скорости на моторах
+            //В offset передается количество моторов
+            putRequest(prepareTelegram(TELE_CMD_MOTORS_CTRL, Telemetry.MOTORS_SP_ADDRESS, telemetry.motor_sp, 8));
+        }
+
+        /// <summary>
+        /// Считает контрольную сумму CRC32
+        /// </summary>
+        public uint crc32(byte[] data, int length)
+        {
+            uint crc = 0xFFFFFFFF;  // Начальное значение CRC
+
+            for (int i = 0; i < length; i++)
+            {
+                crc ^= (uint)data[i] << 24;  // XOR с текущим байтом
+
+                for (int j = 0; j < 8; j++)
+                {
+                    if ((uint)(crc & 0x80000000) != 0)
+                    {
+                        crc = (crc << 1) ^ CRC32_POLY;
+                    }
+                    else
+                    {
+                        crc <<= 1;
+                    }
+                }
+            }
+
+            return crc;
+        }
+        public byte[] prepareTelegram<T>(int cmd, T[] load, string var_name)
+        {
+            VarAddress va = telemetry.getVarAdress(var_name);
+            return prepareTelegram(cmd, va.slot, load, va.offset, va.length);
+        }
+
+        /// <summary>
+        /// Подготавливает данные для отправки, кодируя их в ESCAPE-последовательность
+        /// </summary>
+        public byte[] prepareTelegram<T>(int cmd, int slot, T[] load, int offset, int len = 0)
+        {
+            byte[] byteload = DataArrayToBytes(load);
+            int total_len = 20 + byteload.Length;//cmd[4 bytes] + slot[4 bytes] + len[4 bytes] + offset[4 bytes] + load[len bytes]
+            byte[] data = new byte[total_len];
+
+            //Construct telegram
+            data[0] = (byte)(0xFF & cmd);
+            data[1] = (byte)(0xFF & (cmd >> 8));
+            data[2] = (byte)(0xFF & (cmd >> 16));
+            data[3] = (byte)(0xFF & (cmd >> 24));
+
+            data[4] = (byte)(0xFF & slot);
+            data[5] = (byte)(0xFF & (slot >> 8));
+            data[6] = (byte)(0xFF & (slot >> 16));
+            data[7] = (byte)(0xFF & (slot >> 24));
+
+            int l = 0;
+            if (cmd == TELE_CMD_WR) l = byteload.Length;
+            else l = len;
+
+            data[8] = (byte)(0xFF & l);
+            data[9] = (byte)(0xFF & (l >> 8));
+            data[10] = (byte)(0xFF & (l >> 16));
+            data[11] = (byte)(0xFF & (l >> 24));
+
+            data[12] = (byte)(0xFF & offset);
+            data[13] = (byte)(0xFF & (offset >> 8));
+            data[14] = (byte)(0xFF & (offset >> 16));
+            data[15] = (byte)(0xFF & (offset >> 24));
+
+            if (byteload.Length > 0)
+            {
+                //Copy data
+                Array.Copy(byteload, 0, data, 16, byteload.Length);
+            }
+            //CRC32
+            uint checksum = crc32(data, total_len - 4);
+            data[total_len - 4] = (byte)(0xFF & checksum);
+            data[total_len - 3] = (byte)(0xFF & (checksum >> 8));
+            data[total_len - 2] = (byte)(0xFF & (checksum >> 16));
+            data[total_len - 1] = (byte)(0xFF & (checksum >> 24));
+
+            byte[] escape = BytesToEscapeSeq(data);
+            byte[] ret = new byte[escape.Length + 2];
+            Array.Copy(escape, 0, ret, 1, escape.Length);
+            ret[0] = ESCAPE_BEGIN;
+            ret[ret.Length - 1] = ESCAPE_END;
+            //Array.Copy(ret, saving_request, ret.Length);
+            return ret;
+        }
+
+        /// <summary>
+        /// Конвертирует массив bool/byte/int/float в массив байт. Создает новый массив.
+        /// </summary>
+        byte[] DataArrayToBytes<T>(T[] data)
+        {
+            if (data == null) return new byte[0];
+            List<byte> ret = new List<byte>();
+            for (int i = 0; i < data.Length; i++)
+            {
+                if (typeof(T) == typeof(float))
+                {
+                    ret.AddRange(BitConverter.GetBytes((float)(object)data[i]));
+                }
+                else if (typeof(T) == typeof(int))
+                {
+                    ret.AddRange(BitConverter.GetBytes((int)(object)data[i]));
+                }
+                else if (typeof(T) == typeof(byte))
+                {
+                    ret.Add((byte)(object)data[i]);
+                }
+                else if (typeof(T) == typeof(bool))
+                {
+                    bool t = (bool)(object)data[i];
+                    if (t) ret.Add(1);
+                    else ret.Add(0);
+                }
+
+            }
+            return ret.ToArray();
+        }
+
+        /// <summary>
+        /// Конвертирует массив байт в ESCAPE-последовательность.
+        /// </summary>
+        public byte[] BytesToEscapeSeq(byte[] data)
+        {
+            List<byte> ret = new List<byte>();
+            for (int i = 0; i < data.Length; i++)
+            {
+                if ((data[i] == ESCAPE_BEGIN) || (data[i] == ESCAPE_CHAR) || (data[i] == ESCAPE_END))
+                {
+                    ret.Add(ESCAPE_CHAR);
+                    ret.Add((byte)(data[i] - 0x0E));
+                }
+                else
+                {
+                    ret.Add(data[i]);
+                }
+            }
+            return ret.ToArray();
+        }
+
+        /// <summary>
+        /// Конвертирует ESCAPE-последовательность в массив байт.
+        /// </summary>
+        public byte[] EscapeSeqToBytes(byte[] EscSeq)
+        {
+            List<byte> ret = new List<byte>();
+            for (int i = 0; i < EscSeq.Length; i++)
+            {
+                //if ((EscSeq[i] == ESCAPE_BEGIN) || (EscSeq[i] == ESCAPE_CHAR) || (EscSeq[i] == ESCAPE_END))
+                if (EscSeq[i] == ESCAPE_CHAR)
+                {
+                    i++;
+                    ret.Add((byte)(EscSeq[i] + 0x0E));
+                }
+                else
+                {
+                    ret.Add(EscSeq[i]);
+                }
+            }
+            return ret.ToArray();
+        }
+
+        public void requestExit()
+        {
+            //запрос на закрытие приложения, отправляем команду на очистку массива мониторинга и ждем ответ, либо таймаут,
+            //после чего приложение закрывается
+            RemoveMonitoringItems();
+            exitPending = true;
+        }
+
+        /// <summary>
+        /// Добавляет данные в очередь ожидания отправки.
+        /// </summary>
+        public void putRequest(byte[] request)
+        {
+            //добавить в очередь или отправить(если очередь пуста) пакет в порт
+            lock (lock_obj_put)
+            {
+                if (waitingForResponse)
+                {
+                    req_buffer.Enqueue(request);
+                    return;
+                }
+
+                sendRequest(request);
+            }
+        }
+
+        /// <summary>
+        /// Толкает данные непосредственно в очередь отправки.
+        /// </summary>
+        protected void sendRequest(byte[] request)
+        {
+            //отправка пакета
+            //waitingForResponse = true;
+            if (IsOpen())
+            {
+                sendData(request);
+            }
+            timeout.Stop();
+            timeout.Start();
+        }
+
+        /// <summary>
+        /// Вытягивает данные из очереди ожидания непосредственно в очередь отправки.
+        /// </summary>
+        protected void sendNextRequest()
+        {
+            //вытащить из очереди пакет и отправить в порт
+            waitingForResponse = false;
+            if (req_buffer.Count > 0)
+            {
+                sendRequest(req_buffer.Dequeue());
+            }
+        }
+
+        /// <summary>
+        /// Копирует входящие данные в очередь.
+        /// </summary>
+        protected void EnqueueData(byte[] data, int len)
+        {
+            for (int i = 0; i < len; i++)
+            {
+                dataQueue.Enqueue(data[i]);
+            }
+        }
+
+        public void ClearReqQueue()
+        {
+            req_buffer.Clear();
+        }
+
+        protected virtual void OnAnswerReceived(string answer, int id)
+        {
+            AnswerReceived?.Invoke(this, new FeedbackEventArgs(answer, id));
+        }
+
+        protected virtual void OnHandShakeOccurred()
+        {
+            HandShakeOccurred?.Invoke(this, new EventArgs());
+        }
+
+        protected virtual void OnMonitoringItemsReceived(float[] data)
+        {
+            MonitoringItemsReceived?.Invoke(this, new MonitoringEventArgs(data));
+        }
+    }
+
+    public class FeedbackEventArgs : EventArgs
+    {
+        public string Answer { get; set; }
+        public int Id { get; set; }
+
+        public FeedbackEventArgs(string answer, int id)
+        {
+            Answer = answer;
+            Id = id;
+        }
+    }
+
+    public class MonitoringEventArgs : EventArgs
+    {
+        public float[] Data { get; set; }
+
+        public MonitoringEventArgs(float[] data)
+        {
+            Data = data;
+        }
+    }
+}

DroneClient/Drone.cs

@@ -1,4 +1,7 @@
-using System.Drawing;
+using DroneData;
+using System.Diagnostics.Metrics;
+using System.Drawing;
+using System.Numerics;
 using System.Runtime.InteropServices;
 
 namespace DroneClient
@@ -8,8 +11,13 @@ namespace DroneClient
 
     public float AccX, AccY, AccZ;
     public float GyrX, GyrY, GyrZ;
+    public uint TimeAcc, TimeGyr;
+
     public float PosX, PosY;
     public float LaserRange;
+    public uint TimeRange;
+
+    public Vector2 OF = Vector2.Zero;
 
     public float MotorUL, MotorUR, MotorDL, MotorDR;
 
@@ -70,22 +78,54 @@ namespace DroneClient
       return getBytes(mot4);
     }
 
-    private byte[]? RecvDataIMU(byte[] data)
+    private byte[]? RecvDataAcc(byte[] data)
     {
-      DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
+      DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
 
       AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
-      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
+
+      TimeAcc= imu.Time;
 
       return new byte[0];
     }
 
-    private byte[]? RecvDataPos(byte[] data)
+    private byte[]? RecvDataGyr(byte[] data)
     {
-      DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos));
+      DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));
+
+      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
+
+      TimeGyr = imu.Time;
+
+      return new byte[0];
+    }
+
+    private byte[]? RecvDataRange(byte[] data)
+    {
+      DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));
+
+      LaserRange = pos.LiDAR;
+
+      TimeRange = pos.Time;
+
+      return new byte[0];
+    }
+
+    private byte[]? RecvDataLocal(byte[] data)
+    {
+      DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));
 
       PosX = pos.Local.X; PosY = pos.Local.Y;
-      LaserRange = pos.LiDAR;
+
+      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];
     }
@@ -96,7 +136,7 @@ namespace DroneClient
 
       switch (head.Type)
       {
-        case DroneData.DataType.DataIMU:
+        case DroneData.DataType.DataAcc:
           {
             if (head.Mode == DroneData.DataMode.Request)
             {
@@ -108,11 +148,11 @@ namespace DroneClient
             else
             {
               // Пришли данные
-              return RecvDataIMU(body);
+              return RecvDataAcc(body);
             }
           }
 
-        case DroneData.DataType.DataPos:
+        case DroneData.DataType.DataGyr:
           {
             if (head.Mode == DroneData.DataMode.Request)
             {
@@ -124,7 +164,55 @@ namespace DroneClient
             else
             {
               // Пришли данные
-              return RecvDataPos(body);
+              return RecvDataGyr(body);
+            }
+          }
+
+        case DroneData.DataType.DataRange:
+          {
+            if (head.Mode == DroneData.DataMode.Request)
+            {
+              // Запрос данных
+              // ... //
+              //
+              return zero;
+            }
+            else
+            {
+              // Пришли данные
+              return RecvDataRange(body);
+            }
+          }
+
+        case DroneData.DataType.DataLocal:
+          {
+            if (head.Mode == DroneData.DataMode.Request)
+            {
+              // Запрос данных
+              // ... //
+              //
+              return zero;
+            }
+            else
+            {
+              // Пришли данные
+              return RecvDataLocal(body);
+            }
+          }
+
+        case DroneData.DataType.DataOF:
+          {
+            if (head.Mode == DroneData.DataMode.Request)
+            {
+              // Запрос данных
+              // ... //
+              //
+              return zero;
+            }
+            else
+            {
+              // Пришли данные
+              return RecvDataOF(body);
             }
           }
 
@@ -193,24 +281,52 @@ namespace DroneClient
 
     public byte[] SendReqest()
     {
-      DroneData.DataHead imu = new DroneData.DataHead();
-      imu.Size = DroneData.DataHead.StrLen;
-      imu.Mode = DroneData.DataMode.Request;
-      imu.Type = DroneData.DataType.DataIMU;
+      DroneData.DataHead acc = new DroneData.DataHead();
+      acc.Size = DroneData.DataHead.StrLen;
+      acc.Mode = DroneData.DataMode.Request;
+      acc.Type = DroneData.DataType.DataAcc;
 
-      DroneData.DataHead pos = new DroneData.DataHead();
-      pos.Size = DroneData.DataHead.StrLen;
-      pos.Mode = DroneData.DataMode.Request;
-      pos.Type = DroneData.DataType.DataPos;
+      DroneData.DataHead gyr = new DroneData.DataHead();
+      gyr.Size = DroneData.DataHead.StrLen;
+      gyr.Mode = DroneData.DataMode.Request;
+      gyr.Type = DroneData.DataType.DataGyr;
 
-      byte[] si = getBytes(imu);
-      byte[] sp = getBytes(pos);
-      byte[] sm = SendDataMotor4();
+      DroneData.DataHead range = new DroneData.DataHead();
+      range.Size = DroneData.DataHead.StrLen;
+      range.Mode = DroneData.DataMode.Request;
+      range.Type = DroneData.DataType.DataRange;
 
-      byte[] send = new byte[si.Length + sp.Length + sm.Length];
-      si.CopyTo(send, 0);
-      sp.CopyTo(send, si.Length);
-      sm.CopyTo(send, si.Length + sp.Length);
+      DroneData.DataHead local = new DroneData.DataHead();
+      local.Size = DroneData.DataHead.StrLen;
+      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;
+
+      foreach (byte[] d in list) count += d.Length;
+
+      byte[] send = new byte[count];
+
+      count = 0;
+      foreach (byte[] d in list)
+      {
+        d.CopyTo(send, count);
+        count += d.Length;
+      }
 
       return send;
     }

DroneClient/DroneClient.csproj

@@ -8,4 +8,28 @@
     <ImplicitUsings>enable</ImplicitUsings>
   </PropertyGroup>
 
+  <ItemGroup>
+    <Compile Update="Properties\Resources.Designer.cs">
+      <DesignTime>True</DesignTime>
+      <AutoGen>True</AutoGen>
+      <DependentUpon>Resources.resx</DependentUpon>
+    </Compile>
+  </ItemGroup>
+
+  <ItemGroup>
+    <EmbeddedResource Update="Properties\Resources.resx">
+      <Generator>ResXFileCodeGenerator</Generator>
+      <LastGenOutput>Resources.Designer.cs</LastGenOutput>
+    </EmbeddedResource>
+  </ItemGroup>
+
+  <ItemGroup>
+    <None Update="images\connect.ico">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+    <None Update="images\disconnect.ico">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+
 </Project>

DroneClient/DroneData.cs

@@ -1,4 +1,4 @@
-using System.Runtime.InteropServices;
+using System.Runtime.InteropServices;
 
 namespace DroneData
 {
@@ -8,14 +8,17 @@ namespace DroneData
   };
 
   public enum DataType : ushort
-  { 
+  {
     None = 0, Head = 1,
 
     // Output
-    DataIMU = 1001, DataPos = 1002, 
+    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,32 +28,86 @@ namespace DroneData
     public DataMode Mode;
     public DataType Type;
 
+    public uint Time; // Общее время
+
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
   }
 
   public struct XYZ { public float X, Y, Z; }
 
-  public struct DataIMU
+  public struct DataAcc
   {
     public DataHead Head;
-    public XYZ Acc, Gyr, Mag;
+    public XYZ Acc;
 
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
   }
 
-  public struct DataPos
+  public struct DataGyr
   {
     public DataHead Head;
-    public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-    public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public XYZ Gyr;
 
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataPos));
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
+  }
+
+  public struct DataMag
+  {
+    public DataHead Head;
+    public XYZ Mag;
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
+  }
+
+  public struct DataRange
+  {
+    public DataHead Head;
+    public float LiDAR; // Датчик посадки
+
+    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;
-    public ulong Count;
     public float UL, UR, DL, DR;
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
@@ -59,7 +116,6 @@ namespace DroneData
   public struct DataMotor6
   {
     public DataHead Head;
-    public ulong Count;
     public float UL, UR, LL, RR, DL, DR;
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));

DroneClient/FormMain.cs

@@ -1,161 +1,280 @@
-using System.Text;
+using System.Text;
 using System.Numerics;
 using System.Windows.Forms;
 using static DroneSimulator.NetClient;
 using DroneClient;
+using TelemetryIO.Models;
+using TelemetryIO;
+using DroneClient.Models;
 
 namespace DroneSimulator
 {
-  public partial class Form_Main : Form
-  {
-    private NetClient netClient = new NetClient();
-
-    public Form_Main()
+    public partial class Form_Main : Form
     {
-      InitializeComponent();
-    }
+        private NetClient netClient = new NetClient();
+        private Telemetry telemetry = Telemetry.Instance;
+        private MonitorContainer monitoring = MonitorContainer.Instance;
+        private System.Windows.Forms.Timer copyDataTimer = new System.Windows.Forms.Timer();
+        private System.Windows.Forms.Timer MonitoringTimer = new System.Windows.Forms.Timer();
+        TelemetryServer tele_server;
+        string connectIco = Path.Combine(Application.StartupPath, "Images", "connect.ico");
+        string disconnectIco = Path.Combine(Application.StartupPath, "Images", "disconnect.ico");
 
-    private void ConnectionCallback(object o)
-    {
-      ConnectData data = (ConnectData)o;
-
-      if (!data.Connect)
-      {
-        try
+        public Form_Main()
         {
-          Invoke((MethodInvoker)delegate
-          {
-            button_Connect.Text = "Connect";
-            button_Connect.BackColor = Color.Transparent;
-            MessageBox.Show("Connection closed");
-          });
+            InitializeComponent();
+            tele_server = new TelemetryServer();
+            if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
         }
-        catch { }
 
-        return;
-      }
-    }
-
-    Drone dataDrone = new Drone();
-
-    private void ReceiveCallback(object o)
-    {
-      ReceiveData data = (ReceiveData)o;
-
-      List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
-
-      if (send == null) return;
-      try
-      {
-        foreach (byte[]? b in send)
+        private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e)
         {
-          if (b != null) data.Server?.Send(b);
+            if (TeleClientStatusPicture.InvokeRequired)
+            {
+                TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); }));
+            }
+            else
+            {
+                if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
+            }
+            monitoring.resetMonitorContainer();
+        }
+
+        private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e)
+        {
+            if (TeleClientStatusPicture.InvokeRequired)
+            {
+                TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); }));
+            }
+            else
+            {
+                if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco);
+            }
+        }
+
+        private void MonitoringTimer_Tick(object? sender, EventArgs e)
+        {
+            if (monitoring.isMonitor == 1)
+            {
+                byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)];
+                for (int i = 0; i < monitoring.monitorFillLevel; i++)
+                {
+                    int slot = monitoring.monitorItems[i].slotNo;
+                    int offset = monitoring.monitorItems[i].offset;
+                    int len = monitoring.monitorItems[i].len;
+                    if(len == 0)continue;
+                    Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float));
+                }
+                if (tele_server != null)
+                {
+                    tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length));
+                }
+            }
+        }
+
+        private void CopyDataTimer_Tick(object? sender, EventArgs e)
+        {
+            /*if (dataDrone != null)
+            {
+                float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
+                Array.Copy(imu, telemetry.imu, 6);
+            }*/
+
+            if (monitoring.isMonitor == 1) MonitoringTimer.Start();
+            else MonitoringTimer.Stop();
+        }
+
+        private void ConnectionCallback(object o)
+        {
+            ConnectData data = (ConnectData)o;
+
+            if (!data.Connect)
+            {
+                try
+                {
+                    Invoke((MethodInvoker)delegate
+                    {
+                        button_Connect.Text = "Connect";
+                        button_Connect.BackColor = Color.Transparent;
+                        MessageBox.Show("Connection closed");
+                    });
+                }
+                catch { }
+
+                return;
+            }
+        }
+
+        Drone dataDrone = new Drone();
+
+        private void ReceiveCallback(object o)
+        {
+            ReceiveData data = (ReceiveData)o;
+
+            List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
+
+            if (send == null) return;
+            try
+            {
+                foreach (byte[]? b in send)
+                {
+                    if (b != null) data.Server?.Send(b);
+                }
+            }
+            catch { }
+        }
+
+        private void button_Connect_Click(object sender, EventArgs e)
+        {
+            var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
+
+            switch (done)
+            {
+                case NetClient.ClientState.Error:
+                    {
+                        MessageBox.Show("Error connecting to server");
+                        break;
+                    }
+                case NetClient.ClientState.Connected:
+                    {
+                        button_Connect.Text = "Disconnect";
+                        button_Connect.BackColor = Color.LimeGreen;
+                        break;
+                    }
+                case NetClient.ClientState.Stop:
+                    {
+                        button_Connect.Text = "Connect";
+                        button_Connect.BackColor = Color.Transparent;
+                        break;
+                    }
+            }
+
+            if (done != NetClient.ClientState.Connected) return;
+
+
+            copyDataTimer.Interval = 10;
+            copyDataTimer.Tick += CopyDataTimer_Tick;
+            tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
+            var t = Task.Run(() => tele_server.Open());
+            tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
+            tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
+            MonitoringTimer.Interval = 25;
+            MonitoringTimer.Tick += MonitoringTimer_Tick;
+            copyDataTimer.Start();
+            button1.BackColor = Color.LimeGreen;
+        }
+
+        private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+        {
+            netClient?.Close();
+            netClient = null;
+        }
+
+        private void timer_Test_Tick(object sender, EventArgs e)
+        {
+            label_Acc_X.Text = dataDrone.AccX.ToString();
+            label_Acc_Y.Text = dataDrone.AccY.ToString();
+            label_Acc_Z.Text = dataDrone.AccZ.ToString();
+
+            label_time_acc.Text = dataDrone.TimeAcc.ToString();
+
+            label_Gyr_X.Text = dataDrone.GyrX.ToString();
+            label_Gyr_Y.Text = dataDrone.GyrY.ToString();
+            label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
+
+            label_time_gyr.Text = dataDrone.TimeGyr.ToString();
+
+            float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
+            Array.Copy(imu, telemetry.imu, 6);
+
+            label_Pos_X.Text = dataDrone.PosX.ToString();
+            label_Pos_Y.Text = dataDrone.PosY.ToString();
+            label_Pos_L.Text = dataDrone.LaserRange.ToString();
+
+            label_time_range.Text = dataDrone.TimeRange.ToString();
+
+            float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange };
+            Array.Copy(pos, telemetry.pos, 3);
+
+            label_OF_X.Text = dataDrone.OF.X.ToString();
+            label_OF_Y.Text = dataDrone.OF.Y.ToString();
+
+            label_time_of.Text = dataDrone.TimeRange.ToString();
+
+            float[] of = { dataDrone.OF.X, dataDrone.OF.Y };
+            Array.Copy(of, telemetry.of, 2);
+
+            if (monitoring.isMonitor == 1) MonitoringTimer.Start();
+            else MonitoringTimer.Stop();
+
+            netClient.SendData(dataDrone.SendReqest());
+        }
+
+        private void trackBar_Power_Scroll(object sender, EventArgs e)
+        {
+            float pow = (float)trackBar_Power.Value / 100;
+
+            label_Pow.Text = pow.ToString();
+
+            dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
+        }
+
+        private void button_UU_MouseDown(object sender, MouseEventArgs e)
+        {
+            float pow = ((float)trackBar_Value.Value) / 10.0f;
+
+            if (sender == button_UU)
+            {
+                dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
+                dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
+            }
+            if (sender == button_DD)
+            {
+                dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
+                dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
+            }
+            if (sender == button_LL)
+            {
+                dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
+                dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
+            }
+            if (sender == button_RR)
+            {
+                dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
+                dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
+            }
+
+            if (sender == button_ML)
+            {
+                dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
+                dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
+            }
+
+            if (sender == button_MR)
+            {
+                dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
+                dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
+            }
+        }
+
+        private void button_UU_MouseUp(object sender, MouseEventArgs e)
+        {
+            trackBar_Power_Scroll(null, null);
+        }
+
+        private void button1_Click(object sender, EventArgs e)
+        {
+
+            copyDataTimer.Interval = 10;
+            copyDataTimer.Tick += CopyDataTimer_Tick;
+            tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
+            var t = Task.Run(() => tele_server.Open());
+            tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
+            tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
+            MonitoringTimer.Interval = 25;
+            MonitoringTimer.Tick += MonitoringTimer_Tick;
+            copyDataTimer.Start();
+            button1.BackColor = Color.LimeGreen;
         }
-      }
-      catch { }
     }
-
-    private void button_Connect_Click(object sender, EventArgs e)
-    {
-      var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
-
-      switch (done)
-      {
-        case NetClient.ClientState.Error:
-          {
-            MessageBox.Show("Error connecting to server");
-            break;
-          }
-        case NetClient.ClientState.Connected:
-          {
-            button_Connect.Text = "Disconnect";
-            button_Connect.BackColor = Color.LimeGreen;
-            break;
-          }
-        case NetClient.ClientState.Stop:
-          {
-            button_Connect.Text = "Connect";
-            button_Connect.BackColor = Color.Transparent;
-            break;
-          }
-      }
-
-      if (done != NetClient.ClientState.Connected) return;
-    }
-
-    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
-    {
-      netClient?.Close();
-      netClient = null;
-    }
-
-    private void timer_Test_Tick(object sender, EventArgs e)
-    {
-      label_Acc_X.Text = dataDrone.AccX.ToString();
-      label_Acc_Y.Text = dataDrone.AccY.ToString();
-      label_Acc_Z.Text = dataDrone.AccZ.ToString();
-
-      label_Gyr_X.Text = dataDrone.GyrX.ToString();
-      label_Gyr_Y.Text = dataDrone.GyrY.ToString();
-      label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
-
-      label_Pos_X.Text = dataDrone.PosX.ToString();
-      label_Pos_Y.Text = dataDrone.PosY.ToString();
-      label_Pos_L.Text = dataDrone.LaserRange.ToString();
-
-      netClient.SendData(dataDrone.SendReqest());
-    }
-
-    private void trackBar_Power_Scroll(object sender, EventArgs e)
-    {
-      float pow = (float)trackBar_Power.Value / 100;
-
-      label_Pow.Text = pow.ToString();
-
-      dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
-    }
-
-    private void button_UU_MouseDown(object sender, MouseEventArgs e)
-    {
-      const float pow = 0.1f;
-
-      if (sender == button_UU)
-      {
-        dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
-        dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
-      }
-      if (sender == button_DD)
-      {
-        dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
-        dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
-      }
-      if (sender == button_LL)
-      {
-        dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
-        dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
-      }
-      if (sender == button_RR)
-      {
-        dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
-        dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
-      }
-
-      if (sender == button_ML)
-      {
-        dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
-        dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
-      }
-
-      if (sender == button_MR)
-      {
-        dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
-        dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
-      }
-    }
-
-    private void button_UU_MouseUp(object sender, MouseEventArgs e)
-    {
-      trackBar_Power_Scroll(null, null);
-    }
-  }
 }

DroneClient/Models/MonitorContainer.cs

@@ -0,0 +1,126 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using TelemetryIO;
+
+namespace DroneClient.Models
+{
+    public class monitorItem
+    {
+        public int slotNo = 0;
+        public int offset = 0;
+        public int len = 0;
+        public int id = 0;
+
+        public monitorItem()
+        {
+            
+        }
+        public monitorItem(int slotNo, int offset, int len, int id)
+        {
+            this.slotNo = slotNo;
+            this.offset = offset;
+            this.len = len;
+            this.id = id;
+        }
+    }
+
+    public class MonitorContainer
+    {
+        public const int monitorMaxFill = 32;
+        public int monitorFillLevel = 0;
+        public monitorItem[] monitorItems = new monitorItem[monitorMaxFill];
+        public int isMonitor = 0;
+        private static volatile MonitorContainer instance;
+        private static readonly object _lock = new object();
+
+        public static MonitorContainer Instance
+        {
+            get
+            {
+                if (instance == null)
+                {
+                    lock (_lock)
+                    {
+                        if (instance == null)
+                        {
+                            instance = new MonitorContainer();
+                        }
+                    }
+                }
+                return instance;
+            }
+
+        }
+
+        private MonitorContainer()
+        {
+            for (int i = 0; i < monitorMaxFill; i++)
+            {
+                monitorItems[i] = new monitorItem();
+            }
+        }
+
+        public void resetMonitorContainer()
+        {
+            for (int i = 0; i < monitorMaxFill; i++)
+            {
+                monitorItems[i] = new monitorItem();
+            }
+        }
+
+        public int monitorPut(int slot, int offset, int len)
+        {
+            int ret_id = 0;
+            if (monitorFillLevel < monitorMaxFill)
+            {
+                monitorItems[monitorFillLevel].slotNo = slot;
+                monitorItems[monitorFillLevel].offset = offset;
+                monitorItems[monitorFillLevel].len = len;
+                monitorItems[monitorFillLevel].id = (slot << 8) + offset;
+                ret_id = monitorItems[monitorFillLevel].id;
+                monitorFillLevel++;
+                isMonitor = 1;
+            }
+
+            return ret_id;
+        }
+
+        public int monitorRemove(int id)
+        {
+            int match = 0;
+            int ret_id = 0;
+            for (int i = 0; i < monitorMaxFill; i++)
+            {
+                if (monitorItems[i].id == id)
+                {
+                    match = 1;
+                    if (monitorFillLevel > 0) monitorFillLevel--;
+                    ret_id = id;
+                }
+
+                if (match > 0)
+                {
+                    if (monitorItems[i].len == 0) return ret_id;
+                    if (i < 31)
+                    {
+                        monitorItems[i].id = monitorItems[i + 1].id;
+                        monitorItems[i].offset = monitorItems[i + 1].offset;
+                        monitorItems[i].slotNo = monitorItems[i + 1].slotNo;
+                        monitorItems[i].len = monitorItems[i + 1].len;
+                    }
+                    else
+                    {
+                        monitorItems[i].id = 0;
+                        monitorItems[i].offset = 0;
+                        monitorItems[i].slotNo = 0;
+                        monitorItems[i].len = 0;
+                    }
+                }
+            }
+            return ret_id;
+        }
+    }
+}

DroneClient/Models/Telemetry.cs

@@ -0,0 +1,340 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Drawing;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace TelemetryIO.Models
+{
+    public sealed class Telemetry
+    {
+        //Класс синглтон для хранения данных телеметрии
+        private static volatile Telemetry instance;
+        private static readonly object _lock = new object();
+        private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();//Словарь адресов, для целей мониторинга
+        public string name = "_150";
+        public float[] raw_imu = new float[6];
+        public float[] imu = new float[6];
+        public float[] filtered_imu = new float[6];
+        public float[] g_integration = new float[3];
+        public float[] euler = new float[3];
+        public float[] quaternion = new float[4];
+        public float[] battery = new float[4];
+        public float[] motor_act = new float[8];
+        public float[] motor_sp = new float[8];
+        public float[] pid0 = new float[9];
+        public float[] pid1 = new float[9];
+        public float[] pid2 = new float[9];
+        public float[] pid3 = new float[9];
+        public float[] pos = new float[3];
+        public float[] of = new float[2];
+
+        private object _lockRW = new object();
+
+        private Telemetry() {
+            dictMonitor.Add("rawAx", new VarAddress(RAW_IMU_ADDRESS, 0));
+            dictMonitor.Add("rawAy", new VarAddress(RAW_IMU_ADDRESS, 1));
+            dictMonitor.Add("rawAz", new VarAddress(RAW_IMU_ADDRESS, 2));
+            dictMonitor.Add("rawGx", new VarAddress(RAW_IMU_ADDRESS, 3));
+            dictMonitor.Add("rawGy", new VarAddress(RAW_IMU_ADDRESS, 4));
+            dictMonitor.Add("rawGz", new VarAddress(RAW_IMU_ADDRESS, 5));
+
+            dictMonitor.Add("Ax", new VarAddress(IMU_ADDRESS, 0));
+            dictMonitor.Add("Ay", new VarAddress(IMU_ADDRESS, 1));
+            dictMonitor.Add("Az", new VarAddress(IMU_ADDRESS, 2));
+            dictMonitor.Add("Gx", new VarAddress(IMU_ADDRESS, 3));
+            dictMonitor.Add("Gy", new VarAddress(IMU_ADDRESS, 4));
+            dictMonitor.Add("Gz", new VarAddress(IMU_ADDRESS, 5));
+
+            dictMonitor.Add("filtered_Ax", new VarAddress(FILTERED_IMU_ADDRESS, 0));
+            dictMonitor.Add("filtered_Ay", new VarAddress(FILTERED_IMU_ADDRESS, 1));
+            dictMonitor.Add("filtered_Az", new VarAddress(FILTERED_IMU_ADDRESS, 2));
+            dictMonitor.Add("filtered_Gx", new VarAddress(FILTERED_IMU_ADDRESS, 3));
+            dictMonitor.Add("filtered_Gy", new VarAddress(FILTERED_IMU_ADDRESS, 4));
+            dictMonitor.Add("filtered_Gz", new VarAddress(FILTERED_IMU_ADDRESS, 5));
+
+            dictMonitor.Add("Gx_int", new VarAddress(G_INTEGRATION_ADDRESS, 0));
+            dictMonitor.Add("Gy_int", new VarAddress(G_INTEGRATION_ADDRESS, 1));
+            dictMonitor.Add("Gz_int", new VarAddress(G_INTEGRATION_ADDRESS, 2));
+
+            dictMonitor.Add("Euler.Phi", new VarAddress(EULER_ADDRESS, 0));
+            dictMonitor.Add("Euler.Theta", new VarAddress(EULER_ADDRESS, 1));
+            dictMonitor.Add("Euler.Psi", new VarAddress(EULER_ADDRESS, 2));
+
+            dictMonitor.Add("Qw", new VarAddress(Q_ADDRESS, 0));
+            dictMonitor.Add("Qx", new VarAddress(Q_ADDRESS, 1));
+            dictMonitor.Add("Qy", new VarAddress(Q_ADDRESS, 2));
+            dictMonitor.Add("Qz", new VarAddress(Q_ADDRESS, 3));
+
+            dictMonitor.Add("Battery[V]", new VarAddress(BAT_ADDRESS, 0));
+
+            dictMonitor.Add("motor0_Act", new VarAddress(MOTORS_ACT_ADDRESS, 0));
+            dictMonitor.Add("motor1_Act", new VarAddress(MOTORS_ACT_ADDRESS, 1));
+            dictMonitor.Add("motor2_Act", new VarAddress(MOTORS_ACT_ADDRESS, 2));
+            dictMonitor.Add("motor3_Act", new VarAddress(MOTORS_ACT_ADDRESS, 3));
+            dictMonitor.Add("motor4_Act", new VarAddress(MOTORS_ACT_ADDRESS, 4));
+            dictMonitor.Add("motor5_Act", new VarAddress(MOTORS_ACT_ADDRESS, 5));
+            dictMonitor.Add("motor6_Act", new VarAddress(MOTORS_ACT_ADDRESS, 6));
+            dictMonitor.Add("motor7_Act", new VarAddress(MOTORS_ACT_ADDRESS, 7));
+
+            dictMonitor.Add("Pos_X", new VarAddress(POS_ADDRESS, 0));
+            dictMonitor.Add("Pos_Y", new VarAddress(POS_ADDRESS, 1));
+            dictMonitor.Add("Pos_L", new VarAddress(POS_ADDRESS, 2));
+
+            dictMonitor.Add("OF_X", new VarAddress(OF_ADDRESS, 0));
+            dictMonitor.Add("OF_Y", new VarAddress(OF_ADDRESS, 1));
+        }
+
+        public int getId(string name)
+        {
+            VarAddress var = dictMonitor[name];
+            if (var == null) return -1;
+            return (var.slot << 8) | var.offset;
+        }
+
+        public string getName(int id)
+        {
+            int slot = id >> 8;
+            int offset = id & 0xFF;
+            foreach(var element in dictMonitor)
+            {
+                if ((element.Value.slot == slot) && (element.Value.offset == offset)) return element.Key;
+            }
+            return null;
+        }
+
+        public VarAddress getVarAdress(string name)
+        {
+            VarAddress ret;
+            dictMonitor.TryGetValue(name, out ret);
+            return ret;
+        }
+
+        public string[] getKeys()
+        {
+            return dictMonitor.Keys.ToArray();
+        }
+        
+        public static Telemetry Instance
+        {
+            get
+            {
+                if(instance == null)
+                {
+                    lock(_lock)
+                    {
+                        if (instance == null)
+                        {
+                            instance = new Telemetry();
+                        }
+                    }
+                }
+                return instance;
+            }
+        }
+
+        public void setSlot(int slot, byte[] data, int offset, int len)
+        {
+            switch (slot)
+            {
+                case RAW_IMU_ADDRESS://RAW IMU
+                    set_float(raw_imu, data, offset, len);
+                    break;
+
+                case IMU_ADDRESS://IMU
+                    set_float(imu, data, offset, len);
+                    break;
+
+                case Q_ADDRESS://quaternion
+                    set_float(quaternion, data, offset, len);
+                    break;
+
+                case G_INTEGRATION_ADDRESS://quaternion
+                    set_float(g_integration, data, offset, len);
+                    break;
+
+                case EULER_ADDRESS://quaternion
+                    set_float(euler, data, offset, len);
+                    break;
+
+                case FILTERED_IMU_ADDRESS://IMU
+                    set_float(filtered_imu, data, offset, len);
+                    break;
+
+                case BAT_ADDRESS://battery
+                    set_float(battery, data, offset, len);
+                    break;
+
+                case MOTORS_ACT_ADDRESS://motors act
+                    set_float(motor_act, data, offset, len);
+                    break;
+
+                case MOTORS_SP_ADDRESS://motors act
+                    set_float(motor_sp, data, offset, len);
+                    break;
+
+                case PID0_ADDRESS:
+                    set_float(pid0, data, offset, len);
+                    break;
+
+                case PID1_ADDRESS:
+                    set_float(pid1, data, offset, len);
+                    break;
+
+                case PID2_ADDRESS:
+                    set_float(pid2, data, offset, len);
+                    break;
+
+                case PID3_ADDRESS:
+                    set_float(pid3, data, offset, len);
+                    break;
+
+                case POS_ADDRESS:
+                    set_float(pos, data, offset, len);
+                    break;
+
+                case OF_ADDRESS:
+                    set_float(of, data, offset, len);
+                    break;
+
+                case NAME_ADDRESS:
+                    set_name(data);
+                    break;
+            }
+        }
+
+        public byte[] getSlot(int slot, int offset, int len)
+        {
+            switch (slot)
+            {
+                case RAW_IMU_ADDRESS://RAW IMU
+                    return get_float(raw_imu, offset, len);
+
+                case IMU_ADDRESS://IMU
+                    return get_float(imu, offset, len);
+
+                case Q_ADDRESS://quaternion
+                    return get_float(quaternion, offset, len);
+
+                case G_INTEGRATION_ADDRESS://quaternion
+                    return get_float(g_integration, offset, len);
+
+                case EULER_ADDRESS://quaternion
+                    return get_float(euler, offset, len);
+
+                case FILTERED_IMU_ADDRESS://IMU
+                    return get_float(filtered_imu, offset, len);
+
+                case BAT_ADDRESS://battery
+                    return get_float(battery, offset, len);
+
+                case MOTORS_ACT_ADDRESS://motors act
+                    return get_float(motor_act, offset, len);
+
+                case MOTORS_SP_ADDRESS://motors act
+                    return get_float(motor_sp, offset, len);
+
+                case PID0_ADDRESS:
+                    return get_float(pid0, offset, len);
+
+                case PID1_ADDRESS:
+                    return get_float(pid1, offset, len);
+
+                case PID2_ADDRESS:
+                    return get_float(pid2, offset, len);
+
+                case PID3_ADDRESS:
+                    return get_float(pid3, offset, len);
+
+                case POS_ADDRESS:
+                    return get_float(pos, offset, len);
+                    break;
+
+                case OF_ADDRESS:
+                    return get_float(of, offset, len);
+
+                case NAME_ADDRESS:
+                    return Encoding.ASCII.GetBytes(name);
+
+                default:
+                    byte[] data = new byte[0];
+                    return data;
+            }
+        }
+
+
+        private void set_float(float[] dest, byte[] data, int offset, int len)
+        {
+            lock (_lockRW)
+            {
+                for (int i = 0; i < (int)(len / 4); i++)
+                {
+                    byte[] bs = new byte[4];
+                    Array.Copy(data, i * 4, bs, 0, 4);
+                    float f = BitConverter.ToSingle(bs, 0);
+                    dest[offset + i] = f;
+                }
+            }
+        }
+
+        private byte[] get_float(float[] source, int offset, int len)
+        {
+            lock (_lockRW)
+            {
+                byte[] bs = new byte[len];
+                Buffer.BlockCopy(source, offset * sizeof(float), bs, 0, len);
+                return bs;
+            }
+        }
+
+        private void set_name(byte[] data)
+        {
+            int len = data.Length;
+            if (len > 16) len = 16;
+            name = Encoding.ASCII.GetString(data, 0, len);
+        }
+
+        public const int RAW_IMU_ADDRESS = 0;
+        public const int IMU_ADDRESS = 1;
+        public const int Q_ADDRESS = 2;
+        public const int G_INTEGRATION_ADDRESS = 3;
+        public const int EULER_ADDRESS = 4;
+        public const int FILTERED_IMU_ADDRESS = 5;
+        public const int POS_ADDRESS = 6;
+        public const int OF_ADDRESS = 7;
+
+        public const int BAT_ADDRESS = 50;
+
+        public const int MOTORS_ACT_ADDRESS = 100;
+        public const int MOTORS_SP_ADDRESS = 101;
+
+        public const int PID0_ADDRESS = 1000;
+        public const int PID1_ADDRESS = 1001;
+        public const int PID2_ADDRESS = 1002;
+        public const int PID3_ADDRESS = 1003;
+
+        public const int NAME_ADDRESS = 9999;
+    }
+
+    public class VarAddress
+    {
+        public int slot = 0;
+        public int offset = 0;
+        public int length = 0;
+
+        public VarAddress()
+        {
+            slot = offset = 0;
+            length = 4;
+        }
+
+        public VarAddress(int slot, int offset, int length = 4)
+        {
+            this.slot = slot;
+            this.offset = offset;
+            this.length = length;
+        }
+    }
+}

DroneClient/Properties/Resources.Designer.cs

@@ -0,0 +1,63 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     Этот код создан программой.
+//     Исполняемая версия:4.0.30319.42000
+//
+//     Изменения в этом файле могут привести к неправильной работе и будут потеряны в случае
+//     повторной генерации кода.
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+namespace DroneClient.Properties {
+    using System;
+    
+    
+    /// <summary>
+    ///   Класс ресурса со строгой типизацией для поиска локализованных строк и т.д.
+    /// </summary>
+    // Этот класс создан автоматически классом StronglyTypedResourceBuilder
+    // с помощью такого средства, как ResGen или Visual Studio.
+    // Чтобы добавить или удалить член, измените файл .ResX и снова запустите ResGen
+    // с параметром /str или перестройте свой проект VS.
+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "17.0.0.0")]
+    [global::System.Diagnostics.DebuggerNonUserCodeAttribute()]
+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
+    internal class Resources {
+        
+        private static global::System.Resources.ResourceManager resourceMan;
+        
+        private static global::System.Globalization.CultureInfo resourceCulture;
+        
+        [global::System.Diagnostics.CodeAnalysis.SuppressMessageAttribute("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
+        internal Resources() {
+        }
+        
+        /// <summary>
+        ///   Возвращает кэшированный экземпляр ResourceManager, использованный этим классом.
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Resources.ResourceManager ResourceManager {
+            get {
+                if (object.ReferenceEquals(resourceMan, null)) {
+                    global::System.Resources.ResourceManager temp = new global::System.Resources.ResourceManager("DroneClient.Properties.Resources", typeof(Resources).Assembly);
+                    resourceMan = temp;
+                }
+                return resourceMan;
+            }
+        }
+        
+        /// <summary>
+        ///   Перезаписывает свойство CurrentUICulture текущего потока для всех
+        ///   обращений к ресурсу с помощью этого класса ресурса со строгой типизацией.
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Globalization.CultureInfo Culture {
+            get {
+                return resourceCulture;
+            }
+            set {
+                resourceCulture = value;
+            }
+        }
+    }
+}

DroneClient/Properties/Resources.resx

@@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="utf-8"?>
+<root>
+  <!-- 
+    Microsoft ResX Schema 
+    
+    Version 2.0
+    
+    The primary goals of this format is to allow a simple XML format 
+    that is mostly human readable. The generation and parsing of the 
+    various data types are done through the TypeConverter classes 
+    associated with the data types.
+    
+    Example:
+    
+    ... ado.net/XML headers & schema ...
+    <resheader name="resmimetype">text/microsoft-resx</resheader>
+    <resheader name="version">2.0</resheader>
+    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
+    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
+    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
+    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
+    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
+        <value>[base64 mime encoded serialized .NET Framework object]</value>
+    </data>
+    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
+        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
+        <comment>This is a comment</comment>
+    </data>
+                
+    There are any number of "resheader" rows that contain simple 
+    name/value pairs.
+    
+    Each data row contains a name, and value. The row also contains a 
+    type or mimetype. Type corresponds to a .NET class that support 
+    text/value conversion through the TypeConverter architecture. 
+    Classes that don't support this are serialized and stored with the 
+    mimetype set.
+    
+    The mimetype is used for serialized objects, and tells the 
+    ResXResourceReader how to depersist the object. This is currently not 
+    extensible. For a given mimetype the value must be set accordingly:
+    
+    Note - application/x-microsoft.net.object.binary.base64 is the format 
+    that the ResXResourceWriter will generate, however the reader can 
+    read any of the formats listed below.
+    
+    mimetype: application/x-microsoft.net.object.binary.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
+            : and then encoded with base64 encoding.
+    
+    mimetype: application/x-microsoft.net.object.soap.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.bytearray.base64
+    value   : The object must be serialized into a byte array 
+            : using a System.ComponentModel.TypeConverter
+            : and then encoded with base64 encoding.
+    -->
+  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
+    <xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
+    <xsd:element name="root" msdata:IsDataSet="true">
+      <xsd:complexType>
+        <xsd:choice maxOccurs="unbounded">
+          <xsd:element name="metadata">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" />
+              </xsd:sequence>
+              <xsd:attribute name="name" use="required" type="xsd:string" />
+              <xsd:attribute name="type" type="xsd:string" />
+              <xsd:attribute name="mimetype" type="xsd:string" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="assembly">
+            <xsd:complexType>
+              <xsd:attribute name="alias" type="xsd:string" />
+              <xsd:attribute name="name" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="data">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
+              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
+              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="resheader">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" />
+            </xsd:complexType>
+          </xsd:element>
+        </xsd:choice>
+      </xsd:complexType>
+    </xsd:element>
+  </xsd:schema>
+  <resheader name="resmimetype">
+    <value>text/microsoft-resx</value>
+  </resheader>
+  <resheader name="version">
+    <value>2.0</value>
+  </resheader>
+  <resheader name="reader">
+    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+  <resheader name="writer">
+    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+</root>

DroneClient/TelemetryServer.cs

@@ -0,0 +1,258 @@
+using DroneClient.Models;
+using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Linq;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+using TelemetryIO;
+using TelemetryIO.Models;
+
+namespace TelemetryIO
+{
+    internal class TelemetryServer : TelemetryIO.BaseCommHandler
+    {
+        ConcurrentQueue<byte[]> sendQueue = new ConcurrentQueue<byte[]>();
+        Telemetry telemetry = Telemetry.Instance;
+        MonitorContainer monitoring = MonitorContainer.Instance;
+        private bool isClientConnected = false;
+
+        public event EventHandler OnTeleClientConnected = delegate { };
+        public event EventHandler OnTeleClientDisconnected = delegate { };
+
+        public bool isClientAvailable { get { return isClientConnected; } }
+
+        public TelemetryServer()
+        {
+
+        }
+        public override void Close()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void CloseConnection()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override bool IsOpen()
+        {
+            return true;
+        }
+
+        public async override Task Open()
+        {
+            await StartServerAsync();
+            //Console.ReadLine();
+        }
+
+        public async Task StartServerAsync()
+        {
+            var listener = new TcpListener(IPAddress.Any, Port);
+            listener.Start();
+            Console.WriteLine("Server is running");
+            try
+            {
+                while (true)
+                {
+                    //if (isClientConnected) await Task.Delay(25);
+                    var client = await listener.AcceptTcpClientAsync();
+                    StrikeOnTeleClientConnected();
+                    isClientConnected = true;
+                    Debug.WriteLine("Client is connected");
+                    //this.client = client;
+                    var readTask = StartReadingAsync(client);
+                    var writeTask = SendMessageAsync(client);
+                    await readTask;
+                    await writeTask;
+                }
+            }
+            catch (IOException ex) when (IsNetworkError(ex))
+            {
+                // Специальная обработка сетевых ошибок
+                Debug.WriteLine($"Ошибка при запуске сервера: {ex.Message}");
+            }
+            catch (Exception ex)
+            {
+                Debug.WriteLine("An error occurred: " + ex.Message);
+            }
+            finally
+            {
+                listener.Stop();
+            }
+        }
+
+        public async Task SendMessageAsync(TcpClient c)
+        {
+            if (c.GetStream() == null || !c.Connected)
+            {
+                Console.WriteLine("Not connected to server");
+                return;
+            }
+
+            while (c.Connected)
+            {
+                if (sendQueue.TryDequeue(out byte[] buffer))
+                {
+                    try
+                    {
+                        await c.GetStream().WriteAsync(buffer, 0, buffer.Length);
+                        Debug.WriteLine($"Sent");
+                    }
+                    catch (IOException ex) when (IsNetworkError(ex))
+                    {
+                        // Специальная обработка сетевых ошибок
+                        Debug.WriteLine($"Клиент отключился: {ex.Message}");
+                        HandleDisconnectedClient(c);
+                    }
+                    catch (Exception ex)
+                    {
+                        Debug.WriteLine($"Error sending message: {ex.Message}");
+                    }
+                }
+            }
+        }
+
+        public async override Task StartReadingAsync(object C)
+        {
+            TcpClient client;
+            if (C is TcpClient) client = (TcpClient)C;
+            else
+            {
+                return;
+            }
+            try
+            {
+                using NetworkStream stream = client.GetStream();
+
+                while (client.Connected)
+                {
+                    byte[] buffer = new byte[1024];
+                    int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length);
+                    //string response = await reader.ReadLineAsync();
+                    if (bytesRead > 0)
+                    {
+                        EnqueueData(buffer, bytesRead);
+                    }
+                    data_extract();
+                }
+            }
+            catch (IOException ex) when (IsNetworkError(ex))
+            {
+                // Специальная обработка сетевых ошибок
+                Debug.WriteLine($"Клиент отключился: {ex.Message}");
+                HandleDisconnectedClient(client);
+            }
+            catch (Exception ex)
+            {
+                Debug.WriteLine($"Error sending message: {ex.Message}");
+            }
+        }
+
+        protected override void ProcessCommand(int cmd, int slot, byte[] data, int offset, int len)
+        {
+            Debug.WriteLine(@"server cmd = " + cmd);
+
+            switch (cmd)
+            {
+                case TELE_CMD_HELLO:
+                    byte[] load = telemetry.getSlot(cmd, offset, len);
+                    putRequest(prepareTelegram(cmd, 0, load, 0, load.Length));
+                    break;
+
+                case TELE_CMD_RD_ONCE:
+                    byte[] load1 = telemetry.getSlot(slot, offset, len);
+                    putRequest(prepareTelegram(cmd, slot, load1, offset, load1.Length));
+                    break;
+
+                case TELE_CMD_WR:
+                    setData(data, slot, offset, len);
+                    sendVoidAnswer(TELE_CMD_WR);
+                    break;
+
+                case TELE_CMD_RD_MON_ON:
+                    monitoring.isMonitor = 1;
+                    sendVoidAnswer(TELE_CMD_RD_MON_ON);
+                    break;
+
+                case TELE_CMD_RD_MON_OFF:
+                    monitoring.isMonitor = 0;
+                    sendVoidAnswer(TELE_CMD_RD_MON_OFF);
+                    break;
+
+                case TELE_CMD_RD_MON_ADD:
+                    int id = monitoring.monitorPut(slot, offset, 4);
+                    sendIntAnswer(TELE_CMD_RD_MON_ADD, id);
+                    monitoring.isMonitor = 1;
+                    break;
+
+                case TELE_CMD_RD_MON_REMOVE:
+                    id = monitoring.monitorRemove(offset);//in this case offset == id
+                    if(monitoring.monitorFillLevel == 0)monitoring.isMonitor = 0;//if monitor_fill_level == 0 there is no item to monitor
+                    if (monitoring.isMonitor > 0) sendIntAnswer(TELE_CMD_RD_MON_REMOVE, id);
+                    else sendVoidAnswer(TELE_CMD_RD_MON_REMOVEALL);
+                    break;
+            }
+        }
+
+        private void setData(byte[] load, int slot, int offset, int len)
+        {
+            telemetry.setSlot(slot, load, offset, len);
+        }
+
+        private void sendVoidAnswer(int command)
+        {
+            putRequest(prepareTelegram(command, 0, new byte[0], 0));
+        }
+
+        private void sendIntAnswer(int command, int param)
+        {
+            putRequest(prepareTelegram(command, 0, new byte[0], param));
+        }
+
+        protected override void sendData(byte[] data)
+        {
+            sendQueue.Enqueue(data);
+        }
+
+        public override void setCommParams(iCommParams commParams)
+        {
+            if (commParams.GetType() == typeof(TCPCommParams))
+            {
+                var comm = (TCPCommParams)commParams;
+                IP = comm.IP;
+                Port = comm.Port;
+            }
+        }
+
+        private void HandleDisconnectedClient(TcpClient client)
+        {
+            isClientConnected = false;
+            StrikeOnTeleClientDisconnected();
+        }
+
+        private bool IsNetworkError(Exception ex)
+        {
+            // Проверяем типичные сетевые ошибки при разрыве соединения
+            return ex is IOException
+                   || ex is SocketException
+                   || ex.InnerException is SocketException;
+        }
+
+        public void StrikeOnTeleClientConnected()
+        {
+            OnTeleClientConnected?.Invoke(this, EventArgs.Empty);
+        }
+
+        public void StrikeOnTeleClientDisconnected()
+        {
+            OnTeleClientDisconnected?.Invoke(this, EventArgs.Empty);
+        }
+    }
+}

DroneClientCpp/Drone.cpp

@@ -59,55 +59,91 @@ namespace DroneClient {
         return GetBytes(mot4);
     }
 
-    // Реализация приватного метода RecvDataIMU
-    array<Byte>^ Drone::RecvDataIMU(array<Byte>^ data)
+    array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
     {
-        DroneData::DataIMU imu = (DroneData::DataIMU)FromBytes(data, DroneData::DataIMU::typeid);
+        DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);
 
         AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
-        GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
+        TimeAcc = imu.Time;
 
         return gcnew array<Byte>(0);
     }
 
-    // Реализация приватного метода RecvDataPos
-    array<Byte>^ Drone::RecvDataPos(array<Byte>^ data)
+    array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
     {
-        DroneData::DataPos pos = (DroneData::DataPos)FromBytes(data, DroneData::DataPos::typeid);
+      DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);
+
+      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
+      TimeGyr = imu.Time;
+
+      return gcnew array<Byte>(0);
+    }
+
+    array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
+    {
+      DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);
+
+      LaserRange = pos.LiDAR;
+      TimeRange = pos.Time;
+
+      return gcnew array<Byte>(0);
+    }
+
+    array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
+    {
+        DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);
 
         PosX = pos.Local.X; PosY = pos.Local.Y;
-        LaserRange = pos.LiDAR;
 
         return gcnew array<Byte>(0);
     }
 
-    // Реализация приватного метода ClientRequestResponse
     array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
     {
         array<Byte>^ zero = gcnew array<Byte>(0);
 
         switch (head.Type)
         {
-        case DroneData::DataType::DataIMU:
+        case DroneData::DataType::DataAcc:
             if (head.Mode == DroneData::DataMode::Request)
             {
                 return zero; // Запрос данных (не реализовано)
             }
             else
             {
-                return RecvDataIMU(body); // Пришли данные
+                return RecvDataAcc(body); // Пришли данные
             }
 
-        case DroneData::DataType::DataPos:
+        case DroneData::DataType::DataGyr:
+          if (head.Mode == DroneData::DataMode::Request)
+          {
+            return zero; // Запрос данных (не реализовано)
+          }
+          else
+          {
+            return RecvDataGyr(body); // Пришли данные
+          }
+
+        case DroneData::DataType::DataRange:
             if (head.Mode == DroneData::DataMode::Request)
             {
                 return zero; // Запрос данных (не реализовано)
             }
             else
             {
-                return RecvDataPos(body); // Пришли данные
+                return RecvDataRange(body); // Пришли данные
             }
 
+        case DroneData::DataType::DataLocal:
+          if (head.Mode == DroneData::DataMode::Request)
+          {
+            return zero; // Запрос данных (не реализовано)
+          }
+          else
+          {
+            return RecvDataLocal(body); // Пришли данные
+          }
+
         case DroneData::DataType::DataMotor4:
             if (head.Mode == DroneData::DataMode::Request)
             {
@@ -133,7 +169,7 @@ namespace DroneClient {
     }
 
     // Реализация метода DataStream
-    System::Collections::Generic::List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
+    List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
     {
         System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();
 
@@ -173,25 +209,47 @@ namespace DroneClient {
     // Реализация метода SendRequest
     array<Byte>^ Drone::SendRequest()
     {
-        DroneData::DataHead imu;
-        imu.Size = DroneData::DataHead::StrLen;
-        imu.Mode = DroneData::DataMode::Request;
-        imu.Type = DroneData::DataType::DataIMU;
+      DroneData::DataHead^ acc = gcnew DroneData::DataHead();
+      acc->Size = DroneData::DataHead::StrLen;
+      acc->Mode = DroneData::DataMode::Request;
+      acc->Type = DroneData::DataType::DataAcc;
 
-        DroneData::DataHead pos;
-        pos.Size = DroneData::DataHead::StrLen;
-        pos.Mode = DroneData::DataMode::Request;
-        pos.Type = DroneData::DataType::DataPos;
+      DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
+      gyr->Size = DroneData::DataHead::StrLen;
+      gyr->Mode = DroneData::DataMode::Request;
+      gyr->Type = DroneData::DataType::DataGyr;
 
-        array<Byte>^ si = GetBytes(imu);
-        array<Byte>^ sp = GetBytes(pos);
-        array<Byte>^ sm = SendDataMotor4();
+      DroneData::DataHead^ range = gcnew DroneData::DataHead();
+      range->Size = DroneData::DataHead::StrLen;
+      range->Mode = DroneData::DataMode::Request;
+      range->Type = DroneData::DataType::DataRange;
 
-        array<Byte>^ send = gcnew array<Byte>(si->Length + sp->Length + sm->Length);
-        Array::Copy(si, 0, send, 0, si->Length);
-        Array::Copy(sp, 0, send, si->Length, sp->Length);
-        Array::Copy(sm, 0, send, si->Length + sp->Length, sm->Length);
+      DroneData::DataHead^ local = gcnew DroneData::DataHead();
+      local->Size = DroneData::DataHead::StrLen;
+      local->Mode = DroneData::DataMode::Request;
+      local->Type = DroneData::DataType::DataLocal;
 
-        return send;
+      List<array<byte>^>^ list = gcnew List<array<byte>^>();
+
+      list->Add(GetBytes(acc));
+      list->Add(GetBytes(gyr));
+      list->Add(GetBytes(range));
+      list->Add(GetBytes(local));
+      list->Add(SendDataMotor4());
+
+      int count = 0;
+
+      for each(array<byte>^ d in list) count += d->Length;
+
+      array<byte>^ send = gcnew array<byte>(count);
+
+      count = 0;
+      for each (array<byte> ^ d in list)
+      {
+        d->CopyTo(send, count);
+        count += d->Length;
+      }
+
+      return send;
     }
 }

DroneClientCpp/Drone.h

@@ -18,8 +18,11 @@ namespace DroneClient {
     public:
         float AccX, AccY, AccZ;
         float GyrX, GyrY, GyrZ;
+        unsigned int TimeAcc, TimeGyr;
+
         float PosX, PosY;
         float LaserRange;
+        unsigned int TimeRange;
 
         float MotorUL, MotorUR, MotorDL, MotorDR;
 
@@ -28,8 +31,10 @@ namespace DroneClient {
 
     private:
         array<Byte>^ SendDataMotor4();
-        array<Byte>^ RecvDataIMU(array<Byte>^ data);
-        array<Byte>^ RecvDataPos(array<Byte>^ data);
+        array<Byte>^ RecvDataAcc(array<Byte>^ data);
+        array<Byte>^ RecvDataGyr(array<Byte>^ data);
+        array<Byte>^ RecvDataRange(array<Byte>^ data);
+        array<Byte>^ RecvDataLocal(array<Byte>^ data);
         array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
 
         literal int DroneStreamCount = 512;

DroneClientCpp/DroneData.h

@@ -15,18 +15,24 @@ namespace DroneData {
 
     public enum class DataType : unsigned short
     {
-        None = 0, Head = 1,
-        DataIMU = 1001, DataPos = 1002,
-        DataMotor4 = 2001, DataMotor6 = 2002
+      None = 0, Head = 1,
+
+      // Output
+      DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
+
+      // Input
+      DataMotor4 = 2001, DataMotor6 = 2002
     };
 
     public value struct DataHead
     {
     public:
-        int Size;
+      long Size;
         DataMode Mode;
         DataType Type;
 
+        unsigned long Time;
+
         static const int StrLen = sizeof(DataHead);
     };
 
@@ -36,30 +42,65 @@ namespace DroneData {
         float X, Y, Z;
     };
 
-    public value struct DataIMU
+    public value struct DataAcc
     {
     public:
         DataHead Head;
-        XYZ Acc, Gyr, Mag;
+        XYZ Acc;
 
-        static const int StrLen = sizeof(DataIMU);
+        unsigned long Time;
+
+        static const int StrLen = sizeof(DataAcc);
     };
 
-    public value struct DataPos
+    public value struct DataGyr
+    {
+    public:
+      DataHead Head;
+      XYZ Gyr;
+
+      unsigned long Time;
+
+      static const int StrLen = sizeof(DataGyr);
+    };
+
+    public value struct DataMag
+    {
+    public:
+      DataHead Head;
+      XYZ Mag;
+
+      unsigned long Time;
+
+      static const int StrLen = sizeof(DataMag);
+    };
+
+    public value struct DataRange
     {
     public:
         DataHead Head;
-        XYZ Local;
         float LiDAR;
 
-        static const int StrLen = sizeof(DataPos);
+        unsigned long Time;
+
+        static const int StrLen = sizeof(DataRange);
+    };
+
+    public value struct DataLocal
+    {
+    public:
+      DataHead Head;
+      XYZ Local;
+
+      unsigned long Time;
+
+      static const int StrLen = sizeof(DataLocal);
     };
 
     public value struct DataMotor4
     {
     public:
         DataHead Head;
-        unsigned long long Count;
         float UL, UR, DL, DR;
 
         static const int StrLen = sizeof(DataMotor4);
@@ -69,7 +110,6 @@ namespace DroneData {
     {
     public:
         DataHead Head;
-        unsigned long long Count;
         float UL, UR, LL, RR, DL, DR;
 
         static const int StrLen = sizeof(DataMotor6);

DroneClientCpp/NetClient.cpp

@@ -40,7 +40,7 @@ namespace DroneSimulator {
         IPEndPoint^ ep = gcnew IPEndPoint(IPAddress::Parse(Addr), Port);
         ServerSocket = gcnew Socket(AddressFamily::InterNetwork, SocketType::Stream, ProtocolType::Tcp);
 
-        try { ServerSocket->Connect(ep); }
+        try { if (ServerSocket) ServerSocket->Connect(ep); }
         catch (...) { ServerSocket->Close(); return ClientState::Error; }
 
         Connected = true;
@@ -49,7 +49,7 @@ namespace DroneSimulator {
 
         ReceiveData^ receiveData = gcnew ReceiveData(DataServer->buffer, ServerData::size, ServerSocket);
 
-        try { ServerSocket->BeginReceive(DataServer->buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), receiveData); }
+        try { if (ServerSocket) ServerSocket->BeginReceive(DataServer->buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), receiveData); }
         catch (...) {}
 
         return ClientState::Connected;
@@ -58,9 +58,9 @@ namespace DroneSimulator {
     // Реализация метода Close
     void NetClient::Close()
     {
-        try { ServerSocket->Shutdown(SocketShutdown::Both); }
+        try { if(ServerSocket) ServerSocket->Shutdown(SocketShutdown::Both); }
         catch (...) {}
-        ServerSocket->Close();
+        if(ServerSocket) ServerSocket->Close();
         Connected = false;
     }
 
@@ -69,7 +69,7 @@ namespace DroneSimulator {
     {
         if (ServerSocket != nullptr && Connected)
         {
-            try { ServerSocket->Send(data); }
+            try { if (ServerSocket) ServerSocket->Send(data); }
             catch (...) {}
         }
     }
@@ -86,7 +86,7 @@ namespace DroneSimulator {
 
         if (bytes == 0)
         {
-            ServerSocket->Close();
+          if (ServerSocket) ServerSocket->Close();
             Connected = false;
 
             if (ServerSocket != nullptr)
@@ -99,7 +99,7 @@ namespace DroneSimulator {
 
         ReceiveCallback(gcnew ReceiveData(cd->Buffer, bytes, ServerSocket));
 
-        try { ServerSocket->BeginReceive(cd->Buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), cd); }
+        try { if (ServerSocket) ServerSocket->BeginReceive(cd->Buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), cd); }
         catch (...) {}
     }
 }

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,35 +1,60 @@
-using System.CodeDom;
+using System.Collections.Concurrent;
+using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.InteropServices;
-using System.Security.Cryptography;
+using static System.Net.Mime.MediaTypeNames;
 
 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;
 
     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;
+    public static long Timing = 0;
+    public static long Lag = 0;
+    public static long Freq = 1000;
+    public static bool Boost = false;
 
-    private static int CounterID = 0;
+    private uint Timer;
+
+    private Vector2 MoveOF = Vector2.Zero;
+    private uint CountOF = 0;
+
+    public struct Physics
+    {
+      static public float Mass; // Масса
+      static public float Length; // Длинна лучей
+      static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
+    }
+
+    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)
     {
@@ -72,51 +97,73 @@ namespace DroneSimulator
       return mem;
     }
 
-    /*public struct DataOut
+    public VisualData.VisualDrone GetVisual(int count, int index)
     {
-      public float AccX, AccY, AccZ;
-      public float GyrX, GyrY, GyrZ;
-      public float PosX, PosY;
-      public float LaserRange;
+      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 DataOut GetDataOut()
+    public static void StartThread()
     {
-      DataOut data = new DataOut();
+      if (DroneThread != null) return;
 
-      data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
-      data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
-
-      data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
-
-      data.LaserRange = LaserRange;
-
-      return data;
+      DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
+      DroneThread.Priority = ThreadPriority.Highest;
+      DroneThread.Start();
     }
 
-    public struct DataIn
+    public static void StopThread()
     {
-      public float MotorUL, MotorUR, MotorDL, MotorDR;
-    }*/
+      if (DroneThread == null) return;
 
-    public struct DataVisual
-    {
-      public int ID; // Идентификатор
-      public float W, X, Y, Z; // Кватернион вращения
-      public float PosX, PosY, PosZ; // Координаты в пространстве
+      DroneThread = null;
     }
 
-    public DataVisual GetVisual()
+    private static void ThreadFunction()
     {
-      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 };
-    }
+      long last = Stopwatch.GetTimestamp();
+      long prev = Stopwatch.GetTimestamp();
 
-    private void ThreadFunction()
-    {
       while (DroneThread != null)
       {
-        Action(Environment.TickCount);
-        Thread.Sleep(1);
+        if(!Boost) Thread.Yield();
+
+        long tick = Stopwatch.GetTimestamp();
+
+        if (tick == prev) continue;
+
+        Timing = Stopwatch.Frequency / (tick - prev);
+        prev = tick;
+
+        long quant = Stopwatch.Frequency / Freq;
+
+        if (tick < last + quant) continue;
+
+        if (tick > (last + quant) + quant * 0.1) Lag++;
+
+        last = tick;
+
+        lock (AllDrones)
+          foreach (Drone drone in AllDrones)
+            drone.Action((uint)(tick / (Stopwatch.Frequency / 1000)));
       }
     }
 
@@ -128,28 +175,15 @@ namespace DroneSimulator
       SpdXYZ = Vector3.Zero;
       AccXYZ = Vector3.Zero;
       Quat = Quaternion.Identity;
-
-      DroneThread = new Thread(new ThreadStart(ThreadFunction));
-      Timer = Environment.TickCount;
-      DroneThread.Start();
     }
 
-    public int Create(float power, float mass, float len)
+    public int Create()
     {
-      Mass = mass;
-      Length = len;
-      MaxPower = power;
-
       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)
@@ -183,7 +217,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;
@@ -191,9 +225,13 @@ 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)
+    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;
@@ -205,47 +243,86 @@ 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)
@@ -253,17 +330,35 @@ namespace DroneSimulator
           //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;
       }
+
+      RealAcc.Update(Acc, tick);
+      RealGyr.Update(Gyr, tick);
+      RealRange.Update(LaserRange, tick);
+      RealBar.Update(PosXYZ.Z, tick);
+      RealPos.Update(PosXYZ, tick);
+
+      bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
+
+      if(of) lock (this)
+      {
+        MoveOF += RealOF.result;
+        CountOF += 1;
+      }
+      
+
+      DataTimer = tick;
     }
 
     private float Range(float pow)
@@ -271,7 +366,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)
@@ -292,47 +387,194 @@ namespace DroneSimulator
       SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
     }
 
-    private byte[] SendDataIMU()
+    private byte[] SendDataAcc()
     {
-      DroneData.DataIMU imu = new DroneData.DataIMU();
+      DroneData.DataAcc acc = new DroneData.DataAcc();
 
-      imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
-      imu.Head.Mode = DroneData.DataMode.Response;
-      imu.Head.Type = DroneData.DataType.DataIMU;
+      acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
+      acc.Head.Mode = DroneData.DataMode.Response;
+      acc.Head.Type = DroneData.DataType.DataAcc;
+      acc.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
-      imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
-      imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
-      imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;
+      acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
+      acc.Time = RealAcc.timer;
 
-      return getBytes(imu);
+      return getBytes(acc);
     }
 
-    private byte[] SendDataPos()
+    private byte[] SendDataGyr()
     {
-      DroneData.DataPos pos = new DroneData.DataPos();
+      DroneData.DataGyr gyr = new DroneData.DataGyr();
 
-      pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
-      pos.Head.Mode = DroneData.DataMode.Response;
-      pos.Head.Type = DroneData.DataType.DataPos;
+      gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
+      gyr.Head.Mode = DroneData.DataMode.Response;
+      gyr.Head.Type = DroneData.DataType.DataGyr;
+      gyr.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
-      pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
-      pos.LiDAR = LaserRange;
+      gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
+      gyr.Time = RealGyr.timer;
 
-      return getBytes(pos);
+      return getBytes(gyr);
+    }
+
+    private byte[] SendDataMag()
+    {
+      DroneData.DataMag mag = new DroneData.DataMag();
+
+      mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
+      mag.Head.Mode = DroneData.DataMode.Response;
+      mag.Head.Type = DroneData.DataType.DataMag;
+      mag.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
+      mag.Time = DataTimer;
+
+      return getBytes(mag);
+    }
+
+    private byte[] SendDataRange()
+    {
+      DroneData.DataRange range = new DroneData.DataRange();
+
+      range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
+      range.Head.Mode = DroneData.DataMode.Response;
+      range.Head.Type = DroneData.DataType.DataRange;
+      range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      range.LiDAR = RealRange.result;
+      range.Time = RealRange.timer;
+
+      return getBytes(range);
+    }
+
+    private byte[] SendDataLocal()
+    {
+      DroneData.DataLocal local = new DroneData.DataLocal();
+
+      local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
+      local.Head.Mode = DroneData.DataMode.Response;
+      local.Head.Type = DroneData.DataType.DataLocal;
+      local.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
+      local.Time = RealPos.timer;
+
+      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)(Stopwatch.GetTimestamp() / (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)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      lock (this)
+      {
+        of.X = MoveOF.X / CountOF;
+        of.Y = MoveOF.Y / CountOF;
+        of.Time = RealOF.timer;
+
+        MoveOF = Vector2.Zero;
+        CountOF = 0;
+      }
+
+      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)(Stopwatch.GetTimestamp() / (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)(Stopwatch.GetTimestamp() / (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.None;
+      head.Time = (uint)(Stopwatch.GetTimestamp() / (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.DataIMU:
+        case DroneData.DataType.DataAcc:
           {
             if (head.Mode == DroneData.DataMode.Request)
             {
               // Запрос данных
-              return SendDataIMU();
+              return SendDataAcc();
             }
             else
             {
@@ -343,38 +585,25 @@ namespace DroneSimulator
             }
           }
 
-        case DroneData.DataType.DataPos:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataPos();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
+        case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;
 
-        case DroneData.DataType.DataMotor4:
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              // ... //
-              //
-              return zero;
-            }
-            else
-            {
-              // Пришли данные
-              RecvDataMotor4(body);
-              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.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else 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.None: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       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
-    DataIMU = 1001, DataPos = 1002, 
+    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,32 +29,106 @@ namespace DroneData
     public DataMode Mode;
     public DataType Type;
 
+    public uint Time; // Общее время
+
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
   }
 
   public struct XYZ { public float X, Y, Z; }
 
-  public struct DataIMU
+  public struct DataAcc
   {
     public DataHead Head;
-    public XYZ Acc, Gyr, Mag;
+    public XYZ Acc;
 
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
   }
 
-  public struct DataPos
+  public struct DataGyr
   {
     public DataHead Head;
-    public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-    public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public XYZ Gyr;
 
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataPos));
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
+  }
+
+  public struct DataMag
+  {
+    public DataHead Head;
+    public XYZ Mag;
+
+    public uint Time; // Последнее время изменения данных
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
+  }
+
+  public struct DataRange
+  {
+    public DataHead Head;
+    public float LiDAR; // Датчик посадки
+
+    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 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 ulong Count;
     public float UL, UR, DL, DR;
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
@@ -59,9 +137,16 @@ namespace DroneData
   public struct DataMotor6
   {
     public DataHead Head;
-    public ulong Count;
     public float UL, UR, LL, RR, DL, DR;
 
     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)
@@ -36,24 +45,28 @@ 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);
 
-        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;
@@ -75,7 +91,7 @@ namespace DroneSimulator
       List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
 
       if (send == null) return;
-      try 
+      try
       {
         foreach (byte[]? b in send)
         {
@@ -112,6 +128,8 @@ namespace DroneSimulator
 
       if (done != NetServerClients.ServerState.Start) return;
 
+      Drone.StartThread();
+
       pictureBox_2D.Image = null;
 
       screen2D = new Screen2D(DrawCallback);
@@ -133,19 +151,36 @@ 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;
 
-      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());
+
+            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 { }
+
+      label_Timing.Text = Drone.Timing.ToString() + " Hz";
+      label_Timing_Lag.Text = Drone.Lag.ToString();
 
       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 +188,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 +205,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 +244,133 @@ 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 numericUpDown_Timing_Freq_ValueChanged(object sender, EventArgs e)
+    {
+      Drone.Freq = (long)numericUpDown_Timing_Freq.Value;
+      Drone.Boost = checkBox_Freq_Boost.Checked;
+    }
   }
 }

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,407 @@
+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 * 12.15f;
+
+        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 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 bool Update(Vector2 value, float Range, uint time)
+      {
+        value *= Lens;
+
+        if (!Enable)
+        {
+          result = Vector2.NaN;
+          return true;
+        }
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return true;
+        }
+
+        if (Range > MaxHeight) value = Vector2.Zero;
+        else
+        {
+          int noise = (int)(Noise * 1000);
+          value.X += ((float)rand.Next(-noise, noise)) / 1000;
+          value.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        }
+
+        uint clock = (uint)(Lateness * 1000);
+
+        uint tick = time - last;
+        last = time;
+        while (tick != 0)
+        {
+          tick--;
+          laten[index++] = value;
+          if (index >= clock) index = 0;
+        }
+
+        value = laten[index];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq < time)
+        {
+          result = value;
+          timer = time;
+          return true;
+        }
+
+        return false;
+      }
+    }
+
+    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

@@ -106,51 +106,55 @@ 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;
+            var bmp = RotateImage(d.Drone, d.Azimuth);
 
-          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);
-          }
+            g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
 
-          PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
-          PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
-          PointF[] dest = { ul, ur, dl };
+            g.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, dest);
+            //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)
+            {
+              x1 = (int)(Math.Sin(tilt.Y) * 130);
+              x3 = (int)(Math.Sin(tilt.Y) * 130);
+            }
+            else
+            {
+              x2 = (int)(Math.Cos(tilt.Y) * 130);
+            }
+
+            if (tilt.X > 0)
+            {
+              y1 = (int)(Math.Sin(tilt.X) * 130);
+              y2 = (int)(Math.Sin(tilt.X) * 130);
+            }
+            else
+            {
+              y3 = (int)(Math.Cos(tilt.X) * 130);
+            }
+
+            PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
+            PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
+            PointF[] dest = { ul, ur, dl };
+
+            g.DrawImage(bmp, dest);
+          }
+          catch { }
         }
       }
 

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