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/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/FormMain.Designer.cs

@@ -31,6 +31,11 @@
             components = new System.ComponentModel.Container();
             timer_Test = new System.Windows.Forms.Timer(components);
             groupBox1 = new GroupBox();
+            TeleClientStatusPicture = new PictureBox();
+            label12 = new Label();
+            button1 = new Button();
+            TeleServerPortCtrl = new NumericUpDown();
+            label8 = new Label();
             textBox_Server_Addr = new TextBox();
             label2 = new Label();
             button_Connect = new Button();
@@ -77,6 +82,8 @@
             trackBar_Value = new TrackBar();
             label4 = new Label();
             groupBox1.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)TeleClientStatusPicture).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)TeleServerPortCtrl).BeginInit();
             ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
             groupBox2.SuspendLayout();
             groupBox3.SuspendLayout();
@@ -94,6 +101,11 @@
             // 
             // groupBox1
             // 
+            groupBox1.Controls.Add(TeleClientStatusPicture);
+            groupBox1.Controls.Add(label12);
+            groupBox1.Controls.Add(button1);
+            groupBox1.Controls.Add(TeleServerPortCtrl);
+            groupBox1.Controls.Add(label8);
             groupBox1.Controls.Add(textBox_Server_Addr);
             groupBox1.Controls.Add(label2);
             groupBox1.Controls.Add(button_Connect);
@@ -101,27 +113,83 @@
             groupBox1.Controls.Add(label3);
             groupBox1.Dock = DockStyle.Top;
             groupBox1.Location = new Point(0, 0);
+            groupBox1.Margin = new Padding(3, 4, 3, 4);
             groupBox1.Name = "groupBox1";
-      groupBox1.Size = new Size(446, 80);
+            groupBox1.Padding = new Padding(3, 4, 3, 4);
+            groupBox1.Size = new Size(510, 107);
             groupBox1.TabIndex = 3;
             groupBox1.TabStop = false;
             groupBox1.Tag = "";
             groupBox1.Text = "Server";
             // 
+            // TeleClientStatusPicture
+            // 
+            TeleClientStatusPicture.Anchor = AnchorStyles.Top | AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right;
+            TeleClientStatusPicture.Location = new Point(469, 53);
+            TeleClientStatusPicture.Name = "TeleClientStatusPicture";
+            TeleClientStatusPicture.Size = new Size(24, 24);
+            TeleClientStatusPicture.TabIndex = 9;
+            TeleClientStatusPicture.TabStop = false;
+            // 
+            // label12
+            // 
+            label12.AutoSize = true;
+            label12.Location = new Point(393, 51);
+            label12.Name = "label12";
+            label12.Size = new Size(74, 20);
+            label12.TabIndex = 8;
+            label12.Tag = "#clients_port";
+            label12.Text = "TeleClient";
+            // 
+            // button1
+            // 
+            button1.BackColor = Color.Transparent;
+            button1.Location = new Point(369, 16);
+            button1.Margin = new Padding(3, 4, 3, 4);
+            button1.Name = "button1";
+            button1.Size = new Size(127, 31);
+            button1.TabIndex = 7;
+            button1.Tag = "";
+            button1.Text = "Run TeleServer";
+            button1.UseVisualStyleBackColor = false;
+            button1.Click += button1_Click;
+            // 
+            // TeleServerPortCtrl
+            // 
+            TeleServerPortCtrl.Location = new Point(291, 18);
+            TeleServerPortCtrl.Margin = new Padding(3, 4, 3, 4);
+            TeleServerPortCtrl.Maximum = new decimal(new int[] { 65000, 0, 0, 0 });
+            TeleServerPortCtrl.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
+            TeleServerPortCtrl.Name = "TeleServerPortCtrl";
+            TeleServerPortCtrl.Size = new Size(71, 27);
+            TeleServerPortCtrl.TabIndex = 6;
+            TeleServerPortCtrl.Value = new decimal(new int[] { 8888, 0, 0, 0 });
+            // 
+            // label8
+            // 
+            label8.AutoSize = true;
+            label8.Location = new Point(248, 21);
+            label8.Name = "label8";
+            label8.Size = new Size(38, 20);
+            label8.TabIndex = 5;
+            label8.Tag = "#clients_port";
+            label8.Text = "Port:";
+            // 
             // textBox_Server_Addr
             // 
-      textBox_Server_Addr.Location = new Point(48, 16);
+            textBox_Server_Addr.Location = new Point(55, 21);
+            textBox_Server_Addr.Margin = new Padding(3, 4, 3, 4);
             textBox_Server_Addr.Name = "textBox_Server_Addr";
-      textBox_Server_Addr.Size = new Size(125, 23);
+            textBox_Server_Addr.Size = new Size(142, 27);
             textBox_Server_Addr.TabIndex = 4;
             textBox_Server_Addr.Text = "127.0.0.1";
             // 
             // label2
             // 
             label2.AutoSize = true;
-      label2.Location = new Point(6, 19);
+            label2.Location = new Point(7, 25);
             label2.Name = "label2";
-      label2.Size = new Size(36, 15);
+            label2.Size = new Size(45, 20);
             label2.TabIndex = 3;
             label2.Tag = "";
             label2.Text = "Addr:";
@@ -129,9 +197,10 @@
             // button_Connect
             // 
             button_Connect.BackColor = Color.Transparent;
-      button_Connect.Location = new Point(112, 46);
+            button_Connect.Location = new Point(128, 61);
+            button_Connect.Margin = new Padding(3, 4, 3, 4);
             button_Connect.Name = "button_Connect";
-      button_Connect.Size = new Size(61, 23);
+            button_Connect.Size = new Size(70, 31);
             button_Connect.TabIndex = 2;
             button_Connect.Tag = "";
             button_Connect.Text = "Connect";
@@ -140,20 +209,21 @@
             // 
             // numericUpDown_Server_Port
             // 
-      numericUpDown_Server_Port.Location = new Point(44, 48);
+            numericUpDown_Server_Port.Location = new Point(50, 64);
+            numericUpDown_Server_Port.Margin = new Padding(3, 4, 3, 4);
             numericUpDown_Server_Port.Maximum = new decimal(new int[] { 65000, 0, 0, 0 });
             numericUpDown_Server_Port.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
             numericUpDown_Server_Port.Name = "numericUpDown_Server_Port";
-      numericUpDown_Server_Port.Size = new Size(62, 23);
+            numericUpDown_Server_Port.Size = new Size(71, 27);
             numericUpDown_Server_Port.TabIndex = 1;
             numericUpDown_Server_Port.Value = new decimal(new int[] { 1001, 0, 0, 0 });
             // 
             // label3
             // 
             label3.AutoSize = true;
-      label3.Location = new Point(6, 50);
+            label3.Location = new Point(7, 67);
             label3.Name = "label3";
-      label3.Size = new Size(32, 15);
+            label3.Size = new Size(38, 20);
             label3.TabIndex = 0;
             label3.Tag = "#clients_port";
             label3.Text = "Port:";
@@ -161,9 +231,9 @@
             // label1
             // 
             label1.AutoSize = true;
-      label1.Location = new Point(6, 19);
+            label1.Location = new Point(7, 25);
             label1.Name = "label1";
-      label1.Size = new Size(17, 15);
+            label1.Size = new Size(21, 20);
             label1.TabIndex = 4;
             label1.Text = "X:";
             // 
@@ -176,9 +246,11 @@
             groupBox2.Controls.Add(label5);
             groupBox2.Controls.Add(label_Acc_X);
             groupBox2.Controls.Add(label1);
-      groupBox2.Location = new Point(6, 86);
+            groupBox2.Location = new Point(7, 115);
+            groupBox2.Margin = new Padding(3, 4, 3, 4);
             groupBox2.Name = "groupBox2";
-      groupBox2.Size = new Size(100, 118);
+            groupBox2.Padding = new Padding(3, 4, 3, 4);
+            groupBox2.Size = new Size(114, 157);
             groupBox2.TabIndex = 5;
             groupBox2.TabStop = false;
             groupBox2.Text = "Acc";
@@ -186,54 +258,54 @@
             // label_time_acc
             // 
             label_time_acc.AutoSize = true;
-      label_time_acc.Location = new Point(6, 100);
+            label_time_acc.Location = new Point(7, 133);
             label_time_acc.Name = "label_time_acc";
-      label_time_acc.Size = new Size(13, 15);
+            label_time_acc.Size = new Size(17, 20);
             label_time_acc.TabIndex = 25;
             label_time_acc.Text = "0";
             // 
             // label_Acc_Z
             // 
             label_Acc_Z.AutoSize = true;
-      label_Acc_Z.Location = new Point(19, 70);
+            label_Acc_Z.Location = new Point(22, 93);
             label_Acc_Z.Name = "label_Acc_Z";
-      label_Acc_Z.Size = new Size(13, 15);
+            label_Acc_Z.Size = new Size(17, 20);
             label_Acc_Z.TabIndex = 9;
             label_Acc_Z.Text = "0";
             // 
             // label7
             // 
             label7.AutoSize = true;
-      label7.Location = new Point(6, 70);
+            label7.Location = new Point(7, 93);
             label7.Name = "label7";
-      label7.Size = new Size(17, 15);
+            label7.Size = new Size(21, 20);
             label7.TabIndex = 8;
             label7.Text = "Z:";
             // 
             // label_Acc_Y
             // 
             label_Acc_Y.AutoSize = true;
-      label_Acc_Y.Location = new Point(19, 45);
+            label_Acc_Y.Location = new Point(22, 60);
             label_Acc_Y.Name = "label_Acc_Y";
-      label_Acc_Y.Size = new Size(13, 15);
+            label_Acc_Y.Size = new Size(17, 20);
             label_Acc_Y.TabIndex = 7;
             label_Acc_Y.Text = "0";
             // 
             // label5
             // 
             label5.AutoSize = true;
-      label5.Location = new Point(6, 45);
+            label5.Location = new Point(7, 60);
             label5.Name = "label5";
-      label5.Size = new Size(17, 15);
+            label5.Size = new Size(20, 20);
             label5.TabIndex = 6;
             label5.Text = "Y:";
             // 
             // label_Acc_X
             // 
             label_Acc_X.AutoSize = true;
-      label_Acc_X.Location = new Point(19, 19);
+            label_Acc_X.Location = new Point(22, 25);
             label_Acc_X.Name = "label_Acc_X";
-      label_Acc_X.Size = new Size(13, 15);
+            label_Acc_X.Size = new Size(17, 20);
             label_Acc_X.TabIndex = 5;
             label_Acc_X.Text = "0";
             // 
@@ -246,9 +318,11 @@
             groupBox3.Controls.Add(label11);
             groupBox3.Controls.Add(label_Gyr_X);
             groupBox3.Controls.Add(label13);
-      groupBox3.Location = new Point(112, 86);
+            groupBox3.Location = new Point(128, 115);
+            groupBox3.Margin = new Padding(3, 4, 3, 4);
             groupBox3.Name = "groupBox3";
-      groupBox3.Size = new Size(103, 118);
+            groupBox3.Padding = new Padding(3, 4, 3, 4);
+            groupBox3.Size = new Size(118, 157);
             groupBox3.TabIndex = 6;
             groupBox3.TabStop = false;
             groupBox3.Text = "Gyr";
@@ -256,63 +330,63 @@
             // label_time_gyr
             // 
             label_time_gyr.AutoSize = true;
-      label_time_gyr.Location = new Point(3, 100);
+            label_time_gyr.Location = new Point(3, 133);
             label_time_gyr.Name = "label_time_gyr";
-      label_time_gyr.Size = new Size(13, 15);
+            label_time_gyr.Size = new Size(17, 20);
             label_time_gyr.TabIndex = 26;
             label_time_gyr.Text = "0";
             // 
             // label_Gyr_Z
             // 
             label_Gyr_Z.AutoSize = true;
-      label_Gyr_Z.Location = new Point(19, 70);
+            label_Gyr_Z.Location = new Point(22, 93);
             label_Gyr_Z.Name = "label_Gyr_Z";
-      label_Gyr_Z.Size = new Size(13, 15);
+            label_Gyr_Z.Size = new Size(17, 20);
             label_Gyr_Z.TabIndex = 9;
             label_Gyr_Z.Text = "0";
             // 
             // label9
             // 
             label9.AutoSize = true;
-      label9.Location = new Point(6, 70);
+            label9.Location = new Point(7, 93);
             label9.Name = "label9";
-      label9.Size = new Size(17, 15);
+            label9.Size = new Size(21, 20);
             label9.TabIndex = 8;
             label9.Text = "Z:";
             // 
             // label_Gyr_Y
             // 
             label_Gyr_Y.AutoSize = true;
-      label_Gyr_Y.Location = new Point(19, 45);
+            label_Gyr_Y.Location = new Point(22, 60);
             label_Gyr_Y.Name = "label_Gyr_Y";
-      label_Gyr_Y.Size = new Size(13, 15);
+            label_Gyr_Y.Size = new Size(17, 20);
             label_Gyr_Y.TabIndex = 7;
             label_Gyr_Y.Text = "0";
             // 
             // label11
             // 
             label11.AutoSize = true;
-      label11.Location = new Point(6, 45);
+            label11.Location = new Point(7, 60);
             label11.Name = "label11";
-      label11.Size = new Size(17, 15);
+            label11.Size = new Size(20, 20);
             label11.TabIndex = 6;
             label11.Text = "Y:";
             // 
             // label_Gyr_X
             // 
             label_Gyr_X.AutoSize = true;
-      label_Gyr_X.Location = new Point(19, 19);
+            label_Gyr_X.Location = new Point(22, 25);
             label_Gyr_X.Name = "label_Gyr_X";
-      label_Gyr_X.Size = new Size(13, 15);
+            label_Gyr_X.Size = new Size(17, 20);
             label_Gyr_X.TabIndex = 5;
             label_Gyr_X.Text = "0";
             // 
             // label13
             // 
             label13.AutoSize = true;
-      label13.Location = new Point(6, 19);
+            label13.Location = new Point(7, 25);
             label13.Name = "label13";
-      label13.Size = new Size(17, 15);
+            label13.Size = new Size(21, 20);
             label13.TabIndex = 4;
             label13.Text = "X:";
             // 
@@ -325,9 +399,11 @@
             groupBox4.Controls.Add(label10);
             groupBox4.Controls.Add(label_Pos_X);
             groupBox4.Controls.Add(label14);
-      groupBox4.Location = new Point(221, 86);
+            groupBox4.Location = new Point(253, 115);
+            groupBox4.Margin = new Padding(3, 4, 3, 4);
             groupBox4.Name = "groupBox4";
-      groupBox4.Size = new Size(103, 118);
+            groupBox4.Padding = new Padding(3, 4, 3, 4);
+            groupBox4.Size = new Size(118, 157);
             groupBox4.TabIndex = 7;
             groupBox4.TabStop = false;
             groupBox4.Text = "Pos";
@@ -335,81 +411,83 @@
             // label_time_range
             // 
             label_time_range.AutoSize = true;
-      label_time_range.Location = new Point(6, 100);
+            label_time_range.Location = new Point(7, 133);
             label_time_range.Name = "label_time_range";
-      label_time_range.Size = new Size(13, 15);
+            label_time_range.Size = new Size(17, 20);
             label_time_range.TabIndex = 27;
             label_time_range.Text = "0";
             // 
             // label_Pos_L
             // 
             label_Pos_L.AutoSize = true;
-      label_Pos_L.Location = new Point(19, 70);
+            label_Pos_L.Location = new Point(22, 93);
             label_Pos_L.Name = "label_Pos_L";
-      label_Pos_L.Size = new Size(13, 15);
+            label_Pos_L.Size = new Size(17, 20);
             label_Pos_L.TabIndex = 9;
             label_Pos_L.Text = "0";
             // 
             // label6
             // 
             label6.AutoSize = true;
-      label6.Location = new Point(6, 70);
+            label6.Location = new Point(7, 93);
             label6.Name = "label6";
-      label6.Size = new Size(16, 15);
+            label6.Size = new Size(19, 20);
             label6.TabIndex = 8;
             label6.Text = "L:";
             // 
             // label_Pos_Y
             // 
             label_Pos_Y.AutoSize = true;
-      label_Pos_Y.Location = new Point(19, 45);
+            label_Pos_Y.Location = new Point(22, 60);
             label_Pos_Y.Name = "label_Pos_Y";
-      label_Pos_Y.Size = new Size(13, 15);
+            label_Pos_Y.Size = new Size(17, 20);
             label_Pos_Y.TabIndex = 7;
             label_Pos_Y.Text = "0";
             // 
             // label10
             // 
             label10.AutoSize = true;
-      label10.Location = new Point(6, 45);
+            label10.Location = new Point(7, 60);
             label10.Name = "label10";
-      label10.Size = new Size(17, 15);
+            label10.Size = new Size(20, 20);
             label10.TabIndex = 6;
             label10.Text = "Y:";
             // 
             // label_Pos_X
             // 
             label_Pos_X.AutoSize = true;
-      label_Pos_X.Location = new Point(19, 19);
+            label_Pos_X.Location = new Point(22, 25);
             label_Pos_X.Name = "label_Pos_X";
-      label_Pos_X.Size = new Size(13, 15);
+            label_Pos_X.Size = new Size(17, 20);
             label_Pos_X.TabIndex = 5;
             label_Pos_X.Text = "0";
             // 
             // label14
             // 
             label14.AutoSize = true;
-      label14.Location = new Point(6, 19);
+            label14.Location = new Point(7, 25);
             label14.Name = "label14";
-      label14.Size = new Size(17, 15);
+            label14.Size = new Size(21, 20);
             label14.TabIndex = 4;
             label14.Text = "X:";
             // 
             // trackBar_Power
             // 
-      trackBar_Power.Location = new Point(112, 240);
+            trackBar_Power.Location = new Point(128, 320);
+            trackBar_Power.Margin = new Padding(3, 4, 3, 4);
             trackBar_Power.Maximum = 100;
             trackBar_Power.Name = "trackBar_Power";
             trackBar_Power.Orientation = Orientation.Vertical;
-      trackBar_Power.Size = new Size(45, 141);
+            trackBar_Power.Size = new Size(56, 188);
             trackBar_Power.TabIndex = 12;
             trackBar_Power.Scroll += trackBar_Power_Scroll;
             // 
             // button_LL
             // 
-      button_LL.Location = new Point(9, 318);
+            button_LL.Location = new Point(10, 424);
+            button_LL.Margin = new Padding(3, 4, 3, 4);
             button_LL.Name = "button_LL";
-      button_LL.Size = new Size(75, 23);
+            button_LL.Size = new Size(86, 31);
             button_LL.TabIndex = 13;
             button_LL.Text = "LL";
             button_LL.UseVisualStyleBackColor = true;
@@ -418,9 +496,10 @@
             // 
             // button_UU
             // 
-      button_UU.Location = new Point(98, 211);
+            button_UU.Location = new Point(112, 281);
+            button_UU.Margin = new Padding(3, 4, 3, 4);
             button_UU.Name = "button_UU";
-      button_UU.Size = new Size(75, 23);
+            button_UU.Size = new Size(86, 31);
             button_UU.TabIndex = 14;
             button_UU.Text = "UU";
             button_UU.UseVisualStyleBackColor = true;
@@ -429,9 +508,10 @@
             // 
             // button_DD
             // 
-      button_DD.Location = new Point(98, 412);
+            button_DD.Location = new Point(112, 549);
+            button_DD.Margin = new Padding(3, 4, 3, 4);
             button_DD.Name = "button_DD";
-      button_DD.Size = new Size(75, 23);
+            button_DD.Size = new Size(86, 31);
             button_DD.TabIndex = 15;
             button_DD.Text = "DD";
             button_DD.UseVisualStyleBackColor = true;
@@ -440,9 +520,10 @@
             // 
             // button_RR
             // 
-      button_RR.Location = new Point(188, 318);
+            button_RR.Location = new Point(215, 424);
+            button_RR.Margin = new Padding(3, 4, 3, 4);
             button_RR.Name = "button_RR";
-      button_RR.Size = new Size(75, 23);
+            button_RR.Size = new Size(86, 31);
             button_RR.TabIndex = 16;
             button_RR.Text = "RR";
             button_RR.UseVisualStyleBackColor = true;
@@ -452,17 +533,18 @@
             // label_Pow
             // 
             label_Pow.AutoSize = true;
-      label_Pow.Location = new Point(126, 384);
+            label_Pow.Location = new Point(144, 512);
             label_Pow.Name = "label_Pow";
-      label_Pow.Size = new Size(13, 15);
+            label_Pow.Size = new Size(17, 20);
             label_Pow.TabIndex = 21;
             label_Pow.Text = "0";
             // 
             // button_ML
             // 
-      button_ML.Location = new Point(9, 211);
+            button_ML.Location = new Point(10, 281);
+            button_ML.Margin = new Padding(3, 4, 3, 4);
             button_ML.Name = "button_ML";
-      button_ML.Size = new Size(75, 23);
+            button_ML.Size = new Size(86, 31);
             button_ML.TabIndex = 22;
             button_ML.Text = "<-";
             button_ML.UseVisualStyleBackColor = true;
@@ -471,9 +553,10 @@
             // 
             // button_MR
             // 
-      button_MR.Location = new Point(188, 211);
+            button_MR.Location = new Point(215, 281);
+            button_MR.Margin = new Padding(3, 4, 3, 4);
             button_MR.Name = "button_MR";
-      button_MR.Size = new Size(75, 23);
+            button_MR.Size = new Size(86, 31);
             button_MR.TabIndex = 23;
             button_MR.Text = "->";
             button_MR.UseVisualStyleBackColor = true;
@@ -487,9 +570,11 @@
             groupBox5.Controls.Add(label17);
             groupBox5.Controls.Add(label_OF_X);
             groupBox5.Controls.Add(label19);
-      groupBox5.Location = new Point(330, 86);
+            groupBox5.Location = new Point(377, 115);
+            groupBox5.Margin = new Padding(3, 4, 3, 4);
             groupBox5.Name = "groupBox5";
-      groupBox5.Size = new Size(104, 118);
+            groupBox5.Padding = new Padding(3, 4, 3, 4);
+            groupBox5.Size = new Size(119, 157);
             groupBox5.TabIndex = 24;
             groupBox5.TabStop = false;
             groupBox5.Text = "OF";
@@ -497,73 +582,74 @@
             // label_time_of
             // 
             label_time_of.AutoSize = true;
-      label_time_of.Location = new Point(6, 100);
+            label_time_of.Location = new Point(7, 133);
             label_time_of.Name = "label_time_of";
-      label_time_of.Size = new Size(13, 15);
+            label_time_of.Size = new Size(17, 20);
             label_time_of.TabIndex = 27;
             label_time_of.Text = "0";
             // 
             // label_OF_Y
             // 
             label_OF_Y.AutoSize = true;
-      label_OF_Y.Location = new Point(19, 45);
+            label_OF_Y.Location = new Point(22, 60);
             label_OF_Y.Name = "label_OF_Y";
-      label_OF_Y.Size = new Size(13, 15);
+            label_OF_Y.Size = new Size(17, 20);
             label_OF_Y.TabIndex = 7;
             label_OF_Y.Text = "0";
             // 
             // label17
             // 
             label17.AutoSize = true;
-      label17.Location = new Point(6, 45);
+            label17.Location = new Point(7, 60);
             label17.Name = "label17";
-      label17.Size = new Size(17, 15);
+            label17.Size = new Size(20, 20);
             label17.TabIndex = 6;
             label17.Text = "Y:";
             // 
             // label_OF_X
             // 
             label_OF_X.AutoSize = true;
-      label_OF_X.Location = new Point(19, 19);
+            label_OF_X.Location = new Point(22, 25);
             label_OF_X.Name = "label_OF_X";
-      label_OF_X.Size = new Size(13, 15);
+            label_OF_X.Size = new Size(17, 20);
             label_OF_X.TabIndex = 5;
             label_OF_X.Text = "0";
             // 
             // label19
             // 
             label19.AutoSize = true;
-      label19.Location = new Point(6, 19);
+            label19.Location = new Point(7, 25);
             label19.Name = "label19";
-      label19.Size = new Size(17, 15);
+            label19.Size = new Size(21, 20);
             label19.TabIndex = 4;
             label19.Text = "X:";
             // 
             // trackBar_Value
             // 
-      trackBar_Value.Location = new Point(349, 240);
+            trackBar_Value.Location = new Point(399, 320);
+            trackBar_Value.Margin = new Padding(3, 4, 3, 4);
             trackBar_Value.Maximum = 100;
             trackBar_Value.Minimum = 1;
             trackBar_Value.Name = "trackBar_Value";
             trackBar_Value.Orientation = Orientation.Vertical;
-      trackBar_Value.Size = new Size(45, 195);
+            trackBar_Value.Size = new Size(56, 260);
             trackBar_Value.TabIndex = 25;
             trackBar_Value.Value = 1;
             // 
             // label4
             // 
             label4.AutoSize = true;
-      label4.Location = new Point(347, 219);
+            label4.Location = new Point(397, 292);
             label4.Name = "label4";
-      label4.Size = new Size(47, 15);
+            label4.Size = new Size(59, 20);
             label4.TabIndex = 26;
             label4.Text = "POWER";
             // 
             // Form_Main
             // 
-      AutoScaleDimensions = new SizeF(7F, 15F);
+            AutoScaleDimensions = new SizeF(8F, 20F);
             AutoScaleMode = AutoScaleMode.Font;
-      ClientSize = new Size(446, 447);
+            ClientSize = new Size(510, 596);
             Controls.Add(label4);
             Controls.Add(trackBar_Value);
             Controls.Add(groupBox5);
@@ -579,12 +665,15 @@
             Controls.Add(groupBox3);
             Controls.Add(groupBox2);
             Controls.Add(groupBox1);
-      MinimumSize = new Size(291, 389);
+            Margin = new Padding(3, 4, 3, 4);
+            MinimumSize = new Size(330, 503);
             Name = "Form_Main";
             Text = "Drone Client V1.0";
             FormClosing += Form_Main_FormClosing;
             groupBox1.ResumeLayout(false);
             groupBox1.PerformLayout();
+            ((System.ComponentModel.ISupportInitialize)TeleClientStatusPicture).EndInit();
+            ((System.ComponentModel.ISupportInitialize)TeleServerPortCtrl).EndInit();
             ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).EndInit();
             groupBox2.ResumeLayout(false);
             groupBox2.PerformLayout();
@@ -648,5 +737,10 @@
     private Label label_OF_X;
     private Label label19;
     private TrackBar trackBar_Value;
+        private Button button1;
+        private NumericUpDown TeleServerPortCtrl;
+        private Label label8;
+        private PictureBox TeleClientStatusPicture;
+        private Label label12;
     }
 }

DroneClient/FormMain.cs

@@ -3,16 +3,85 @@ 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();
+        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");
 
         public Form_Main()
         {
             InitializeComponent();
+            tele_server = new TelemetryServer();
+            if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
+        }
+
+        private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e)
+        {
+            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)
@@ -81,6 +150,18 @@ namespace DroneSimulator
             }
 
             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)
@@ -103,17 +184,29 @@ namespace DroneSimulator
 
             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());
         }
 
@@ -168,5 +261,20 @@ namespace DroneSimulator
         {
             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;
+        }
     }
 }

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);
+        }
+    }
+}

DroneSimulator/Drone.cs

@@ -1,8 +1,8 @@
-using System.Diagnostics;
+using System.Collections.Concurrent;
-using System.Net.Sockets;
+using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.InteropServices;
-using static VisualData.VisualDrone;
+using static System.Net.Mime.MediaTypeNames;
 
 namespace DroneSimulator
 {
@@ -10,12 +10,10 @@ namespace DroneSimulator
   {
     public int ID;
 
-    private Socket? Client = null;
+    public bool Active; // Живой?
-
-    public bool Active = false; // Живой?
     public const float Dynamic = 10; // Динамика вращения
-    public Vector3 PosXYZ = Vector3.Zero, SpdXYZ = Vector3.Zero, AccXYZ = Vector3.Zero; // Положение в пространстве: Позиция, Скорость, Ускорение
+    public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
-    public Quaternion Quat = Quaternion.Identity; // Основной кватернион
+    public Quaternion Quat; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей (0-1)
     
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
@@ -25,6 +23,8 @@ namespace DroneSimulator
 
     public Vector4 Orientation;
 
+    private uint DataTimer;
+
     private const float Gravity = 9.8f;
 
     private const float TO_GRAD = 180 / MathF.PI;
@@ -32,16 +32,15 @@ namespace DroneSimulator
 
     public static List<Drone> AllDrones = new List<Drone>();
     private static Thread? DroneThread = null;
-    private uint StepTime = 0;
+    public static long Timing = 0;
-
+    public static long Lag = 0;
-    public static Semaphore StepSemaphore = new Semaphore(0, 10);
+    public static long Freq = 1000;
+    public static bool Boost = false;
 
     private uint Timer;
 
-    public static bool TimeLimit = false;
-
     private Vector2 MoveOF = Vector2.Zero;
-    private bool ReadyOF = false;
+    private uint CountOF = 0;
 
     public struct Physics
     {
@@ -50,19 +49,12 @@ namespace DroneSimulator
       static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
     }
 
-    public struct Propeller
-    {
-      static public float Diameter; // Диаметр лопостей
-      static public float MaxRotate; // Максимальнные обороты в секунду
-    }
-
     private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
     private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
     private RealMode.Position RealPos = new RealMode.Position();
     private RealMode.Barometer RealBar = new RealMode.Barometer();
     private RealMode.Range RealRange = new RealMode.Range();
     private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
-    private RealMode.Magnetometer RealMag = new RealMode.Magnetometer();
 
     public static byte[] getBytes(object data)
     {
@@ -147,35 +139,42 @@ namespace DroneSimulator
 
     private static void ThreadFunction()
     {
+      long last = Stopwatch.GetTimestamp();
+      long prev = Stopwatch.GetTimestamp();
+
       while (DroneThread != null)
       {
-        if (StepSemaphore.WaitOne(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)));
-              if (drone.StepTime > 0)
-              {
-                uint prev = drone.Timer;
-                uint next = prev + drone.StepTime;
-
-                drone.Action(next);
-                drone.StepTime = 0;
-                drone.SendStep(next, prev);
-                break;
-              }
-            }
-          }
-        }
       }
     }
 
-    public Drone(int id, Socket? client)
+    public Drone(int id)
     {
       ID = id;
-      Timer = 0;
+      Active = false;
-      Client = client;
+      PosXYZ = Vector3.Zero;
+      SpdXYZ = Vector3.Zero;
+      AccXYZ = Vector3.Zero;
+      Quat = Quaternion.Identity;
     }
 
     public int Create()
@@ -214,17 +213,6 @@ namespace DroneSimulator
       Quat = Quaternion.Normalize(map);
     }
 
-    Vector2 RotateToZ(Vector2 vec, bool Rev = false)
-    {
-      Quaternion v = new Quaternion(vec.X, vec.Y, 0, 0);
-      Quaternion q = new Quaternion(0, 0, Rev ? -Quat.Z : Quat.Z, Quat.W);
-      q = Quaternion.Normalize(q);
-
-      q = (v * q) * q;
-
-      return new Vector2(q.X, q.Y);
-    }
-
     public Vector4 GetOrientation()
     {
       Quaternion grav = new Quaternion(0, 0, 1, 0);
@@ -237,14 +225,11 @@ namespace DroneSimulator
       return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
     }
 
-    int TestGyr = 0;
-    int TestDir = 1;
-
     public void Action(uint tick)
     {
       uint period = tick - Timer;
 
-      if (period == 0) return;
+      if (period <= 0) return;
 
       float time = period / 1000.0f;
       Timer = tick;
@@ -340,7 +325,7 @@ namespace DroneSimulator
       }*/
       else
       {
-        if (ori.W < 0) // это обман
+        if (ori.W < 0)
         {
           //Active = false; // Перевернулся вверх ногами
         }
@@ -364,23 +349,16 @@ namespace DroneSimulator
       RealBar.Update(PosXYZ.Z, tick);
       RealPos.Update(PosXYZ, tick);
 
-      Vector2 xy = new Vector2(SpdXYZ.X * TO_GRAD / range, SpdXYZ.Y * TO_GRAD / range);
+      bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
-      xy = RotateToZ(xy, true);
 
-      Vector2 of_xy;
+      if(of) lock (this)
-      if (range > 0.1) of_xy = new Vector2(xy.X - Gyr.Y, xy.Y + Gyr.X);
-      else of_xy = Vector2.Zero;
-
-      bool of = RealOF.Update(of_xy, LaserRange, tick);
-
-      RealMag.Update(Quat, tick);
-
-      lock (this)
       {
-        MoveOF += RealOF.result * time;
+        MoveOF += RealOF.result;
-
+        CountOF += 1;
-        if (of) ReadyOF = true;
       }
+      
+
+      DataTimer = tick;
     }
 
     private float Range(float pow)
@@ -395,13 +373,6 @@ namespace DroneSimulator
     {
       ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);
 
-      float coef = Area.Wind.Density * MathF.Pow(Propeller.Diameter, 4);
-
-      //ul = MathF.Pow(ul * Propeller.MaxRotate, 2) * coef; // я хз как делать
-      //ur = MathF.Pow(ur * Propeller.MaxRotate, 2) * coef;
-      //dl = MathF.Pow(dl * Propeller.MaxRotate, 2) * coef;
-      //dr = MathF.Pow(dr * Propeller.MaxRotate, 2) * coef;
-
       Power = (ul + ur + dl + dr) / 4;
 
       AccPRY.Y = ((ul + dl) - (ur + dr));
@@ -409,32 +380,6 @@ namespace DroneSimulator
       AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
     }
 
-    private void SendStep(uint time, uint prev)
-    {
-      DroneData.Step step = new DroneData.Step();
-
-      step.Head.Size = Marshal.SizeOf(typeof(DroneData.Step));
-      step.Head.Mode = DroneData.DataMode.Response;
-      step.Head.Type = DroneData.DataType.Step;
-      step.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
-
-      step.StepTime = time;
-      step.PrevTime = prev;
-
-      byte[] bytes = getBytes(step);
-
-      try { Client?.Send(bytes); } catch { }
-    }
-
-    private void RecvStep(byte[] data)
-    {
-      DroneData.Step step = (DroneData.Step)fromBytes(data, typeof(DroneData.Step));
-      
-      StepTime = step.StepTime;
-
-      StepSemaphore.Release();
-    }
-
     private void RecvDataMotor4(byte[] data)
     {
       DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
@@ -449,7 +394,7 @@ namespace DroneSimulator
       acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
       acc.Head.Mode = DroneData.DataMode.Response;
       acc.Head.Type = DroneData.DataType.DataAcc;
-      acc.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      acc.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
       acc.Time = RealAcc.timer;
@@ -464,7 +409,7 @@ namespace DroneSimulator
       gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
       gyr.Head.Mode = DroneData.DataMode.Response;
       gyr.Head.Type = DroneData.DataType.DataGyr;
-      gyr.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      gyr.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
       gyr.Time = RealGyr.timer;
@@ -479,10 +424,10 @@ namespace DroneSimulator
       mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
       mag.Head.Mode = DroneData.DataMode.Response;
       mag.Head.Type = DroneData.DataType.DataMag;
-      mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      mag.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
-      mag.Mag.X = RealMag.result.X; mag.Mag.Y = RealMag.result.Y; mag.Mag.Z = RealMag.result.Z;
+      mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
-      mag.Time = RealMag.timer;
+      mag.Time = DataTimer;
 
       return getBytes(mag);
     }
@@ -494,7 +439,7 @@ namespace DroneSimulator
       range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
       range.Head.Mode = DroneData.DataMode.Response;
       range.Head.Type = DroneData.DataType.DataRange;
-      range.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       range.LiDAR = RealRange.result;
       range.Time = RealRange.timer;
@@ -509,7 +454,7 @@ namespace DroneSimulator
       local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
       local.Head.Mode = DroneData.DataMode.Response;
       local.Head.Type = DroneData.DataType.DataLocal;
-      local.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      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;
@@ -524,7 +469,7 @@ namespace DroneSimulator
       bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
       bar.Head.Mode = DroneData.DataMode.Response;
       bar.Head.Type = DroneData.DataType.DataBar;
-      bar.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      bar.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       bar.Pressure = RealBar.result;
       bar.Time = RealBar.timer;
@@ -539,25 +484,16 @@ namespace DroneSimulator
       of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
       of.Head.Mode = DroneData.DataMode.Response;
       of.Head.Type = DroneData.DataType.DataOF;
-      of.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       lock (this)
       {
-        if (ReadyOF)
+        of.X = MoveOF.X / CountOF;
-        {
+        of.Y = MoveOF.Y / CountOF;
-          of.X = MoveOF.X;
-          of.Y = MoveOF.Y;
-          MoveOF = Vector2.Zero;
-        }
-        else
-        {
-          of.X = 0;
-          of.Y = 0;
-        }
-
         of.Time = RealOF.timer;
 
-        ReadyOF = false;
+        MoveOF = Vector2.Zero;
+        CountOF = 0;
       }
 
       return getBytes(of);
@@ -570,7 +506,7 @@ namespace DroneSimulator
       gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
       gps.Head.Mode = DroneData.DataMode.Response;
       gps.Head.Type = DroneData.DataType.DataGPS;
-      gps.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      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;
@@ -608,7 +544,7 @@ namespace DroneSimulator
       quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
       quat.Head.Mode = DroneData.DataMode.Response;
       quat.Head.Type = DroneData.DataType.DataQuat;
-      quat.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      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;
 
@@ -621,8 +557,8 @@ namespace DroneSimulator
 
       head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
       head.Mode = DroneData.DataMode.Response;
-      head.Type = DroneData.DataType.Ping;
+      head.Type = DroneData.DataType.None;
-      head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
+      head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       return getBytes(head);
     }
@@ -633,8 +569,6 @@ namespace DroneSimulator
 
       switch (head.Type)
       {
-        case DroneData.DataType.Step: if (head.Mode == DroneData.DataMode.Request) RecvStep(body); return zero;
-
         case DroneData.DataType.DataAcc:
           {
             if (head.Mode == DroneData.DataMode.Request)
@@ -669,7 +603,7 @@ namespace DroneSimulator
 
         case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
 
-        case DroneData.DataType.Ping: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
+        case DroneData.DataType.None: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       return zero;
@@ -678,7 +612,7 @@ namespace DroneSimulator
     private const int DroneStreamCount = 512;
     private byte[] DroneStreamData = new byte[DroneStreamCount];
     private int DroneStreamIndex = 0;
-    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.Ping };
+    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
 
     public List<byte[]?>? DataStream(byte[]? data, int size)
     {
@@ -699,8 +633,6 @@ namespace DroneSimulator
           DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
         }
 
-        if (DroneStreamHead.Size == 0) return null; // Поток сломан (конец)
-
         if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный
 
         byte[] body = new byte[DroneStreamHead.Size];

DroneSimulator/DroneData.cs

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

DroneSimulator/FormMain.cs

@@ -44,7 +44,7 @@ namespace DroneSimulator
 
       if (data.Connect)
       {
-        Drone drone = new Drone(data.ID, data.Client);
+        Drone drone = new Drone(data.ID);
         drone.Create();
 
         screen2D.CreateDrone(Color.Red, data.ID);
@@ -115,9 +115,6 @@ namespace DroneSimulator
           {
             button_Client_Start.Text = "Stop";
             button_Client_Start.BackColor = Color.LimeGreen;
-            numericUpDown_Clients_Limit.Enabled = false;
-            numericUpDown_Clients_Port.Enabled = false;
-            checkBox_Lockstep_Limit.Enabled = false;
             break;
           }
         case NetServerClients.ServerState.Stop:
@@ -125,9 +122,6 @@ namespace DroneSimulator
             label_Clients_Num.Text = "0";
             button_Client_Start.Text = "Start";
             button_Client_Start.BackColor = Color.Transparent;
-            numericUpDown_Clients_Limit.Enabled = true;
-            numericUpDown_Clients_Port.Enabled = true;
-            checkBox_Lockstep_Limit.Enabled = true;
             break;
           }
       }
@@ -157,9 +151,10 @@ namespace DroneSimulator
 
     private void timer_Test_Tick(object sender, EventArgs e)
     {
-      DateTime time = DateTime.Now;
+      DateTime test = DateTime.Now;
+
+      int tim = test.Second + test.Minute * 100 + test.Hour * 10000;
 
-      label_Lockstep_Time.Text = time.Hour.ToString("D2") + ":" + time.Minute.ToString("D2") + ":" + time.Second.ToString("D2") + "." + time.Millisecond.ToString("D3");
 
       if (screen2D == null) return;
 
@@ -171,7 +166,7 @@ namespace DroneSimulator
         {
           foreach (Drone d in Drone.AllDrones)
           {
-            screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation(), d.Quat);
+            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();
 
@@ -181,6 +176,9 @@ namespace DroneSimulator
       }
       catch { }
 
+      label_Timing.Text = Drone.Timing.ToString() + " Hz";
+      label_Timing_Lag.Text = Drone.Lag.ToString();
+
       screen2D.DrawScene();
     }
 
@@ -369,15 +367,10 @@ namespace DroneSimulator
       Drone.StopThread();
     }
 
-    private void checkBox_Lockstep_Limit_CheckedChanged(object sender, EventArgs e)
+    private void numericUpDown_Timing_Freq_ValueChanged(object sender, EventArgs e)
     {
-      Drone.TimeLimit = checkBox_Lockstep_Limit.Checked;
+      Drone.Freq = (long)numericUpDown_Timing_Freq.Value;
-    }
+      Drone.Boost = checkBox_Freq_Boost.Checked;
-
-    private void numericUpDown_Propeller_ValueChanged(object sender, EventArgs e)
-    {
-      Drone.Propeller.Diameter = (float)numericUpDown_Propeller_Diameter.Value;
-      Drone.Propeller.MaxRotate = ((float)numericUpDown_Propeller_Rotation.Value) / 60;
     }
   }
 }

DroneSimulator/RealMode.cs

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

DroneSimulator/Screen2D.cs

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