Update Drone.cs

DroneClient/BaseCommHandler.cs

@@ -1,478 +0,0 @@
-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,28 +8,4 @@
     <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.cs

@@ -3,278 +3,170 @@ 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
+  public partial class Form_Main : Form
+  {
+    private NetClient netClient = new NetClient();
+
+    public Form_Main()
     {
-        private NetClient netClient = new NetClient();
+      InitializeComponent();
-        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)
-        {
-            ConnectData data = (ConnectData)o;
-
-            if (!data.Connect)
-            {
-                try
-                {
-                    Invoke((MethodInvoker)delegate
-                    {
-                        button_Connect.Text = "Connect";
-                        button_Connect.BackColor = Color.Transparent;
-                        MessageBox.Show("Connection closed");
-                    });
-                }
-                catch { }
-
-                return;
-            }
-        }
-
-        Drone dataDrone = new Drone();
-
-        private void ReceiveCallback(object o)
-        {
-            ReceiveData data = (ReceiveData)o;
-
-            List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
-
-            if (send == null) return;
-            try
-            {
-                foreach (byte[]? b in send)
-                {
-                    if (b != null) data.Server?.Send(b);
-                }
-            }
-            catch { }
-        }
-
-        private void button_Connect_Click(object sender, EventArgs e)
-        {
-            var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
-
-            switch (done)
-            {
-                case NetClient.ClientState.Error:
-                    {
-                        MessageBox.Show("Error connecting to server");
-                        break;
-                    }
-                case NetClient.ClientState.Connected:
-                    {
-                        button_Connect.Text = "Disconnect";
-                        button_Connect.BackColor = Color.LimeGreen;
-                        break;
-                    }
-                case NetClient.ClientState.Stop:
-                    {
-                        button_Connect.Text = "Connect";
-                        button_Connect.BackColor = Color.Transparent;
-                        break;
-                    }
-            }
-
-            if (done != NetClient.ClientState.Connected) return;
-
-
-            copyDataTimer.Interval = 10;
-            copyDataTimer.Tick += CopyDataTimer_Tick;
-            tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
-            var t = Task.Run(() => tele_server.Open());
-            tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
-            tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
-            MonitoringTimer.Interval = 25;
-            MonitoringTimer.Tick += MonitoringTimer_Tick;
-            copyDataTimer.Start();
-            button1.BackColor = Color.LimeGreen;
-        }
-
-        private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
-        {
-            netClient?.Close();
-            netClient = null;
-        }
-
-        private void timer_Test_Tick(object sender, EventArgs e)
-        {
-            label_Acc_X.Text = dataDrone.AccX.ToString();
-            label_Acc_Y.Text = dataDrone.AccY.ToString();
-            label_Acc_Z.Text = dataDrone.AccZ.ToString();
-
-            label_time_acc.Text = dataDrone.TimeAcc.ToString();
-
-            label_Gyr_X.Text = dataDrone.GyrX.ToString();
-            label_Gyr_Y.Text = dataDrone.GyrY.ToString();
-            label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
-
-            label_time_gyr.Text = dataDrone.TimeGyr.ToString();
-
-            float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
-            Array.Copy(imu, telemetry.imu, 6);
-
-            label_Pos_X.Text = dataDrone.PosX.ToString();
-            label_Pos_Y.Text = dataDrone.PosY.ToString();
-            label_Pos_L.Text = dataDrone.LaserRange.ToString();
-
-            label_time_range.Text = dataDrone.TimeRange.ToString();
-
-            float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange };
-            Array.Copy(pos, telemetry.pos, 3);
-
-            label_OF_X.Text = dataDrone.OF.X.ToString();
-            label_OF_Y.Text = dataDrone.OF.Y.ToString();
-
-            label_time_of.Text = dataDrone.TimeRange.ToString();
-
-            float[] of = { dataDrone.OF.X, dataDrone.OF.Y };
-            Array.Copy(of, telemetry.of, 2);
-
-            if (monitoring.isMonitor == 1) MonitoringTimer.Start();
-            else MonitoringTimer.Stop();
-
-            netClient.SendData(dataDrone.SendReqest());
-        }
-
-        private void trackBar_Power_Scroll(object sender, EventArgs e)
-        {
-            float pow = (float)trackBar_Power.Value / 100;
-
-            label_Pow.Text = pow.ToString();
-
-            dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
-        }
-
-        private void button_UU_MouseDown(object sender, MouseEventArgs e)
-        {
-            float pow = ((float)trackBar_Value.Value) / 10.0f;
-
-            if (sender == button_UU)
-            {
-                dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
-                dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
-            }
-            if (sender == button_DD)
-            {
-                dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
-                dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
-            }
-            if (sender == button_LL)
-            {
-                dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
-                dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
-            }
-            if (sender == button_RR)
-            {
-                dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
-                dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
-            }
-
-            if (sender == button_ML)
-            {
-                dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
-                dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
-            }
-
-            if (sender == button_MR)
-            {
-                dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
-                dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
-            }
-        }
-
-        private void button_UU_MouseUp(object sender, MouseEventArgs e)
-        {
-            trackBar_Power_Scroll(null, null);
-        }
-
-        private void button1_Click(object sender, EventArgs e)
-        {
-
-            copyDataTimer.Interval = 10;
-            copyDataTimer.Tick += CopyDataTimer_Tick;
-            tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
-            var t = Task.Run(() => tele_server.Open());
-            tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
-            tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
-            MonitoringTimer.Interval = 25;
-            MonitoringTimer.Tick += MonitoringTimer_Tick;
-            copyDataTimer.Start();
-            button1.BackColor = Color.LimeGreen;
-        }
     }
+
+    private void ConnectionCallback(object o)
+    {
+      ConnectData data = (ConnectData)o;
+
+      if (!data.Connect)
+      {
+        try
+        {
+          Invoke((MethodInvoker)delegate
+          {
+            button_Connect.Text = "Connect";
+            button_Connect.BackColor = Color.Transparent;
+            MessageBox.Show("Connection closed");
+          });
+        }
+        catch { }
+
+        return;
+      }
+    }
+
+    Drone dataDrone = new Drone();
+
+    private void ReceiveCallback(object o)
+    {
+      ReceiveData data = (ReceiveData)o;
+
+      List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
+
+      if (send == null) return;
+      try
+      {
+        foreach (byte[]? b in send)
+        {
+          if (b != null) data.Server?.Send(b);
+        }
+      }
+      catch { }
+    }
+
+    private void button_Connect_Click(object sender, EventArgs e)
+    {
+      var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
+
+      switch (done)
+      {
+        case NetClient.ClientState.Error:
+          {
+            MessageBox.Show("Error connecting to server");
+            break;
+          }
+        case NetClient.ClientState.Connected:
+          {
+            button_Connect.Text = "Disconnect";
+            button_Connect.BackColor = Color.LimeGreen;
+            break;
+          }
+        case NetClient.ClientState.Stop:
+          {
+            button_Connect.Text = "Connect";
+            button_Connect.BackColor = Color.Transparent;
+            break;
+          }
+      }
+
+      if (done != NetClient.ClientState.Connected) return;
+    }
+
+    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+    {
+      netClient?.Close();
+      netClient = null;
+    }
+
+    private void timer_Test_Tick(object sender, EventArgs e)
+    {
+      label_Acc_X.Text = dataDrone.AccX.ToString();
+      label_Acc_Y.Text = dataDrone.AccY.ToString();
+      label_Acc_Z.Text = dataDrone.AccZ.ToString();
+
+      label_time_acc.Text = dataDrone.TimeAcc.ToString();
+
+      label_Gyr_X.Text = dataDrone.GyrX.ToString();
+      label_Gyr_Y.Text = dataDrone.GyrY.ToString();
+      label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
+
+      label_time_gyr.Text = dataDrone.TimeGyr.ToString();
+
+      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();
+
+      label_OF_X.Text = dataDrone.OF.X.ToString();
+      label_OF_Y.Text = dataDrone.OF.Y.ToString();
+
+      label_time_of.Text = dataDrone.TimeRange.ToString();
+
+      netClient.SendData(dataDrone.SendReqest());
+    }
+
+    private void trackBar_Power_Scroll(object sender, EventArgs e)
+    {
+      float pow = (float)trackBar_Power.Value / 100;
+
+      label_Pow.Text = pow.ToString();
+
+      dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
+    }
+
+    private void button_UU_MouseDown(object sender, MouseEventArgs e)
+    {
+      float pow = ((float)trackBar_Value.Value) / 10.0f;
+
+      if (sender == button_UU)
+      {
+        dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
+        dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
+      }
+      if (sender == button_DD)
+      {
+        dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
+        dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
+      }
+      if (sender == button_LL)
+      {
+        dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
+        dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
+      }
+      if (sender == button_RR)
+      {
+        dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
+        dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
+      }
+
+      if (sender == button_ML)
+      {
+        dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
+        dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
+      }
+
+      if (sender == button_MR)
+      {
+        dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
+        dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
+      }
+    }
+
+    private void button_UU_MouseUp(object sender, MouseEventArgs e)
+    {
+      trackBar_Power_Scroll(null, null);
+    }
+  }
 }

DroneClient/Models/MonitorContainer.cs

@@ -1,126 +0,0 @@
-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

@@ -1,340 +0,0 @@
-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

@@ -1,63 +0,0 @@
-//------------------------------------------------------------------------------
-// <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

@@ -1,120 +0,0 @@
-<?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

@@ -1,258 +0,0 @@
-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,4 +1,6 @@
-using System.Numerics;
+using System.Collections.Concurrent;
+using System.Diagnostics;
+using System.Numerics;
 using System.Runtime.InteropServices;
 using static System.Net.Mime.MediaTypeNames;
 
@@ -28,10 +30,17 @@ namespace DroneSimulator
     private const float TO_GRAD = 180 / MathF.PI;
     private const float TO_RADI = MathF.PI / 180;
 
-    private Thread DroneThread;
+    public static List<Drone> AllDrones = new List<Drone>();
-    private int Timer;
+    private static Thread? DroneThread = null;
+    public static long Timing = 0;
+    public static long Lag = 0;
+    public static long Freq = 1000;
+    public static bool Boost = false;
+
+    private uint Timer;
 
     private Vector2 MoveOF = Vector2.Zero;
+    private uint CountOF = 0;
 
     public struct Physics
     {
@@ -112,12 +121,49 @@ namespace DroneSimulator
       return drone;
     }
 
-    private void ThreadFunction()
+    public static void StartThread()
     {
+      if (DroneThread != null) return;
+
+      DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
+      DroneThread.Priority = ThreadPriority.Highest;
+      DroneThread.Start();
+    }
+
+    public static void StopThread()
+    {
+      if (DroneThread == null) return;
+
+      DroneThread = null;
+    }
+
+    private static void ThreadFunction()
+    {
+      long last = Stopwatch.GetTimestamp();
+      long prev = Stopwatch.GetTimestamp();
+
       while (DroneThread != null)
       {
-        Action(Environment.TickCount);
+        if(!Boost) Thread.Yield();
-        Thread.Sleep(1);
+
+        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)));
       }
     }
 
@@ -129,10 +175,6 @@ namespace DroneSimulator
       SpdXYZ = Vector3.Zero;
       AccXYZ = Vector3.Zero;
       Quat = Quaternion.Identity;
-
-      DroneThread = new Thread(new ThreadStart(ThreadFunction));
-      Timer = Environment.TickCount;
-      DroneThread.Start();
     }
 
     public int Create()
@@ -142,11 +184,6 @@ namespace DroneSimulator
       return ID;
     }
 
-    public void Close()
-    {
-      DroneThread = null;
-    }
-
     private float GetAngle(float a1, float a2, float az)
     {
       if (a2 == 0.0f && az == 0.0f)
@@ -188,9 +225,13 @@ namespace DroneSimulator
       return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
     }
 
-    public void Action(int tick)
+    public void Action(uint tick)
     {
-      float time = (tick - Timer) / 1000.0f;
+      uint period = tick - Timer;
+
+      if (period <= 0) return;
+
+      float time = period / 1000.0f;
       Timer = tick;
 
       if (!Active) return;
@@ -202,8 +243,6 @@ namespace DroneSimulator
         flow += flow * 0.1f; // Воздушная подушка
       }
 
-      
-
       float wind_x = 0, wind_y = 0, wind_z = 0;
       float wind_p = 0, wind_r = 0, wind_w = 0;
 
@@ -241,39 +280,49 @@ namespace DroneSimulator
       if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
       if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
 
-      if (PosXYZ.Z < 0)
+      /*if (PosXYZ.Z < 0)
       {
         SpdPRY = Vector3.Zero;
         SpdXYZ.X = 0;
         SpdXYZ.Y = 0;
         Quat = Quaternion.Identity;
       }
-      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
+      else */
+        Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
 
       Vector4 ori = GetOrientation();
 
       Orientation = ori;
+      float range = 0;
 
-      if (PosXYZ.Z < 0)
+      if (PosXYZ.Z <= 0)
+      {
+        PosXYZ.Z = 0;
+        SpdXYZ.Z = 0;
+        LaserRange = 0;
+        Acc = new Vector3(0, 0, 1);
+      }
+
+      /*if (PosXYZ.Z < 0)
       {
         PosXYZ.Z = 0;
 
-        /*if (SpdXYZ.Z < -5)
+        //if (SpdXYZ.Z < -5)
-        {
+        //{
-          Active = false; // Сильно ударился о землю
+        //  Active = false; // Сильно ударился о землю
-        }*/
+        //}
 
-        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
+        //if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
-        {
+        //{
-          Active = false; // Повредил винты при посадке
+        //  Active = false; // Повредил винты при посадке
-        }*/
+        //}
 
         SpdXYZ.Z = 0;
 
         Acc = new Vector3(0, 0, 1);
         Gyr = Vector3.Zero;
         LaserRange = 0;
-      }
+      }*/
       else
       {
         if (ori.W < 0)
@@ -288,21 +337,28 @@ namespace DroneSimulator
 
         float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
 
-        if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
+        range = PosXYZ.Z / MathF.Cos(tilt);
+
+        if (tilt < 90 && ori.W > 0) LaserRange = range;
         else LaserRange = float.MaxValue;
       }
 
-      MoveOF.X += SpdXYZ.X - Gyr.Y;
+      RealAcc.Update(Acc, tick);
-      MoveOF.Y += SpdXYZ.Y + Gyr.X;
+      RealGyr.Update(Gyr, tick);
+      RealRange.Update(LaserRange, tick);
+      RealBar.Update(PosXYZ.Z, tick);
+      RealPos.Update(PosXYZ, tick);
 
-      RealAcc.Update(Acc, (uint)tick);
+      bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
-      RealGyr.Update(Gyr, (uint)tick);
-      RealRange.Update(LaserRange, (uint)tick);
-      RealBar.Update(PosXYZ.Z * 11, (uint)tick);
-      RealPos.Update(PosXYZ, (uint)tick);
-      RealOF.Update(MoveOF, LaserRange, (uint)tick);
 
-      DataTimer = (uint)tick;
+      if(of) lock (this)
+      {
+        MoveOF += RealOF.result;
+        CountOF += 1;
+      }
+      
+
+      DataTimer = tick;
     }
 
     private float Range(float pow)
@@ -338,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)Environment.TickCount;
+      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;
@@ -353,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)Environment.TickCount;
+      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;
@@ -368,7 +424,7 @@ namespace DroneSimulator
       mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
       mag.Head.Mode = DroneData.DataMode.Response;
       mag.Head.Type = DroneData.DataType.DataMag;
-      mag.Head.Time = (uint)Environment.TickCount;
+      mag.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
       mag.Time = DataTimer;
@@ -383,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)Environment.TickCount;
+      range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       range.LiDAR = RealRange.result;
       range.Time = RealRange.timer;
@@ -398,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)Environment.TickCount;
+      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;
@@ -413,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)Environment.TickCount;
+      bar.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       bar.Pressure = RealBar.result;
       bar.Time = RealBar.timer;
@@ -428,13 +484,17 @@ namespace DroneSimulator
       of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
       of.Head.Mode = DroneData.DataMode.Response;
       of.Head.Type = DroneData.DataType.DataOF;
-      of.Head.Time = (uint)Environment.TickCount;
+      of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
-      of.X = RealOF.result.X;
+      lock (this)
-      of.Y = RealOF.result.Y;
+      {
-      of.Time = RealBar.timer;
+        of.X = MoveOF.X / CountOF;
+        of.Y = MoveOF.Y / CountOF;
+        of.Time = RealOF.timer;
 
-      MoveOF = Vector2.Zero;
+        MoveOF = Vector2.Zero;
+        CountOF = 0;
+      }
 
       return getBytes(of);
     }
@@ -446,10 +506,10 @@ namespace DroneSimulator
       gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
       gps.Head.Mode = DroneData.DataMode.Response;
       gps.Head.Type = DroneData.DataType.DataGPS;
-      gps.Head.Time = (uint)Environment.TickCount;
+      gps.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       GPS.Point p = new GPS.Point();
-      p.x = PosXYZ.Y; p.y= PosXYZ.X;
+      p.x = RealPos.result.Y; p.y= RealPos.result.X;
 
       GPS.GlobalCoords g = new GPS.GlobalCoords();
       g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
@@ -459,7 +519,7 @@ namespace DroneSimulator
       gps.Lat = g.latitude; gps.Lon = g.longitude;
 
       gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
-      gps.Alt = GPS.Home.Alt + PosXYZ.Z;
+      gps.Alt = GPS.Home.Alt + RealPos.result.Z;
 
       DateTime tim = DateTime.Now;
       gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
@@ -472,6 +532,8 @@ namespace DroneSimulator
       gps.Vdop = GPS.State.Vdop;
       gps.Pdop = GPS.State.Pdop;
 
+      gps.Time = RealPos.timer;
+
       return getBytes(gps);
     }
 
@@ -482,13 +544,25 @@ namespace DroneSimulator
       quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
       quat.Head.Mode = DroneData.DataMode.Response;
       quat.Head.Type = DroneData.DataType.DataQuat;
-      quat.Head.Time = (uint)Environment.TickCount;
+      quat.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
 
       return getBytes(quat);
     }
 
+    private byte[] SendPingPong()
+    {
+      DroneData.DataHead head = new DroneData.DataHead();
+
+      head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
+      head.Mode = DroneData.DataMode.Response;
+      head.Type = DroneData.DataType.None;
+      head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      return getBytes(head);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
       byte[] zero = Array.Empty<byte>();
@@ -528,6 +602,8 @@ namespace DroneSimulator
         case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero;
 
         case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
+
+        case DroneData.DataType.None: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       return zero;

DroneSimulator/FormMain.Designer.cs

@@ -34,6 +34,13 @@
       pictureBox_2D = new PictureBox();
       tabControl_Menu = new TabControl();
       tabPage_Main = new TabPage();
+      groupBox6 = new GroupBox();
+      checkBox_Freq_Boost = new CheckBox();
+      label84 = new Label();
+      numericUpDown_Timing_Freq = new NumericUpDown();
+      label83 = new Label();
+      label_Timing = new Label();
+      label79 = new Label();
       groupBox_Visual = new GroupBox();
       numericUpDown_Visual_Limit = new NumericUpDown();
       label1 = new Label();
@@ -71,14 +78,9 @@
       checkBox_Model_OF_Real = new CheckBox();
       label40 = new Label();
       numericUpDown_OF_Lens = new NumericUpDown();
-      label39 = new Label();
-      numericUpDown_OF_Wait = new NumericUpDown();
       label53 = new Label();
       numericUpDown_OF_Laten = new NumericUpDown();
       label54 = new Label();
-      numericUpDown_OF_Error = new NumericUpDown();
-      label38 = new Label();
-      label37 = new Label();
       numericUpDown_OF_Len = new NumericUpDown();
       checkBox_OF_Enable = new CheckBox();
       label17 = new Label();
@@ -217,10 +219,14 @@
       comboBox_Drone_Rotor = new ComboBox();
       comboBox_Drone = new ComboBox();
       timer_Test = new System.Windows.Forms.Timer(components);
+      label37 = new Label();
+      label_Timing_Lag = new Label();
       groupBox_Screen.SuspendLayout();
       ((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
       tabControl_Menu.SuspendLayout();
       tabPage_Main.SuspendLayout();
+      groupBox6.SuspendLayout();
+      ((System.ComponentModel.ISupportInitialize)numericUpDown_Timing_Freq).BeginInit();
       groupBox_Visual.SuspendLayout();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).BeginInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Port).BeginInit();
@@ -236,9 +242,7 @@
       ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).BeginInit();
       groupBox_OF.SuspendLayout();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).BeginInit();
-      ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).BeginInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).BeginInit();
-      ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).BeginInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).BeginInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit();
@@ -303,7 +307,7 @@
       groupBox_Screen.Dock = DockStyle.Fill;
       groupBox_Screen.Location = new Point(218, 0);
       groupBox_Screen.Name = "groupBox_Screen";
-      groupBox_Screen.Size = new Size(466, 816);
+      groupBox_Screen.Size = new Size(466, 781);
       groupBox_Screen.TabIndex = 1;
       groupBox_Screen.TabStop = false;
       // 
@@ -313,7 +317,7 @@
       pictureBox_2D.Dock = DockStyle.Fill;
       pictureBox_2D.Location = new Point(3, 19);
       pictureBox_2D.Name = "pictureBox_2D";
-      pictureBox_2D.Size = new Size(460, 794);
+      pictureBox_2D.Size = new Size(460, 759);
       pictureBox_2D.SizeMode = PictureBoxSizeMode.Zoom;
       pictureBox_2D.TabIndex = 0;
       pictureBox_2D.TabStop = false;
@@ -329,22 +333,99 @@
       tabControl_Menu.Location = new Point(0, 0);
       tabControl_Menu.Name = "tabControl_Menu";
       tabControl_Menu.SelectedIndex = 0;
-      tabControl_Menu.Size = new Size(218, 816);
+      tabControl_Menu.Size = new Size(218, 781);
       tabControl_Menu.TabIndex = 2;
       // 
       // tabPage_Main
       // 
+      tabPage_Main.Controls.Add(groupBox6);
       tabPage_Main.Controls.Add(groupBox_Visual);
       tabPage_Main.Controls.Add(groupBox_Clients);
       tabPage_Main.Location = new Point(4, 24);
       tabPage_Main.Name = "tabPage_Main";
       tabPage_Main.Padding = new Padding(3);
-      tabPage_Main.Size = new Size(210, 788);
+      tabPage_Main.Size = new Size(210, 753);
       tabPage_Main.TabIndex = 0;
       tabPage_Main.Tag = "#main";
       tabPage_Main.Text = "Main";
       tabPage_Main.UseVisualStyleBackColor = true;
       // 
+      // groupBox6
+      // 
+      groupBox6.Controls.Add(label_Timing_Lag);
+      groupBox6.Controls.Add(numericUpDown_Timing_Freq);
+      groupBox6.Controls.Add(label37);
+      groupBox6.Controls.Add(checkBox_Freq_Boost);
+      groupBox6.Controls.Add(label84);
+      groupBox6.Controls.Add(label83);
+      groupBox6.Controls.Add(label_Timing);
+      groupBox6.Controls.Add(label79);
+      groupBox6.Dock = DockStyle.Top;
+      groupBox6.Location = new Point(3, 174);
+      groupBox6.Name = "groupBox6";
+      groupBox6.Size = new Size(204, 75);
+      groupBox6.TabIndex = 3;
+      groupBox6.TabStop = false;
+      groupBox6.Text = "Frequency";
+      // 
+      // checkBox_Freq_Boost
+      // 
+      checkBox_Freq_Boost.AutoSize = true;
+      checkBox_Freq_Boost.Location = new Point(148, 18);
+      checkBox_Freq_Boost.Name = "checkBox_Freq_Boost";
+      checkBox_Freq_Boost.Size = new Size(56, 19);
+      checkBox_Freq_Boost.TabIndex = 11;
+      checkBox_Freq_Boost.Text = "boost";
+      checkBox_Freq_Boost.UseVisualStyleBackColor = true;
+      checkBox_Freq_Boost.CheckedChanged += numericUpDown_Timing_Freq_ValueChanged;
+      // 
+      // label84
+      // 
+      label84.AutoSize = true;
+      label84.Location = new Point(116, 43);
+      label84.Name = "label84";
+      label84.Size = new Size(21, 15);
+      label84.TabIndex = 10;
+      label84.Text = "Hz";
+      // 
+      // numericUpDown_Timing_Freq
+      // 
+      numericUpDown_Timing_Freq.Location = new Point(64, 41);
+      numericUpDown_Timing_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
+      numericUpDown_Timing_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
+      numericUpDown_Timing_Freq.Name = "numericUpDown_Timing_Freq";
+      numericUpDown_Timing_Freq.Size = new Size(51, 23);
+      numericUpDown_Timing_Freq.TabIndex = 9;
+      numericUpDown_Timing_Freq.Value = new decimal(new int[] { 1000, 0, 0, 0 });
+      numericUpDown_Timing_Freq.ValueChanged += numericUpDown_Timing_Freq_ValueChanged;
+      // 
+      // label83
+      // 
+      label83.AutoSize = true;
+      label83.Location = new Point(7, 43);
+      label83.Name = "label83";
+      label83.Size = new Size(57, 15);
+      label83.TabIndex = 2;
+      label83.Text = "Required:";
+      // 
+      // label_Timing
+      // 
+      label_Timing.AutoSize = true;
+      label_Timing.Location = new Point(70, 19);
+      label_Timing.Name = "label_Timing";
+      label_Timing.Size = new Size(13, 15);
+      label_Timing.TabIndex = 1;
+      label_Timing.Text = "0";
+      // 
+      // label79
+      // 
+      label79.AutoSize = true;
+      label79.Location = new Point(6, 19);
+      label79.Name = "label79";
+      label79.Size = new Size(58, 15);
+      label79.TabIndex = 0;
+      label79.Text = "Available:";
+      // 
       // groupBox_Visual
       // 
       groupBox_Visual.Controls.Add(numericUpDown_Visual_Limit);
@@ -528,7 +609,7 @@
       tabPage_Model.Location = new Point(4, 24);
       tabPage_Model.Name = "tabPage_Model";
       tabPage_Model.Padding = new Padding(3);
-      tabPage_Model.Size = new Size(210, 788);
+      tabPage_Model.Size = new Size(210, 753);
       tabPage_Model.TabIndex = 1;
       tabPage_Model.Tag = "#model";
       tabPage_Model.Text = "Model";
@@ -546,7 +627,7 @@
       panel_Menu_Model.Dock = DockStyle.Fill;
       panel_Menu_Model.Location = new Point(3, 3);
       panel_Menu_Model.Name = "panel_Menu_Model";
-      panel_Menu_Model.Size = new Size(204, 782);
+      panel_Menu_Model.Size = new Size(204, 747);
       panel_Menu_Model.TabIndex = 5;
       // 
       // groupBox1
@@ -566,7 +647,7 @@
       groupBox1.Controls.Add(label46);
       groupBox1.Controls.Add(label47);
       groupBox1.Dock = DockStyle.Top;
-      groupBox1.Location = new Point(0, 652);
+      groupBox1.Location = new Point(0, 623);
       groupBox1.Name = "groupBox1";
       groupBox1.Size = new Size(204, 126);
       groupBox1.TabIndex = 8;
@@ -691,7 +772,7 @@
       // numericUpDown_Range_Freq
       // 
       numericUpDown_Range_Freq.Location = new Point(70, 16);
-      numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Range_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Range_Freq.Name = "numericUpDown_Range_Freq";
       numericUpDown_Range_Freq.Size = new Size(40, 23);
@@ -723,14 +804,9 @@
       groupBox_OF.Controls.Add(checkBox_Model_OF_Real);
       groupBox_OF.Controls.Add(label40);
       groupBox_OF.Controls.Add(numericUpDown_OF_Lens);
-      groupBox_OF.Controls.Add(label39);
-      groupBox_OF.Controls.Add(numericUpDown_OF_Wait);
       groupBox_OF.Controls.Add(label53);
       groupBox_OF.Controls.Add(numericUpDown_OF_Laten);
       groupBox_OF.Controls.Add(label54);
-      groupBox_OF.Controls.Add(numericUpDown_OF_Error);
-      groupBox_OF.Controls.Add(label38);
-      groupBox_OF.Controls.Add(label37);
       groupBox_OF.Controls.Add(numericUpDown_OF_Len);
       groupBox_OF.Controls.Add(checkBox_OF_Enable);
       groupBox_OF.Controls.Add(label17);
@@ -744,7 +820,7 @@
       groupBox_OF.Dock = DockStyle.Top;
       groupBox_OF.Location = new Point(0, 489);
       groupBox_OF.Name = "groupBox_OF";
-      groupBox_OF.Size = new Size(204, 163);
+      groupBox_OF.Size = new Size(204, 134);
       groupBox_OF.TabIndex = 4;
       groupBox_OF.TabStop = false;
       groupBox_OF.Text = "Optical flow";
@@ -783,31 +859,10 @@
       numericUpDown_OF_Lens.Value = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_OF_Lens.ValueChanged += numericUpDown_OF_Update;
       // 
-      // label39
-      // 
-      label39.AutoSize = true;
-      label39.Location = new Point(168, 105);
-      label39.Name = "label39";
-      label39.Size = new Size(24, 15);
-      label39.TabIndex = 34;
-      label39.Text = "sec";
-      // 
-      // numericUpDown_OF_Wait
-      // 
-      numericUpDown_OF_Wait.DecimalPlaces = 2;
-      numericUpDown_OF_Wait.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
-      numericUpDown_OF_Wait.Location = new Point(119, 103);
-      numericUpDown_OF_Wait.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
-      numericUpDown_OF_Wait.Name = "numericUpDown_OF_Wait";
-      numericUpDown_OF_Wait.Size = new Size(47, 23);
-      numericUpDown_OF_Wait.TabIndex = 33;
-      numericUpDown_OF_Wait.Value = new decimal(new int[] { 1, 0, 0, 65536 });
-      numericUpDown_OF_Wait.ValueChanged += numericUpDown_OF_Update;
-      // 
       // label53
       // 
       label53.AutoSize = true;
-      label53.Location = new Point(113, 132);
+      label53.Location = new Point(114, 107);
       label53.Name = "label53";
       label53.Size = new Size(24, 15);
       label53.TabIndex = 32;
@@ -817,7 +872,7 @@
       // 
       numericUpDown_OF_Laten.DecimalPlaces = 2;
       numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
-      numericUpDown_OF_Laten.Location = new Point(66, 130);
+      numericUpDown_OF_Laten.Location = new Point(67, 105);
       numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten";
       numericUpDown_OF_Laten.Size = new Size(41, 23);
@@ -828,41 +883,12 @@
       // label54
       // 
       label54.AutoSize = true;
-      label54.Location = new Point(3, 132);
+      label54.Location = new Point(4, 107);
       label54.Name = "label54";
       label54.Size = new Size(55, 15);
       label54.TabIndex = 30;
       label54.Text = "Lateness:";
       // 
-      // numericUpDown_OF_Error
-      // 
-      numericUpDown_OF_Error.DecimalPlaces = 1;
-      numericUpDown_OF_Error.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
-      numericUpDown_OF_Error.Location = new Point(38, 103);
-      numericUpDown_OF_Error.Name = "numericUpDown_OF_Error";
-      numericUpDown_OF_Error.Size = new Size(47, 23);
-      numericUpDown_OF_Error.TabIndex = 24;
-      numericUpDown_OF_Error.Value = new decimal(new int[] { 1, 0, 0, 0 });
-      numericUpDown_OF_Error.ValueChanged += numericUpDown_OF_Update;
-      // 
-      // label38
-      // 
-      label38.AutoSize = true;
-      label38.Location = new Point(83, 105);
-      label38.Name = "label38";
-      label38.Size = new Size(17, 15);
-      label38.TabIndex = 25;
-      label38.Text = "%";
-      // 
-      // label37
-      // 
-      label37.AutoSize = true;
-      label37.Location = new Point(1, 105);
-      label37.Name = "label37";
-      label37.Size = new Size(35, 15);
-      label37.TabIndex = 23;
-      label37.Text = "Error:";
-      // 
       // numericUpDown_OF_Len
       // 
       numericUpDown_OF_Len.Location = new Point(68, 76);
@@ -878,7 +904,7 @@
       checkBox_OF_Enable.AutoSize = true;
       checkBox_OF_Enable.Checked = true;
       checkBox_OF_Enable.CheckState = CheckState.Checked;
-      checkBox_OF_Enable.Location = new Point(159, 134);
+      checkBox_OF_Enable.Location = new Point(160, 109);
       checkBox_OF_Enable.Name = "checkBox_OF_Enable";
       checkBox_OF_Enable.Size = new Size(39, 19);
       checkBox_OF_Enable.TabIndex = 22;
@@ -939,7 +965,7 @@
       // numericUpDown_OF_Freq
       // 
       numericUpDown_OF_Freq.Location = new Point(69, 22);
-      numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_OF_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_OF_Freq.Name = "numericUpDown_OF_Freq";
       numericUpDown_OF_Freq.Size = new Size(40, 23);
@@ -1108,7 +1134,7 @@
       // numericUpDown_Bar_Freq
       // 
       numericUpDown_Bar_Freq.Location = new Point(68, 42);
-      numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Bar_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Bar_Freq.Name = "numericUpDown_Bar_Freq";
       numericUpDown_Bar_Freq.Size = new Size(40, 23);
@@ -1232,7 +1258,7 @@
       // numericUpDown_Pos_Freq
       // 
       numericUpDown_Pos_Freq.Location = new Point(69, 17);
-      numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Pos_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Pos_Freq.Name = "numericUpDown_Pos_Freq";
       numericUpDown_Pos_Freq.Size = new Size(40, 23);
@@ -1307,7 +1333,7 @@
       // numericUpDown_Mag_Freq
       // 
       numericUpDown_Mag_Freq.Location = new Point(37, 18);
-      numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Mag_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Mag_Freq.Name = "numericUpDown_Mag_Freq";
       numericUpDown_Mag_Freq.Size = new Size(40, 23);
@@ -1499,7 +1525,7 @@
       // numericUpDown_Gyr_Freq
       // 
       numericUpDown_Gyr_Freq.Location = new Point(37, 19);
-      numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Gyr_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Gyr_Freq.Name = "numericUpDown_Gyr_Freq";
       numericUpDown_Gyr_Freq.Size = new Size(40, 23);
@@ -1658,7 +1684,7 @@
       // numericUpDown_Acc_Freq
       // 
       numericUpDown_Acc_Freq.Location = new Point(37, 16);
-      numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
+      numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
       numericUpDown_Acc_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
       numericUpDown_Acc_Freq.Name = "numericUpDown_Acc_Freq";
       numericUpDown_Acc_Freq.Size = new Size(40, 23);
@@ -1681,7 +1707,7 @@
       tabPage_Area.Controls.Add(groupBox3);
       tabPage_Area.Location = new Point(4, 24);
       tabPage_Area.Name = "tabPage_Area";
-      tabPage_Area.Size = new Size(210, 788);
+      tabPage_Area.Size = new Size(210, 753);
       tabPage_Area.TabIndex = 2;
       tabPage_Area.Tag = "#area";
       tabPage_Area.Text = "Area";
@@ -1932,7 +1958,7 @@
       tabPage_GPS.Location = new Point(4, 24);
       tabPage_GPS.Name = "tabPage_GPS";
       tabPage_GPS.Padding = new Padding(3);
-      tabPage_GPS.Size = new Size(210, 788);
+      tabPage_GPS.Size = new Size(210, 753);
       tabPage_GPS.TabIndex = 3;
       tabPage_GPS.Text = "GPS";
       tabPage_GPS.UseVisualStyleBackColor = true;
@@ -2190,7 +2216,7 @@
       tabPage_Drone.Location = new Point(4, 24);
       tabPage_Drone.Name = "tabPage_Drone";
       tabPage_Drone.Padding = new Padding(3);
-      tabPage_Drone.Size = new Size(210, 788);
+      tabPage_Drone.Size = new Size(210, 753);
       tabPage_Drone.TabIndex = 4;
       tabPage_Drone.Text = "Drone";
       tabPage_Drone.UseVisualStyleBackColor = true;
@@ -2311,7 +2337,7 @@
       groupBox_Navi.Dock = DockStyle.Right;
       groupBox_Navi.Location = new Point(684, 0);
       groupBox_Navi.Name = "groupBox_Navi";
-      groupBox_Navi.Size = new Size(200, 816);
+      groupBox_Navi.Size = new Size(200, 781);
       groupBox_Navi.TabIndex = 3;
       groupBox_Navi.TabStop = false;
       groupBox_Navi.Tag = "#navigation";
@@ -2325,7 +2351,7 @@
       panel1.Dock = DockStyle.Fill;
       panel1.Location = new Point(3, 42);
       panel1.Name = "panel1";
-      panel1.Size = new Size(194, 771);
+      panel1.Size = new Size(194, 736);
       panel1.TabIndex = 3;
       // 
       // listBox_Drones
@@ -2370,16 +2396,34 @@
       timer_Test.Interval = 10;
       timer_Test.Tick += timer_Test_Tick;
       // 
+      // label37
+      // 
+      label37.AutoSize = true;
+      label37.Location = new Point(143, 43);
+      label37.Name = "label37";
+      label37.Size = new Size(29, 15);
+      label37.TabIndex = 12;
+      label37.Text = "Lag:";
+      // 
+      // label_Timing_Lag
+      // 
+      label_Timing_Lag.AutoSize = true;
+      label_Timing_Lag.Location = new Point(168, 43);
+      label_Timing_Lag.Name = "label_Timing_Lag";
+      label_Timing_Lag.Size = new Size(13, 15);
+      label_Timing_Lag.TabIndex = 13;
+      label_Timing_Lag.Text = "0";
+      // 
       // Form_Main
       // 
       AutoScaleDimensions = new SizeF(7F, 15F);
       AutoScaleMode = AutoScaleMode.Font;
-      ClientSize = new Size(884, 816);
+      ClientSize = new Size(884, 781);
       Controls.Add(groupBox_Screen);
       Controls.Add(groupBox_Navi);
       Controls.Add(tabControl_Menu);
       Icon = (Icon)resources.GetObject("$this.Icon");
-      MinimumSize = new Size(900, 855);
+      MinimumSize = new Size(900, 820);
       Name = "Form_Main";
       Text = "Drone Simulator V1.0";
       FormClosing += Form_Main_FormClosing;
@@ -2387,6 +2431,9 @@
       ((System.ComponentModel.ISupportInitialize)pictureBox_2D).EndInit();
       tabControl_Menu.ResumeLayout(false);
       tabPage_Main.ResumeLayout(false);
+      groupBox6.ResumeLayout(false);
+      groupBox6.PerformLayout();
+      ((System.ComponentModel.ISupportInitialize)numericUpDown_Timing_Freq).EndInit();
       groupBox_Visual.ResumeLayout(false);
       groupBox_Visual.PerformLayout();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).EndInit();
@@ -2406,9 +2453,7 @@
       groupBox_OF.ResumeLayout(false);
       groupBox_OF.PerformLayout();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).EndInit();
-      ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).EndInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).EndInit();
-      ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).EndInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).EndInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit();
       ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit();
@@ -2563,9 +2608,6 @@
     private Label label34;
     private Label label36;
     private Label label35;
-    private NumericUpDown numericUpDown_OF_Error;
-    private Label label37;
-    private Label label38;
     private GroupBox groupBox1;
     private CheckBox checkBox_Range_Enable;
     private NumericUpDown numericUpDown_Range_Max;
@@ -2599,8 +2641,6 @@
     private NumericUpDown numericUpDown_Range_Laten;
     private Label label56;
     private ListBox listBox_Drones;
-    private Label label39;
-    private NumericUpDown numericUpDown_OF_Wait;
     private Label label40;
     private NumericUpDown numericUpDown_OF_Lens;
     private GroupBox groupBox3;
@@ -2666,5 +2706,14 @@
     private Label label78;
     private NumericUpDown numericUpDown_Physics_Power;
     private Label label77;
+    private GroupBox groupBox6;
+    private Label label_Timing;
+    private Label label79;
+    private Label label84;
+    private NumericUpDown numericUpDown_Timing_Freq;
+    private Label label83;
+    private CheckBox checkBox_Freq_Boost;
+    private Label label_Timing_Lag;
+    private Label label37;
   }
 }

DroneSimulator/FormMain.cs

@@ -17,8 +17,6 @@ namespace DroneSimulator
     NetServerClients netServerClient = new NetServerClients();
     NetServerVisual netServerVisual = new NetServerVisual();
 
-    List<Drone> AllDrones = new List<Drone>();
-
     public Form_Main()
     {
       InitializeComponent();
@@ -51,20 +49,24 @@ namespace DroneSimulator
 
         screen2D.CreateDrone(Color.Red, data.ID);
 
-        AllDrones.Add(drone);
+        lock (Drone.AllDrones) Drone.AllDrones.Add(drone);
       }
       else
       {
-        foreach (Drone drone in AllDrones)
+        Drone? d = null;
+
+        lock (Drone.AllDrones)
         {
-          if (drone.ID != data.ID) continue;
+          foreach (Drone drone in Drone.AllDrones)
-          drone.Close();
+          {
-
+            if (drone.ID != data.ID) continue;
-          screen2D.RemoveDrone(data.ID);
+            d = drone;
-
+            Drone.AllDrones.Remove(drone);
-          AllDrones.Remove(drone);
+            break;
-          break;
+          }
         }
+
+        if (d != null) screen2D.RemoveDrone(d.ID);
       }
     }
 
@@ -74,11 +76,14 @@ namespace DroneSimulator
 
       Drone? drone = null;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
       {
-        if (d.ID != data.ID) continue;
+        foreach (Drone d in Drone.AllDrones)
-        drone = d;
+        {
-        break;
+          if (d.ID != data.ID) continue;
+          drone = d;
+          break;
+        }
       }
 
       if (drone == null) return;
@@ -123,6 +128,8 @@ namespace DroneSimulator
 
       if (done != NetServerClients.ServerState.Start) return;
 
+      Drone.StartThread();
+
       pictureBox_2D.Image = null;
 
       screen2D = new Screen2D(DrawCallback);
@@ -155,23 +162,25 @@ namespace DroneSimulator
 
       try
       {
-        foreach (Drone d in AllDrones)
+        lock (Drone.AllDrones)
         {
-          screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+          foreach (Drone d in Drone.AllDrones)
+          {
+            screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
 
-          string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
+            string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
 
-          listBox_Drones.Items.Add(line);
+            listBox_Drones.Items.Add(line);
+          }
         }
       }
       catch { }
 
+      label_Timing.Text = Drone.Timing.ToString() + " Hz";
+      label_Timing_Lag.Text = Drone.Lag.ToString();
+
       screen2D.DrawScene();
     }
-    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
-    {
-      foreach (Drone d in AllDrones) d.Close();
-    }
 
     private void VisualConnectionCallback(object o)
     {
@@ -198,12 +207,15 @@ namespace DroneSimulator
 
       int index = 0;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
       {
-        VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
+        foreach (Drone d in Drone.AllDrones)
+        {
+          VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++);
 
-        try { data.Client.Send(Drone.getBytes(v)); }
+          try { data.Client.Send(Drone.getBytes(v)); }
-        catch { }
+          catch { }
+        }
       }
     }
 
@@ -306,11 +318,8 @@ namespace DroneSimulator
       RealMode.OpticalFlow.Lateness = (float)numericUpDown_OF_Laten.Value;
       RealMode.OpticalFlow.Enable = checkBox_OF_Enable.Checked;
 
-      RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10;
+      RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value;
       RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
-
-      RealMode.OpticalFlow.Error = (float)numericUpDown_OF_Error.Value * 10;
-      RealMode.OpticalFlow.Wait = (uint)numericUpDown_OF_Wait.Value * 1000;
     }
 
     private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
@@ -352,5 +361,16 @@ namespace DroneSimulator
       Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value;
       Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value;
     }
+
+    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+    {
+      Drone.StopThread();
+    }
+
+    private void numericUpDown_Timing_Freq_ValueChanged(object sender, EventArgs e)
+    {
+      Drone.Freq = (long)numericUpDown_Timing_Freq.Value;
+      Drone.Boost = checkBox_Freq_Boost.Checked;
+    }
   }
 }

DroneSimulator/RealMode.cs

@@ -225,7 +225,7 @@ namespace DroneSimulator
 
       public void Update(float value, uint time)
       {
-        value = Pressure - value;
+        value = Pressure - value * 12.15f;
 
         if (!Enable)
         {
@@ -273,8 +273,6 @@ namespace DroneSimulator
       public static uint Freq;
       public static float Noise;
       public static float Lateness;
-      public static float Error;
-      public static uint Wait;
       public static float Lens;
       public static bool RealSimulation;
 
@@ -290,31 +288,21 @@ namespace DroneSimulator
 
       public uint timer = 0;
       public Vector2 result;
-      public void Update(Vector2 value, float Range, uint time)
+      public bool Update(Vector2 value, float Range, uint time)
       {
+        value *= Lens;
+
         if (!Enable)
         {
           result = Vector2.NaN;
-          return;
+          return true;
         }
 
         if (!RealSimulation)
         {
           result = value;
           timer = time;
-          return;
+          return true;
-        }
-
-        value *= Lens;
-
-        if (rand.Next(0, 1000) < (Error * 10))
-        {
-          value = Vector2.Zero;
-          delay = time + Wait;
-        }
-        else if (delay > time)
-        {
-          value = Vector2.Zero;
         }
 
         if (Range > MaxHeight) value = Vector2.Zero;
@@ -344,7 +332,10 @@ namespace DroneSimulator
         {
           result = value;
           timer = time;
+          return true;
         }
+
+        return false;
       }
     }