diff --git a/DroneClient/BaseCommHandler.cs b/DroneClient/BaseCommHandler.cs new file mode 100644 index 0000000..d5bbbf3 --- /dev/null +++ b/DroneClient/BaseCommHandler.cs @@ -0,0 +1,478 @@ +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using TelemetryIO.Models; + +namespace TelemetryIO +{ + public interface iCommParams + { + + } + + internal class TCPCommParams : iCommParams + { + public string IP = ""; + public int Port = 0; + public TCPCommParams(string addr, int port) + { + this.IP = addr; + this.Port = port; + } + } + + public class SerialCommParams : iCommParams + { + public string PortName = ""; + public int BaudRate = 9600; + public SerialCommParams(string portName, int baudRate) + { + PortName = portName; + BaudRate = baudRate; + } + } + + public abstract class BaseCommHandler + { + public const int TELE_CMD_RD_ONCE = 1; + public const int TELE_CMD_RD_MON_ON = 2; + public const int TELE_CMD_RD_MON_OFF = 3; + public const int TELE_CMD_RD_MON_ADD = 4; + public const int TELE_CMD_RD_MON_REMOVE = 5; + public const int TELE_CMD_RD_MON_REMOVEALL = 6; + public const int TELE_CMD_WR = 10; + public const int TELE_CMD_MOTORS_CTRL = 100; + public const int TELE_CMD_ABORT = 999; + public const int TELE_CMD_HELLO = 9999; + public const byte CRC8_POLYNOMIAL = 0x07; + public const int CRC32_POLY = 0x04C11DB7; + public const byte ESCAPE_BEGIN = 0xBE; + public const byte ESCAPE_END = 0xED; + public const byte ESCAPE_CHAR = 0x0E; + + + private volatile bool _isReading = true; + private readonly ConcurrentQueue dataQueue = new ConcurrentQueue(); + static private List rx_buf = new List(); + 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 req_buffer = new Queue(); + + //Generate event when answer is received + public delegate void AnswerEventHandler(object sender, SerialEventArgs e); + public event AnswerEventHandler 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(object sender, MonitoringEventArgs e); + public event MonitoringEventHandler 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); + + /// + /// Обрабатывает входящий поток данных, при обнаружении ECSAPE_END байта декодирует данные, проверяет контрольную сумму и запускает обработку команды + /// + 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)); + } + + /// + /// Считает контрольную сумму CRC32 + /// + 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(int cmd, T[] load, string var_name) + { + VarAddress va = telemetry.getVarAdress(var_name); + return prepareTelegram(cmd, va.slot, load, va.offset, va.length); + } + + /// + /// Подготавливает данные для отправки, кодируя их в ESCAPE-последовательность + /// + public byte[] prepareTelegram(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; + } + + /// + /// Конвертирует массив bool/byte/int/float в массив байт. Создает новый массив. + /// + byte[] DataArrayToBytes(T[] data) + { + if (data == null) return new byte[0]; + List ret = new List(); + 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(); + } + + /// + /// Конвертирует массив байт в ESCAPE-последовательность. + /// + public byte[] BytesToEscapeSeq(byte[] data) + { + List ret = new List(); + 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(); + } + + /// + /// Конвертирует ESCAPE-последовательность в массив байт. + /// + public byte[] EscapeSeqToBytes(byte[] EscSeq) + { + List ret = new List(); + 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; + } + + /// + /// Добавляет данные в очередь ожидания отправки. + /// + public void putRequest(byte[] request) + { + //добавить в очередь или отправить(если очередь пуста) пакет в порт + lock (lock_obj_put) + { + if (waitingForResponse) + { + req_buffer.Enqueue(request); + return; + } + + sendRequest(request); + } + } + + /// + /// Толкает данные непосредственно в очередь отправки. + /// + protected void sendRequest(byte[] request) + { + //отправка пакета + //waitingForResponse = true; + if (IsOpen()) + { + sendData(request); + } + timeout.Stop(); + timeout.Start(); + } + + /// + /// Вытягивает данные из очереди ожидания непосредственно в очередь отправки. + /// + protected void sendNextRequest() + { + //вытащить из очереди пакет и отправить в порт + waitingForResponse = false; + if (req_buffer.Count > 0) + { + sendRequest(req_buffer.Dequeue()); + } + } + + /// + /// Копирует входящие данные в очередь. + /// + 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; + } + } +} diff --git a/DroneClient/DroneClient.csproj b/DroneClient/DroneClient.csproj index c27cd77..1654505 100644 --- a/DroneClient/DroneClient.csproj +++ b/DroneClient/DroneClient.csproj @@ -8,4 +8,28 @@ enable + + + True + True + Resources.resx + + + + + + ResXFileCodeGenerator + Resources.Designer.cs + + + + + + PreserveNewest + + + PreserveNewest + + + \ No newline at end of file diff --git a/DroneClient/FormMain.Designer.cs b/DroneClient/FormMain.Designer.cs index 4604a5d..9af3517 100644 --- a/DroneClient/FormMain.Designer.cs +++ b/DroneClient/FormMain.Designer.cs @@ -20,588 +20,677 @@ base.Dispose(disposing); } - #region Windows Form Designer generated code + #region Windows Form Designer generated code - /// - /// Required method for Designer support - do not modify - /// the contents of this method with the code editor. - /// - private void InitializeComponent() - { - components = new System.ComponentModel.Container(); - timer_Test = new System.Windows.Forms.Timer(components); - groupBox1 = new GroupBox(); - textBox_Server_Addr = new TextBox(); - label2 = new Label(); - button_Connect = new Button(); - numericUpDown_Server_Port = new NumericUpDown(); - label3 = new Label(); - label1 = new Label(); - groupBox2 = new GroupBox(); - label_time_acc = new Label(); - label_Acc_Z = new Label(); - label7 = new Label(); - label_Acc_Y = new Label(); - label5 = new Label(); - label_Acc_X = new Label(); - groupBox3 = new GroupBox(); - label_time_gyr = new Label(); - label_Gyr_Z = new Label(); - label9 = new Label(); - label_Gyr_Y = new Label(); - label11 = new Label(); - label_Gyr_X = new Label(); - label13 = new Label(); - groupBox4 = new GroupBox(); - label_time_range = new Label(); - label_Pos_L = new Label(); - label6 = new Label(); - label_Pos_Y = new Label(); - label10 = new Label(); - label_Pos_X = new Label(); - label14 = new Label(); - trackBar_Power = new TrackBar(); - button_LL = new Button(); - button_UU = new Button(); - button_DD = new Button(); - button_RR = new Button(); - label_Pow = new Label(); - button_ML = new Button(); - button_MR = new Button(); - groupBox5 = new GroupBox(); - label_time_of = new Label(); - label_OF_Y = new Label(); - label17 = new Label(); - label_OF_X = new Label(); - label19 = new Label(); - trackBar_Value = new TrackBar(); - label4 = new Label(); - groupBox1.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit(); - groupBox2.SuspendLayout(); - groupBox3.SuspendLayout(); - groupBox4.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit(); - groupBox5.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)trackBar_Value).BeginInit(); - SuspendLayout(); - // - // timer_Test - // - timer_Test.Enabled = true; - timer_Test.Interval = 10; - timer_Test.Tick += timer_Test_Tick; - // - // groupBox1 - // - groupBox1.Controls.Add(textBox_Server_Addr); - groupBox1.Controls.Add(label2); - groupBox1.Controls.Add(button_Connect); - groupBox1.Controls.Add(numericUpDown_Server_Port); - groupBox1.Controls.Add(label3); - groupBox1.Dock = DockStyle.Top; - groupBox1.Location = new Point(0, 0); - groupBox1.Name = "groupBox1"; - groupBox1.Size = new Size(446, 80); - groupBox1.TabIndex = 3; - groupBox1.TabStop = false; - groupBox1.Tag = ""; - groupBox1.Text = "Server"; - // - // textBox_Server_Addr - // - textBox_Server_Addr.Location = new Point(48, 16); - textBox_Server_Addr.Name = "textBox_Server_Addr"; - textBox_Server_Addr.Size = new Size(125, 23); - textBox_Server_Addr.TabIndex = 4; - textBox_Server_Addr.Text = "127.0.0.1"; - // - // label2 - // - label2.AutoSize = true; - label2.Location = new Point(6, 19); - label2.Name = "label2"; - label2.Size = new Size(36, 15); - label2.TabIndex = 3; - label2.Tag = ""; - label2.Text = "Addr:"; - // - // button_Connect - // - button_Connect.BackColor = Color.Transparent; - button_Connect.Location = new Point(112, 46); - button_Connect.Name = "button_Connect"; - button_Connect.Size = new Size(61, 23); - button_Connect.TabIndex = 2; - button_Connect.Tag = ""; - button_Connect.Text = "Connect"; - button_Connect.UseVisualStyleBackColor = false; - button_Connect.Click += button_Connect_Click; - // - // numericUpDown_Server_Port - // - numericUpDown_Server_Port.Location = new Point(44, 48); - numericUpDown_Server_Port.Maximum = new decimal(new int[] { 65000, 0, 0, 0 }); - numericUpDown_Server_Port.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); - numericUpDown_Server_Port.Name = "numericUpDown_Server_Port"; - numericUpDown_Server_Port.Size = new Size(62, 23); - numericUpDown_Server_Port.TabIndex = 1; - numericUpDown_Server_Port.Value = new decimal(new int[] { 1001, 0, 0, 0 }); - // - // label3 - // - label3.AutoSize = true; - label3.Location = new Point(6, 50); - label3.Name = "label3"; - label3.Size = new Size(32, 15); - label3.TabIndex = 0; - label3.Tag = "#clients_port"; - label3.Text = "Port:"; - // - // label1 - // - label1.AutoSize = true; - label1.Location = new Point(6, 19); - label1.Name = "label1"; - label1.Size = new Size(17, 15); - label1.TabIndex = 4; - label1.Text = "X:"; - // - // groupBox2 - // - groupBox2.Controls.Add(label_time_acc); - groupBox2.Controls.Add(label_Acc_Z); - groupBox2.Controls.Add(label7); - groupBox2.Controls.Add(label_Acc_Y); - groupBox2.Controls.Add(label5); - groupBox2.Controls.Add(label_Acc_X); - groupBox2.Controls.Add(label1); - groupBox2.Location = new Point(6, 86); - groupBox2.Name = "groupBox2"; - groupBox2.Size = new Size(100, 118); - groupBox2.TabIndex = 5; - groupBox2.TabStop = false; - groupBox2.Text = "Acc"; - // - // label_time_acc - // - label_time_acc.AutoSize = true; - label_time_acc.Location = new Point(6, 100); - label_time_acc.Name = "label_time_acc"; - label_time_acc.Size = new Size(13, 15); - label_time_acc.TabIndex = 25; - label_time_acc.Text = "0"; - // - // label_Acc_Z - // - label_Acc_Z.AutoSize = true; - label_Acc_Z.Location = new Point(19, 70); - label_Acc_Z.Name = "label_Acc_Z"; - label_Acc_Z.Size = new Size(13, 15); - label_Acc_Z.TabIndex = 9; - label_Acc_Z.Text = "0"; - // - // label7 - // - label7.AutoSize = true; - label7.Location = new Point(6, 70); - label7.Name = "label7"; - label7.Size = new Size(17, 15); - label7.TabIndex = 8; - label7.Text = "Z:"; - // - // label_Acc_Y - // - label_Acc_Y.AutoSize = true; - label_Acc_Y.Location = new Point(19, 45); - label_Acc_Y.Name = "label_Acc_Y"; - label_Acc_Y.Size = new Size(13, 15); - label_Acc_Y.TabIndex = 7; - label_Acc_Y.Text = "0"; - // - // label5 - // - label5.AutoSize = true; - label5.Location = new Point(6, 45); - label5.Name = "label5"; - label5.Size = new Size(17, 15); - label5.TabIndex = 6; - label5.Text = "Y:"; - // - // label_Acc_X - // - label_Acc_X.AutoSize = true; - label_Acc_X.Location = new Point(19, 19); - label_Acc_X.Name = "label_Acc_X"; - label_Acc_X.Size = new Size(13, 15); - label_Acc_X.TabIndex = 5; - label_Acc_X.Text = "0"; - // - // groupBox3 - // - groupBox3.Controls.Add(label_time_gyr); - groupBox3.Controls.Add(label_Gyr_Z); - groupBox3.Controls.Add(label9); - groupBox3.Controls.Add(label_Gyr_Y); - groupBox3.Controls.Add(label11); - groupBox3.Controls.Add(label_Gyr_X); - groupBox3.Controls.Add(label13); - groupBox3.Location = new Point(112, 86); - groupBox3.Name = "groupBox3"; - groupBox3.Size = new Size(103, 118); - groupBox3.TabIndex = 6; - groupBox3.TabStop = false; - groupBox3.Text = "Gyr"; - // - // label_time_gyr - // - label_time_gyr.AutoSize = true; - label_time_gyr.Location = new Point(3, 100); - label_time_gyr.Name = "label_time_gyr"; - label_time_gyr.Size = new Size(13, 15); - label_time_gyr.TabIndex = 26; - label_time_gyr.Text = "0"; - // - // label_Gyr_Z - // - label_Gyr_Z.AutoSize = true; - label_Gyr_Z.Location = new Point(19, 70); - label_Gyr_Z.Name = "label_Gyr_Z"; - label_Gyr_Z.Size = new Size(13, 15); - label_Gyr_Z.TabIndex = 9; - label_Gyr_Z.Text = "0"; - // - // label9 - // - label9.AutoSize = true; - label9.Location = new Point(6, 70); - label9.Name = "label9"; - label9.Size = new Size(17, 15); - label9.TabIndex = 8; - label9.Text = "Z:"; - // - // label_Gyr_Y - // - label_Gyr_Y.AutoSize = true; - label_Gyr_Y.Location = new Point(19, 45); - label_Gyr_Y.Name = "label_Gyr_Y"; - label_Gyr_Y.Size = new Size(13, 15); - label_Gyr_Y.TabIndex = 7; - label_Gyr_Y.Text = "0"; - // - // label11 - // - label11.AutoSize = true; - label11.Location = new Point(6, 45); - label11.Name = "label11"; - label11.Size = new Size(17, 15); - label11.TabIndex = 6; - label11.Text = "Y:"; - // - // label_Gyr_X - // - label_Gyr_X.AutoSize = true; - label_Gyr_X.Location = new Point(19, 19); - label_Gyr_X.Name = "label_Gyr_X"; - label_Gyr_X.Size = new Size(13, 15); - label_Gyr_X.TabIndex = 5; - label_Gyr_X.Text = "0"; - // - // label13 - // - label13.AutoSize = true; - label13.Location = new Point(6, 19); - label13.Name = "label13"; - label13.Size = new Size(17, 15); - label13.TabIndex = 4; - label13.Text = "X:"; - // - // groupBox4 - // - groupBox4.Controls.Add(label_time_range); - groupBox4.Controls.Add(label_Pos_L); - groupBox4.Controls.Add(label6); - groupBox4.Controls.Add(label_Pos_Y); - groupBox4.Controls.Add(label10); - groupBox4.Controls.Add(label_Pos_X); - groupBox4.Controls.Add(label14); - groupBox4.Location = new Point(221, 86); - groupBox4.Name = "groupBox4"; - groupBox4.Size = new Size(103, 118); - groupBox4.TabIndex = 7; - groupBox4.TabStop = false; - groupBox4.Text = "Pos"; - // - // label_time_range - // - label_time_range.AutoSize = true; - label_time_range.Location = new Point(6, 100); - label_time_range.Name = "label_time_range"; - label_time_range.Size = new Size(13, 15); - label_time_range.TabIndex = 27; - label_time_range.Text = "0"; - // - // label_Pos_L - // - label_Pos_L.AutoSize = true; - label_Pos_L.Location = new Point(19, 70); - label_Pos_L.Name = "label_Pos_L"; - label_Pos_L.Size = new Size(13, 15); - label_Pos_L.TabIndex = 9; - label_Pos_L.Text = "0"; - // - // label6 - // - label6.AutoSize = true; - label6.Location = new Point(6, 70); - label6.Name = "label6"; - label6.Size = new Size(16, 15); - label6.TabIndex = 8; - label6.Text = "L:"; - // - // label_Pos_Y - // - label_Pos_Y.AutoSize = true; - label_Pos_Y.Location = new Point(19, 45); - label_Pos_Y.Name = "label_Pos_Y"; - label_Pos_Y.Size = new Size(13, 15); - label_Pos_Y.TabIndex = 7; - label_Pos_Y.Text = "0"; - // - // label10 - // - label10.AutoSize = true; - label10.Location = new Point(6, 45); - label10.Name = "label10"; - label10.Size = new Size(17, 15); - label10.TabIndex = 6; - label10.Text = "Y:"; - // - // label_Pos_X - // - label_Pos_X.AutoSize = true; - label_Pos_X.Location = new Point(19, 19); - label_Pos_X.Name = "label_Pos_X"; - label_Pos_X.Size = new Size(13, 15); - label_Pos_X.TabIndex = 5; - label_Pos_X.Text = "0"; - // - // label14 - // - label14.AutoSize = true; - label14.Location = new Point(6, 19); - label14.Name = "label14"; - label14.Size = new Size(17, 15); - label14.TabIndex = 4; - label14.Text = "X:"; - // - // trackBar_Power - // - trackBar_Power.Location = new Point(112, 240); - trackBar_Power.Maximum = 100; - trackBar_Power.Name = "trackBar_Power"; - trackBar_Power.Orientation = Orientation.Vertical; - trackBar_Power.Size = new Size(45, 141); - trackBar_Power.TabIndex = 12; - trackBar_Power.Scroll += trackBar_Power_Scroll; - // - // button_LL - // - button_LL.Location = new Point(9, 318); - button_LL.Name = "button_LL"; - button_LL.Size = new Size(75, 23); - button_LL.TabIndex = 13; - button_LL.Text = "LL"; - button_LL.UseVisualStyleBackColor = true; - button_LL.MouseDown += button_UU_MouseDown; - button_LL.MouseUp += button_UU_MouseUp; - // - // button_UU - // - button_UU.Location = new Point(98, 211); - button_UU.Name = "button_UU"; - button_UU.Size = new Size(75, 23); - button_UU.TabIndex = 14; - button_UU.Text = "UU"; - button_UU.UseVisualStyleBackColor = true; - button_UU.MouseDown += button_UU_MouseDown; - button_UU.MouseUp += button_UU_MouseUp; - // - // button_DD - // - button_DD.Location = new Point(98, 412); - button_DD.Name = "button_DD"; - button_DD.Size = new Size(75, 23); - button_DD.TabIndex = 15; - button_DD.Text = "DD"; - button_DD.UseVisualStyleBackColor = true; - button_DD.MouseDown += button_UU_MouseDown; - button_DD.MouseUp += button_UU_MouseUp; - // - // button_RR - // - button_RR.Location = new Point(188, 318); - button_RR.Name = "button_RR"; - button_RR.Size = new Size(75, 23); - button_RR.TabIndex = 16; - button_RR.Text = "RR"; - button_RR.UseVisualStyleBackColor = true; - button_RR.MouseDown += button_UU_MouseDown; - button_RR.MouseUp += button_UU_MouseUp; - // - // label_Pow - // - label_Pow.AutoSize = true; - label_Pow.Location = new Point(126, 384); - label_Pow.Name = "label_Pow"; - label_Pow.Size = new Size(13, 15); - label_Pow.TabIndex = 21; - label_Pow.Text = "0"; - // - // button_ML - // - button_ML.Location = new Point(9, 211); - button_ML.Name = "button_ML"; - button_ML.Size = new Size(75, 23); - button_ML.TabIndex = 22; - button_ML.Text = "<-"; - button_ML.UseVisualStyleBackColor = true; - button_ML.MouseDown += button_UU_MouseDown; - button_ML.MouseUp += button_UU_MouseUp; - // - // button_MR - // - button_MR.Location = new Point(188, 211); - button_MR.Name = "button_MR"; - button_MR.Size = new Size(75, 23); - button_MR.TabIndex = 23; - button_MR.Text = "->"; - button_MR.UseVisualStyleBackColor = true; - button_MR.MouseDown += button_UU_MouseDown; - button_MR.MouseUp += button_UU_MouseUp; - // - // groupBox5 - // - groupBox5.Controls.Add(label_time_of); - groupBox5.Controls.Add(label_OF_Y); - groupBox5.Controls.Add(label17); - groupBox5.Controls.Add(label_OF_X); - groupBox5.Controls.Add(label19); - groupBox5.Location = new Point(330, 86); - groupBox5.Name = "groupBox5"; - groupBox5.Size = new Size(104, 118); - groupBox5.TabIndex = 24; - groupBox5.TabStop = false; - groupBox5.Text = "OF"; - // - // label_time_of - // - label_time_of.AutoSize = true; - label_time_of.Location = new Point(6, 100); - label_time_of.Name = "label_time_of"; - label_time_of.Size = new Size(13, 15); - label_time_of.TabIndex = 27; - label_time_of.Text = "0"; - // - // label_OF_Y - // - label_OF_Y.AutoSize = true; - label_OF_Y.Location = new Point(19, 45); - label_OF_Y.Name = "label_OF_Y"; - label_OF_Y.Size = new Size(13, 15); - label_OF_Y.TabIndex = 7; - label_OF_Y.Text = "0"; - // - // label17 - // - label17.AutoSize = true; - label17.Location = new Point(6, 45); - label17.Name = "label17"; - label17.Size = new Size(17, 15); - label17.TabIndex = 6; - label17.Text = "Y:"; - // - // label_OF_X - // - label_OF_X.AutoSize = true; - label_OF_X.Location = new Point(19, 19); - label_OF_X.Name = "label_OF_X"; - label_OF_X.Size = new Size(13, 15); - label_OF_X.TabIndex = 5; - label_OF_X.Text = "0"; - // - // label19 - // - label19.AutoSize = true; - label19.Location = new Point(6, 19); - label19.Name = "label19"; - label19.Size = new Size(17, 15); - label19.TabIndex = 4; - label19.Text = "X:"; - // - // trackBar_Value - // - trackBar_Value.Location = new Point(349, 240); - trackBar_Value.Maximum = 100; - trackBar_Value.Minimum = 1; - trackBar_Value.Name = "trackBar_Value"; - trackBar_Value.Orientation = Orientation.Vertical; - trackBar_Value.Size = new Size(45, 195); - trackBar_Value.TabIndex = 25; - trackBar_Value.Value = 1; - // - // label4 - // - label4.AutoSize = true; - label4.Location = new Point(347, 219); - label4.Name = "label4"; - label4.Size = new Size(47, 15); - label4.TabIndex = 26; - label4.Text = "POWER"; - // - // Form_Main - // - AutoScaleDimensions = new SizeF(7F, 15F); - AutoScaleMode = AutoScaleMode.Font; - ClientSize = new Size(446, 447); - Controls.Add(label4); - Controls.Add(trackBar_Value); - Controls.Add(groupBox5); - Controls.Add(button_MR); - Controls.Add(button_ML); - Controls.Add(label_Pow); - Controls.Add(button_RR); - Controls.Add(button_DD); - Controls.Add(button_UU); - Controls.Add(button_LL); - Controls.Add(trackBar_Power); - Controls.Add(groupBox4); - Controls.Add(groupBox3); - Controls.Add(groupBox2); - Controls.Add(groupBox1); - MinimumSize = new Size(291, 389); - Name = "Form_Main"; - Text = "Drone Client V1.0"; - FormClosing += Form_Main_FormClosing; - groupBox1.ResumeLayout(false); - groupBox1.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).EndInit(); - groupBox2.ResumeLayout(false); - groupBox2.PerformLayout(); - groupBox3.ResumeLayout(false); - groupBox3.PerformLayout(); - groupBox4.ResumeLayout(false); - groupBox4.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit(); - groupBox5.ResumeLayout(false); - groupBox5.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)trackBar_Value).EndInit(); - ResumeLayout(false); - PerformLayout(); - } + /// + /// Required method for Designer support - do not modify + /// the contents of this method with the code editor. + /// + private void InitializeComponent() + { + components = new System.ComponentModel.Container(); + timer_Test = new System.Windows.Forms.Timer(components); + groupBox1 = new GroupBox(); + TeleClientStatusPicture = new PictureBox(); + label12 = new Label(); + button1 = new Button(); + TeleServerPortCtrl = new NumericUpDown(); + label8 = new Label(); + textBox_Server_Addr = new TextBox(); + label2 = new Label(); + button_Connect = new Button(); + numericUpDown_Server_Port = new NumericUpDown(); + label3 = new Label(); + label1 = new Label(); + groupBox2 = new GroupBox(); + label_time_acc = new Label(); + label_Acc_Z = new Label(); + label7 = new Label(); + label_Acc_Y = new Label(); + label5 = new Label(); + label_Acc_X = new Label(); + groupBox3 = new GroupBox(); + label_time_gyr = new Label(); + label_Gyr_Z = new Label(); + label9 = new Label(); + label_Gyr_Y = new Label(); + label11 = new Label(); + label_Gyr_X = new Label(); + label13 = new Label(); + groupBox4 = new GroupBox(); + label_time_range = new Label(); + label_Pos_L = new Label(); + label6 = new Label(); + label_Pos_Y = new Label(); + label10 = new Label(); + label_Pos_X = new Label(); + label14 = new Label(); + trackBar_Power = new TrackBar(); + button_LL = new Button(); + button_UU = new Button(); + button_DD = new Button(); + button_RR = new Button(); + label_Pow = new Label(); + button_ML = new Button(); + button_MR = new Button(); + groupBox5 = new GroupBox(); + label_time_of = new Label(); + label_OF_Y = new Label(); + label17 = new Label(); + label_OF_X = new Label(); + label19 = new Label(); + trackBar_Value = new TrackBar(); + label4 = new Label(); + groupBox1.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)TeleClientStatusPicture).BeginInit(); + ((System.ComponentModel.ISupportInitialize)TeleServerPortCtrl).BeginInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit(); + groupBox2.SuspendLayout(); + groupBox3.SuspendLayout(); + groupBox4.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit(); + groupBox5.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)trackBar_Value).BeginInit(); + SuspendLayout(); + // + // timer_Test + // + timer_Test.Enabled = true; + timer_Test.Interval = 10; + timer_Test.Tick += timer_Test_Tick; + // + // groupBox1 + // + groupBox1.Controls.Add(TeleClientStatusPicture); + groupBox1.Controls.Add(label12); + groupBox1.Controls.Add(button1); + groupBox1.Controls.Add(TeleServerPortCtrl); + groupBox1.Controls.Add(label8); + groupBox1.Controls.Add(textBox_Server_Addr); + groupBox1.Controls.Add(label2); + groupBox1.Controls.Add(button_Connect); + groupBox1.Controls.Add(numericUpDown_Server_Port); + groupBox1.Controls.Add(label3); + groupBox1.Dock = DockStyle.Top; + groupBox1.Location = new Point(0, 0); + groupBox1.Margin = new Padding(3, 4, 3, 4); + groupBox1.Name = "groupBox1"; + groupBox1.Padding = new Padding(3, 4, 3, 4); + groupBox1.Size = new Size(510, 107); + groupBox1.TabIndex = 3; + groupBox1.TabStop = false; + groupBox1.Tag = ""; + groupBox1.Text = "Server"; + // + // TeleClientStatusPicture + // + TeleClientStatusPicture.Anchor = AnchorStyles.Top | AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right; + TeleClientStatusPicture.Location = new Point(469, 53); + TeleClientStatusPicture.Name = "TeleClientStatusPicture"; + TeleClientStatusPicture.Size = new Size(24, 24); + TeleClientStatusPicture.TabIndex = 9; + TeleClientStatusPicture.TabStop = false; + // + // label12 + // + label12.AutoSize = true; + label12.Location = new Point(393, 51); + label12.Name = "label12"; + label12.Size = new Size(74, 20); + label12.TabIndex = 8; + label12.Tag = "#clients_port"; + label12.Text = "TeleClient"; + // + // button1 + // + button1.BackColor = Color.Transparent; + button1.Location = new Point(369, 16); + button1.Margin = new Padding(3, 4, 3, 4); + button1.Name = "button1"; + button1.Size = new Size(127, 31); + button1.TabIndex = 7; + button1.Tag = ""; + button1.Text = "Run TeleServer"; + button1.UseVisualStyleBackColor = false; + button1.Click += button1_Click; + // + // TeleServerPortCtrl + // + TeleServerPortCtrl.Location = new Point(291, 18); + TeleServerPortCtrl.Margin = new Padding(3, 4, 3, 4); + TeleServerPortCtrl.Maximum = new decimal(new int[] { 65000, 0, 0, 0 }); + TeleServerPortCtrl.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); + TeleServerPortCtrl.Name = "TeleServerPortCtrl"; + TeleServerPortCtrl.Size = new Size(71, 27); + TeleServerPortCtrl.TabIndex = 6; + TeleServerPortCtrl.Value = new decimal(new int[] { 8888, 0, 0, 0 }); + // + // label8 + // + label8.AutoSize = true; + label8.Location = new Point(248, 21); + label8.Name = "label8"; + label8.Size = new Size(38, 20); + label8.TabIndex = 5; + label8.Tag = "#clients_port"; + label8.Text = "Port:"; + // + // textBox_Server_Addr + // + textBox_Server_Addr.Location = new Point(55, 21); + textBox_Server_Addr.Margin = new Padding(3, 4, 3, 4); + textBox_Server_Addr.Name = "textBox_Server_Addr"; + textBox_Server_Addr.Size = new Size(142, 27); + textBox_Server_Addr.TabIndex = 4; + textBox_Server_Addr.Text = "127.0.0.1"; + // + // label2 + // + label2.AutoSize = true; + label2.Location = new Point(7, 25); + label2.Name = "label2"; + label2.Size = new Size(45, 20); + label2.TabIndex = 3; + label2.Tag = ""; + label2.Text = "Addr:"; + // + // button_Connect + // + button_Connect.BackColor = Color.Transparent; + button_Connect.Location = new Point(128, 61); + button_Connect.Margin = new Padding(3, 4, 3, 4); + button_Connect.Name = "button_Connect"; + button_Connect.Size = new Size(70, 31); + button_Connect.TabIndex = 2; + button_Connect.Tag = ""; + button_Connect.Text = "Connect"; + button_Connect.UseVisualStyleBackColor = false; + button_Connect.Click += button_Connect_Click; + // + // numericUpDown_Server_Port + // + numericUpDown_Server_Port.Location = new Point(50, 64); + numericUpDown_Server_Port.Margin = new Padding(3, 4, 3, 4); + numericUpDown_Server_Port.Maximum = new decimal(new int[] { 65000, 0, 0, 0 }); + numericUpDown_Server_Port.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Server_Port.Name = "numericUpDown_Server_Port"; + numericUpDown_Server_Port.Size = new Size(71, 27); + numericUpDown_Server_Port.TabIndex = 1; + numericUpDown_Server_Port.Value = new decimal(new int[] { 1001, 0, 0, 0 }); + // + // label3 + // + label3.AutoSize = true; + label3.Location = new Point(7, 67); + label3.Name = "label3"; + label3.Size = new Size(38, 20); + label3.TabIndex = 0; + label3.Tag = "#clients_port"; + label3.Text = "Port:"; + // + // label1 + // + label1.AutoSize = true; + label1.Location = new Point(7, 25); + label1.Name = "label1"; + label1.Size = new Size(21, 20); + label1.TabIndex = 4; + label1.Text = "X:"; + // + // groupBox2 + // + groupBox2.Controls.Add(label_time_acc); + groupBox2.Controls.Add(label_Acc_Z); + groupBox2.Controls.Add(label7); + groupBox2.Controls.Add(label_Acc_Y); + groupBox2.Controls.Add(label5); + groupBox2.Controls.Add(label_Acc_X); + groupBox2.Controls.Add(label1); + groupBox2.Location = new Point(7, 115); + groupBox2.Margin = new Padding(3, 4, 3, 4); + groupBox2.Name = "groupBox2"; + groupBox2.Padding = new Padding(3, 4, 3, 4); + groupBox2.Size = new Size(114, 157); + groupBox2.TabIndex = 5; + groupBox2.TabStop = false; + groupBox2.Text = "Acc"; + // + // label_time_acc + // + label_time_acc.AutoSize = true; + label_time_acc.Location = new Point(7, 133); + label_time_acc.Name = "label_time_acc"; + label_time_acc.Size = new Size(17, 20); + label_time_acc.TabIndex = 25; + label_time_acc.Text = "0"; + // + // label_Acc_Z + // + label_Acc_Z.AutoSize = true; + label_Acc_Z.Location = new Point(22, 93); + label_Acc_Z.Name = "label_Acc_Z"; + label_Acc_Z.Size = new Size(17, 20); + label_Acc_Z.TabIndex = 9; + label_Acc_Z.Text = "0"; + // + // label7 + // + label7.AutoSize = true; + label7.Location = new Point(7, 93); + label7.Name = "label7"; + label7.Size = new Size(21, 20); + label7.TabIndex = 8; + label7.Text = "Z:"; + // + // label_Acc_Y + // + label_Acc_Y.AutoSize = true; + label_Acc_Y.Location = new Point(22, 60); + label_Acc_Y.Name = "label_Acc_Y"; + label_Acc_Y.Size = new Size(17, 20); + label_Acc_Y.TabIndex = 7; + label_Acc_Y.Text = "0"; + // + // label5 + // + label5.AutoSize = true; + label5.Location = new Point(7, 60); + label5.Name = "label5"; + label5.Size = new Size(20, 20); + label5.TabIndex = 6; + label5.Text = "Y:"; + // + // label_Acc_X + // + label_Acc_X.AutoSize = true; + label_Acc_X.Location = new Point(22, 25); + label_Acc_X.Name = "label_Acc_X"; + label_Acc_X.Size = new Size(17, 20); + label_Acc_X.TabIndex = 5; + label_Acc_X.Text = "0"; + // + // groupBox3 + // + groupBox3.Controls.Add(label_time_gyr); + groupBox3.Controls.Add(label_Gyr_Z); + groupBox3.Controls.Add(label9); + groupBox3.Controls.Add(label_Gyr_Y); + groupBox3.Controls.Add(label11); + groupBox3.Controls.Add(label_Gyr_X); + groupBox3.Controls.Add(label13); + groupBox3.Location = new Point(128, 115); + groupBox3.Margin = new Padding(3, 4, 3, 4); + groupBox3.Name = "groupBox3"; + groupBox3.Padding = new Padding(3, 4, 3, 4); + groupBox3.Size = new Size(118, 157); + groupBox3.TabIndex = 6; + groupBox3.TabStop = false; + groupBox3.Text = "Gyr"; + // + // label_time_gyr + // + label_time_gyr.AutoSize = true; + label_time_gyr.Location = new Point(3, 133); + label_time_gyr.Name = "label_time_gyr"; + label_time_gyr.Size = new Size(17, 20); + label_time_gyr.TabIndex = 26; + label_time_gyr.Text = "0"; + // + // label_Gyr_Z + // + label_Gyr_Z.AutoSize = true; + label_Gyr_Z.Location = new Point(22, 93); + label_Gyr_Z.Name = "label_Gyr_Z"; + label_Gyr_Z.Size = new Size(17, 20); + label_Gyr_Z.TabIndex = 9; + label_Gyr_Z.Text = "0"; + // + // label9 + // + label9.AutoSize = true; + label9.Location = new Point(7, 93); + label9.Name = "label9"; + label9.Size = new Size(21, 20); + label9.TabIndex = 8; + label9.Text = "Z:"; + // + // label_Gyr_Y + // + label_Gyr_Y.AutoSize = true; + label_Gyr_Y.Location = new Point(22, 60); + label_Gyr_Y.Name = "label_Gyr_Y"; + label_Gyr_Y.Size = new Size(17, 20); + label_Gyr_Y.TabIndex = 7; + label_Gyr_Y.Text = "0"; + // + // label11 + // + label11.AutoSize = true; + label11.Location = new Point(7, 60); + label11.Name = "label11"; + label11.Size = new Size(20, 20); + label11.TabIndex = 6; + label11.Text = "Y:"; + // + // label_Gyr_X + // + label_Gyr_X.AutoSize = true; + label_Gyr_X.Location = new Point(22, 25); + label_Gyr_X.Name = "label_Gyr_X"; + label_Gyr_X.Size = new Size(17, 20); + label_Gyr_X.TabIndex = 5; + label_Gyr_X.Text = "0"; + // + // label13 + // + label13.AutoSize = true; + label13.Location = new Point(7, 25); + label13.Name = "label13"; + label13.Size = new Size(21, 20); + label13.TabIndex = 4; + label13.Text = "X:"; + // + // groupBox4 + // + groupBox4.Controls.Add(label_time_range); + groupBox4.Controls.Add(label_Pos_L); + groupBox4.Controls.Add(label6); + groupBox4.Controls.Add(label_Pos_Y); + groupBox4.Controls.Add(label10); + groupBox4.Controls.Add(label_Pos_X); + groupBox4.Controls.Add(label14); + groupBox4.Location = new Point(253, 115); + groupBox4.Margin = new Padding(3, 4, 3, 4); + groupBox4.Name = "groupBox4"; + groupBox4.Padding = new Padding(3, 4, 3, 4); + groupBox4.Size = new Size(118, 157); + groupBox4.TabIndex = 7; + groupBox4.TabStop = false; + groupBox4.Text = "Pos"; + // + // label_time_range + // + label_time_range.AutoSize = true; + label_time_range.Location = new Point(7, 133); + label_time_range.Name = "label_time_range"; + label_time_range.Size = new Size(17, 20); + label_time_range.TabIndex = 27; + label_time_range.Text = "0"; + // + // label_Pos_L + // + label_Pos_L.AutoSize = true; + label_Pos_L.Location = new Point(22, 93); + label_Pos_L.Name = "label_Pos_L"; + label_Pos_L.Size = new Size(17, 20); + label_Pos_L.TabIndex = 9; + label_Pos_L.Text = "0"; + // + // label6 + // + label6.AutoSize = true; + label6.Location = new Point(7, 93); + label6.Name = "label6"; + label6.Size = new Size(19, 20); + label6.TabIndex = 8; + label6.Text = "L:"; + // + // label_Pos_Y + // + label_Pos_Y.AutoSize = true; + label_Pos_Y.Location = new Point(22, 60); + label_Pos_Y.Name = "label_Pos_Y"; + label_Pos_Y.Size = new Size(17, 20); + label_Pos_Y.TabIndex = 7; + label_Pos_Y.Text = "0"; + // + // label10 + // + label10.AutoSize = true; + label10.Location = new Point(7, 60); + label10.Name = "label10"; + label10.Size = new Size(20, 20); + label10.TabIndex = 6; + label10.Text = "Y:"; + // + // label_Pos_X + // + label_Pos_X.AutoSize = true; + label_Pos_X.Location = new Point(22, 25); + label_Pos_X.Name = "label_Pos_X"; + label_Pos_X.Size = new Size(17, 20); + label_Pos_X.TabIndex = 5; + label_Pos_X.Text = "0"; + // + // label14 + // + label14.AutoSize = true; + label14.Location = new Point(7, 25); + label14.Name = "label14"; + label14.Size = new Size(21, 20); + label14.TabIndex = 4; + label14.Text = "X:"; + // + // trackBar_Power + // + trackBar_Power.Location = new Point(128, 320); + trackBar_Power.Margin = new Padding(3, 4, 3, 4); + trackBar_Power.Maximum = 100; + trackBar_Power.Name = "trackBar_Power"; + trackBar_Power.Orientation = Orientation.Vertical; + trackBar_Power.Size = new Size(56, 188); + trackBar_Power.TabIndex = 12; + trackBar_Power.Scroll += trackBar_Power_Scroll; + // + // button_LL + // + button_LL.Location = new Point(10, 424); + button_LL.Margin = new Padding(3, 4, 3, 4); + button_LL.Name = "button_LL"; + button_LL.Size = new Size(86, 31); + button_LL.TabIndex = 13; + button_LL.Text = "LL"; + button_LL.UseVisualStyleBackColor = true; + button_LL.MouseDown += button_UU_MouseDown; + button_LL.MouseUp += button_UU_MouseUp; + // + // button_UU + // + button_UU.Location = new Point(112, 281); + button_UU.Margin = new Padding(3, 4, 3, 4); + button_UU.Name = "button_UU"; + button_UU.Size = new Size(86, 31); + button_UU.TabIndex = 14; + button_UU.Text = "UU"; + button_UU.UseVisualStyleBackColor = true; + button_UU.MouseDown += button_UU_MouseDown; + button_UU.MouseUp += button_UU_MouseUp; + // + // button_DD + // + button_DD.Location = new Point(112, 549); + button_DD.Margin = new Padding(3, 4, 3, 4); + button_DD.Name = "button_DD"; + button_DD.Size = new Size(86, 31); + button_DD.TabIndex = 15; + button_DD.Text = "DD"; + button_DD.UseVisualStyleBackColor = true; + button_DD.MouseDown += button_UU_MouseDown; + button_DD.MouseUp += button_UU_MouseUp; + // + // button_RR + // + button_RR.Location = new Point(215, 424); + button_RR.Margin = new Padding(3, 4, 3, 4); + button_RR.Name = "button_RR"; + button_RR.Size = new Size(86, 31); + button_RR.TabIndex = 16; + button_RR.Text = "RR"; + button_RR.UseVisualStyleBackColor = true; + button_RR.MouseDown += button_UU_MouseDown; + button_RR.MouseUp += button_UU_MouseUp; + // + // label_Pow + // + label_Pow.AutoSize = true; + label_Pow.Location = new Point(144, 512); + label_Pow.Name = "label_Pow"; + label_Pow.Size = new Size(17, 20); + label_Pow.TabIndex = 21; + label_Pow.Text = "0"; + // + // button_ML + // + button_ML.Location = new Point(10, 281); + button_ML.Margin = new Padding(3, 4, 3, 4); + button_ML.Name = "button_ML"; + button_ML.Size = new Size(86, 31); + button_ML.TabIndex = 22; + button_ML.Text = "<-"; + button_ML.UseVisualStyleBackColor = true; + button_ML.MouseDown += button_UU_MouseDown; + button_ML.MouseUp += button_UU_MouseUp; + // + // button_MR + // + button_MR.Location = new Point(215, 281); + button_MR.Margin = new Padding(3, 4, 3, 4); + button_MR.Name = "button_MR"; + button_MR.Size = new Size(86, 31); + button_MR.TabIndex = 23; + button_MR.Text = "->"; + button_MR.UseVisualStyleBackColor = true; + button_MR.MouseDown += button_UU_MouseDown; + button_MR.MouseUp += button_UU_MouseUp; + // + // groupBox5 + // + groupBox5.Controls.Add(label_time_of); + groupBox5.Controls.Add(label_OF_Y); + groupBox5.Controls.Add(label17); + groupBox5.Controls.Add(label_OF_X); + groupBox5.Controls.Add(label19); + groupBox5.Location = new Point(377, 115); + groupBox5.Margin = new Padding(3, 4, 3, 4); + groupBox5.Name = "groupBox5"; + groupBox5.Padding = new Padding(3, 4, 3, 4); + groupBox5.Size = new Size(119, 157); + groupBox5.TabIndex = 24; + groupBox5.TabStop = false; + groupBox5.Text = "OF"; + // + // label_time_of + // + label_time_of.AutoSize = true; + label_time_of.Location = new Point(7, 133); + label_time_of.Name = "label_time_of"; + label_time_of.Size = new Size(17, 20); + label_time_of.TabIndex = 27; + label_time_of.Text = "0"; + // + // label_OF_Y + // + label_OF_Y.AutoSize = true; + label_OF_Y.Location = new Point(22, 60); + label_OF_Y.Name = "label_OF_Y"; + label_OF_Y.Size = new Size(17, 20); + label_OF_Y.TabIndex = 7; + label_OF_Y.Text = "0"; + // + // label17 + // + label17.AutoSize = true; + label17.Location = new Point(7, 60); + label17.Name = "label17"; + label17.Size = new Size(20, 20); + label17.TabIndex = 6; + label17.Text = "Y:"; + // + // label_OF_X + // + label_OF_X.AutoSize = true; + label_OF_X.Location = new Point(22, 25); + label_OF_X.Name = "label_OF_X"; + label_OF_X.Size = new Size(17, 20); + label_OF_X.TabIndex = 5; + label_OF_X.Text = "0"; + // + // label19 + // + label19.AutoSize = true; + label19.Location = new Point(7, 25); + label19.Name = "label19"; + label19.Size = new Size(21, 20); + label19.TabIndex = 4; + label19.Text = "X:"; + // + // trackBar_Value + // + trackBar_Value.Location = new Point(399, 320); + trackBar_Value.Margin = new Padding(3, 4, 3, 4); + trackBar_Value.Maximum = 100; + trackBar_Value.Minimum = 1; + trackBar_Value.Name = "trackBar_Value"; + trackBar_Value.Orientation = Orientation.Vertical; + trackBar_Value.Size = new Size(56, 260); + trackBar_Value.TabIndex = 25; + trackBar_Value.Value = 1; + // + // label4 + // + label4.AutoSize = true; + label4.Location = new Point(397, 292); + label4.Name = "label4"; + label4.Size = new Size(59, 20); + label4.TabIndex = 26; + label4.Text = "POWER"; + // + // Form_Main + // + AutoScaleDimensions = new SizeF(8F, 20F); + AutoScaleMode = AutoScaleMode.Font; + ClientSize = new Size(510, 596); + Controls.Add(label4); + Controls.Add(trackBar_Value); + Controls.Add(groupBox5); + Controls.Add(button_MR); + Controls.Add(button_ML); + Controls.Add(label_Pow); + Controls.Add(button_RR); + Controls.Add(button_DD); + Controls.Add(button_UU); + Controls.Add(button_LL); + Controls.Add(trackBar_Power); + Controls.Add(groupBox4); + Controls.Add(groupBox3); + Controls.Add(groupBox2); + Controls.Add(groupBox1); + Margin = new Padding(3, 4, 3, 4); + MinimumSize = new Size(330, 503); + Name = "Form_Main"; + Text = "Drone Client V1.0"; + FormClosing += Form_Main_FormClosing; + groupBox1.ResumeLayout(false); + groupBox1.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)TeleClientStatusPicture).EndInit(); + ((System.ComponentModel.ISupportInitialize)TeleServerPortCtrl).EndInit(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).EndInit(); + groupBox2.ResumeLayout(false); + groupBox2.PerformLayout(); + groupBox3.ResumeLayout(false); + groupBox3.PerformLayout(); + groupBox4.ResumeLayout(false); + groupBox4.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit(); + groupBox5.ResumeLayout(false); + groupBox5.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)trackBar_Value).EndInit(); + ResumeLayout(false); + PerformLayout(); + } - #endregion - private System.Windows.Forms.Timer timer_Test; + #endregion + private System.Windows.Forms.Timer timer_Test; private GroupBox groupBox1; private TextBox textBox_Server_Addr; private Label label2; @@ -648,5 +737,10 @@ private Label label_OF_X; private Label label19; private TrackBar trackBar_Value; - } + private Button button1; + private NumericUpDown TeleServerPortCtrl; + private Label label8; + private PictureBox TeleClientStatusPicture; + private Label label12; + } } diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index f33d230..540f602 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -3,170 +3,278 @@ using System.Numerics; using System.Windows.Forms; using static DroneSimulator.NetClient; using DroneClient; +using TelemetryIO.Models; +using TelemetryIO; +using DroneClient.Models; namespace DroneSimulator { - public partial class Form_Main : Form - { - private NetClient netClient = new NetClient(); - - public Form_Main() + public partial class Form_Main : Form { - InitializeComponent(); - } + private NetClient netClient = new NetClient(); + private Telemetry telemetry = Telemetry.Instance; + private MonitorContainer monitoring = MonitorContainer.Instance; + private System.Windows.Forms.Timer copyDataTimer = new System.Windows.Forms.Timer(); + private System.Windows.Forms.Timer MonitoringTimer = new System.Windows.Forms.Timer(); + TelemetryServer tele_server; + string connectIco = Path.Combine(Application.StartupPath, "Images", "connect.ico"); + string disconnectIco = Path.Combine(Application.StartupPath, "Images", "disconnect.ico"); - private void ConnectionCallback(object o) - { - ConnectData data = (ConnectData)o; - - if (!data.Connect) - { - try + public Form_Main() { - Invoke((MethodInvoker)delegate - { - button_Connect.Text = "Connect"; - button_Connect.BackColor = Color.Transparent; - MessageBox.Show("Connection closed"); - }); + InitializeComponent(); + tele_server = new TelemetryServer(); + if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); } - catch { } - return; - } - } - - Drone dataDrone = new Drone(); - - private void ReceiveCallback(object o) - { - ReceiveData data = (ReceiveData)o; - - List? send = dataDrone.DataStream(data.Buffer, data.Size); - - if (send == null) return; - try - { - foreach (byte[]? b in send) + private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e) { - if (b != null) data.Server?.Send(b); + if (TeleClientStatusPicture.InvokeRequired) + { + TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); })); + } + else + { + if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); + } + monitoring.resetMonitorContainer(); + } + + private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e) + { + if (TeleClientStatusPicture.InvokeRequired) + { + TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); })); + } + else + { + if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); + } + } + + private void MonitoringTimer_Tick(object? sender, EventArgs e) + { + if (monitoring.isMonitor == 1) + { + byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)]; + for (int i = 0; i < monitoring.monitorFillLevel; i++) + { + int slot = monitoring.monitorItems[i].slotNo; + int offset = monitoring.monitorItems[i].offset; + int len = monitoring.monitorItems[i].len; + if(len == 0)continue; + Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float)); + } + if (tele_server != null) + { + tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length)); + } + } + } + + private void CopyDataTimer_Tick(object? sender, EventArgs e) + { + /*if (dataDrone != null) + { + float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ }; + Array.Copy(imu, telemetry.imu, 6); + }*/ + + if (monitoring.isMonitor == 1) MonitoringTimer.Start(); + else MonitoringTimer.Stop(); + } + + private void ConnectionCallback(object o) + { + ConnectData data = (ConnectData)o; + + if (!data.Connect) + { + try + { + Invoke((MethodInvoker)delegate + { + button_Connect.Text = "Connect"; + button_Connect.BackColor = Color.Transparent; + MessageBox.Show("Connection closed"); + }); + } + catch { } + + return; + } + } + + Drone dataDrone = new Drone(); + + private void ReceiveCallback(object o) + { + ReceiveData data = (ReceiveData)o; + + List? send = dataDrone.DataStream(data.Buffer, data.Size); + + if (send == null) return; + try + { + foreach (byte[]? b in send) + { + if (b != null) data.Server?.Send(b); + } + } + catch { } + } + + private void button_Connect_Click(object sender, EventArgs e) + { + var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback); + + switch (done) + { + case NetClient.ClientState.Error: + { + MessageBox.Show("Error connecting to server"); + break; + } + case NetClient.ClientState.Connected: + { + button_Connect.Text = "Disconnect"; + button_Connect.BackColor = Color.LimeGreen; + break; + } + case NetClient.ClientState.Stop: + { + button_Connect.Text = "Connect"; + button_Connect.BackColor = Color.Transparent; + break; + } + } + + if (done != NetClient.ClientState.Connected) return; + + + copyDataTimer.Interval = 10; + copyDataTimer.Tick += CopyDataTimer_Tick; + tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value)); + var t = Task.Run(() => tele_server.Open()); + tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected; + tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected; + MonitoringTimer.Interval = 25; + MonitoringTimer.Tick += MonitoringTimer_Tick; + copyDataTimer.Start(); + button1.BackColor = Color.LimeGreen; + } + + private void Form_Main_FormClosing(object sender, FormClosingEventArgs e) + { + netClient?.Close(); + netClient = null; + } + + private void timer_Test_Tick(object sender, EventArgs e) + { + label_Acc_X.Text = dataDrone.AccX.ToString(); + label_Acc_Y.Text = dataDrone.AccY.ToString(); + label_Acc_Z.Text = dataDrone.AccZ.ToString(); + + label_time_acc.Text = dataDrone.TimeAcc.ToString(); + + label_Gyr_X.Text = dataDrone.GyrX.ToString(); + label_Gyr_Y.Text = dataDrone.GyrY.ToString(); + label_Gyr_Z.Text = dataDrone.GyrZ.ToString(); + + label_time_gyr.Text = dataDrone.TimeGyr.ToString(); + + float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ }; + Array.Copy(imu, telemetry.imu, 6); + + label_Pos_X.Text = dataDrone.PosX.ToString(); + label_Pos_Y.Text = dataDrone.PosY.ToString(); + label_Pos_L.Text = dataDrone.LaserRange.ToString(); + + label_time_range.Text = dataDrone.TimeRange.ToString(); + + float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange }; + Array.Copy(pos, telemetry.pos, 3); + + label_OF_X.Text = dataDrone.OF.X.ToString(); + label_OF_Y.Text = dataDrone.OF.Y.ToString(); + + label_time_of.Text = dataDrone.TimeRange.ToString(); + + float[] of = { dataDrone.OF.X, dataDrone.OF.Y }; + Array.Copy(of, telemetry.of, 2); + + if (monitoring.isMonitor == 1) MonitoringTimer.Start(); + else MonitoringTimer.Stop(); + + netClient.SendData(dataDrone.SendReqest()); + } + + private void trackBar_Power_Scroll(object sender, EventArgs e) + { + float pow = (float)trackBar_Power.Value / 100; + + label_Pow.Text = pow.ToString(); + + dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow; + } + + private void button_UU_MouseDown(object sender, MouseEventArgs e) + { + float pow = ((float)trackBar_Value.Value) / 10.0f; + + if (sender == button_UU) + { + dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR += pow; + } + if (sender == button_DD) + { + dataDrone.MotorUL += pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow; + } + if (sender == button_LL) + { + dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; + } + if (sender == button_RR) + { + dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; + } + + if (sender == button_ML) + { + dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; + } + + if (sender == button_MR) + { + dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; + } + } + + private void button_UU_MouseUp(object sender, MouseEventArgs e) + { + trackBar_Power_Scroll(null, null); + } + + private void button1_Click(object sender, EventArgs e) + { + + copyDataTimer.Interval = 10; + copyDataTimer.Tick += CopyDataTimer_Tick; + tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value)); + var t = Task.Run(() => tele_server.Open()); + tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected; + tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected; + MonitoringTimer.Interval = 25; + MonitoringTimer.Tick += MonitoringTimer_Tick; + copyDataTimer.Start(); + button1.BackColor = Color.LimeGreen; } - } - catch { } } - - private void button_Connect_Click(object sender, EventArgs e) - { - var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback); - - switch (done) - { - case NetClient.ClientState.Error: - { - MessageBox.Show("Error connecting to server"); - break; - } - case NetClient.ClientState.Connected: - { - button_Connect.Text = "Disconnect"; - button_Connect.BackColor = Color.LimeGreen; - break; - } - case NetClient.ClientState.Stop: - { - button_Connect.Text = "Connect"; - button_Connect.BackColor = Color.Transparent; - break; - } - } - - if (done != NetClient.ClientState.Connected) return; - } - - private void Form_Main_FormClosing(object sender, FormClosingEventArgs e) - { - netClient?.Close(); - netClient = null; - } - - private void timer_Test_Tick(object sender, EventArgs e) - { - label_Acc_X.Text = dataDrone.AccX.ToString(); - label_Acc_Y.Text = dataDrone.AccY.ToString(); - label_Acc_Z.Text = dataDrone.AccZ.ToString(); - - label_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); - } - } } diff --git a/DroneClient/Models/MonitorContainer.cs b/DroneClient/Models/MonitorContainer.cs new file mode 100644 index 0000000..57fc853 --- /dev/null +++ b/DroneClient/Models/MonitorContainer.cs @@ -0,0 +1,126 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using TelemetryIO; + +namespace DroneClient.Models +{ + public class monitorItem + { + public int slotNo = 0; + public int offset = 0; + public int len = 0; + public int id = 0; + + public monitorItem() + { + + } + public monitorItem(int slotNo, int offset, int len, int id) + { + this.slotNo = slotNo; + this.offset = offset; + this.len = len; + this.id = id; + } + } + + public class MonitorContainer + { + public const int monitorMaxFill = 32; + public int monitorFillLevel = 0; + public monitorItem[] monitorItems = new monitorItem[monitorMaxFill]; + public int isMonitor = 0; + private static volatile MonitorContainer instance; + private static readonly object _lock = new object(); + + public static MonitorContainer Instance + { + get + { + if (instance == null) + { + lock (_lock) + { + if (instance == null) + { + instance = new MonitorContainer(); + } + } + } + return instance; + } + + } + + private MonitorContainer() + { + for (int i = 0; i < monitorMaxFill; i++) + { + monitorItems[i] = new monitorItem(); + } + } + + public void resetMonitorContainer() + { + for (int i = 0; i < monitorMaxFill; i++) + { + monitorItems[i] = new monitorItem(); + } + } + + public int monitorPut(int slot, int offset, int len) + { + int ret_id = 0; + if (monitorFillLevel < monitorMaxFill) + { + monitorItems[monitorFillLevel].slotNo = slot; + monitorItems[monitorFillLevel].offset = offset; + monitorItems[monitorFillLevel].len = len; + monitorItems[monitorFillLevel].id = (slot << 8) + offset; + ret_id = monitorItems[monitorFillLevel].id; + monitorFillLevel++; + isMonitor = 1; + } + + return ret_id; + } + + public int monitorRemove(int id) + { + int match = 0; + int ret_id = 0; + for (int i = 0; i < monitorMaxFill; i++) + { + if (monitorItems[i].id == id) + { + match = 1; + if (monitorFillLevel > 0) monitorFillLevel--; + ret_id = id; + } + + if (match > 0) + { + if (monitorItems[i].len == 0) return ret_id; + if (i < 31) + { + monitorItems[i].id = monitorItems[i + 1].id; + monitorItems[i].offset = monitorItems[i + 1].offset; + monitorItems[i].slotNo = monitorItems[i + 1].slotNo; + monitorItems[i].len = monitorItems[i + 1].len; + } + else + { + monitorItems[i].id = 0; + monitorItems[i].offset = 0; + monitorItems[i].slotNo = 0; + monitorItems[i].len = 0; + } + } + } + return ret_id; + } + } +} diff --git a/DroneClient/Models/Telemetry.cs b/DroneClient/Models/Telemetry.cs new file mode 100644 index 0000000..f172f15 --- /dev/null +++ b/DroneClient/Models/Telemetry.cs @@ -0,0 +1,340 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Drawing; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace TelemetryIO.Models +{ + public sealed class Telemetry + { + //Класс синглтон для хранения данных телеметрии + private static volatile Telemetry instance; + private static readonly object _lock = new object(); + private Dictionary dictMonitor = new Dictionary();//Словарь адресов, для целей мониторинга + 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; + } + } +} diff --git a/DroneClient/Properties/Resources.Designer.cs b/DroneClient/Properties/Resources.Designer.cs new file mode 100644 index 0000000..2771671 --- /dev/null +++ b/DroneClient/Properties/Resources.Designer.cs @@ -0,0 +1,63 @@ +//------------------------------------------------------------------------------ +// +// Этот код создан программой. +// Исполняемая версия:4.0.30319.42000 +// +// Изменения в этом файле могут привести к неправильной работе и будут потеряны в случае +// повторной генерации кода. +// +//------------------------------------------------------------------------------ + +namespace DroneClient.Properties { + using System; + + + /// + /// Класс ресурса со строгой типизацией для поиска локализованных строк и т.д. + /// + // Этот класс создан автоматически классом 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() { + } + + /// + /// Возвращает кэшированный экземпляр ResourceManager, использованный этим классом. + /// + [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; + } + } + + /// + /// Перезаписывает свойство CurrentUICulture текущего потока для всех + /// обращений к ресурсу с помощью этого класса ресурса со строгой типизацией. + /// + [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)] + internal static global::System.Globalization.CultureInfo Culture { + get { + return resourceCulture; + } + set { + resourceCulture = value; + } + } + } +} diff --git a/DroneClient/Properties/Resources.resx b/DroneClient/Properties/Resources.resx new file mode 100644 index 0000000..1af7de1 --- /dev/null +++ b/DroneClient/Properties/Resources.resx @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + \ No newline at end of file diff --git a/DroneClient/TelemetryServer.cs b/DroneClient/TelemetryServer.cs new file mode 100644 index 0000000..f619039 --- /dev/null +++ b/DroneClient/TelemetryServer.cs @@ -0,0 +1,258 @@ +using DroneClient.Models; +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.IO; +using System.Linq; +using System.Net; +using System.Net.Sockets; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using TelemetryIO; +using TelemetryIO.Models; + +namespace TelemetryIO +{ + internal class TelemetryServer : TelemetryIO.BaseCommHandler + { + ConcurrentQueue sendQueue = new ConcurrentQueue(); + 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); + } + } +} diff --git a/DroneClient/images/connect.ico b/DroneClient/images/connect.ico new file mode 100644 index 0000000..f3d3c3d Binary files /dev/null and b/DroneClient/images/connect.ico differ diff --git a/DroneClient/images/disconnect.ico b/DroneClient/images/disconnect.ico new file mode 100644 index 0000000..37f053f Binary files /dev/null and b/DroneClient/images/disconnect.ico differ diff --git a/connect.png b/connect.png new file mode 100644 index 0000000..23b2df5 Binary files /dev/null and b/connect.png differ diff --git a/disconnect.png b/disconnect.png new file mode 100644 index 0000000..b12a154 Binary files /dev/null and b/disconnect.png differ