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/Drone.cs b/DroneClient/Drone.cs index 324f4ab..632b365 100644 --- a/DroneClient/Drone.cs +++ b/DroneClient/Drone.cs @@ -6,329 +6,426 @@ using System.Runtime.InteropServices; namespace DroneClient { - internal class Drone - { - - public float AccX, AccY, AccZ; - public float GyrX, GyrY, GyrZ; - public uint TimeAcc, TimeGyr; - - public float PosX, PosY; - public float LaserRange; - public uint TimeRange; - - public Vector2 OF = Vector2.Zero; - - public float MotorUL, MotorUR, MotorDL, MotorDR; - - public static byte[] getBytes(object data) + internal class Drone { - int size = Marshal.SizeOf(data); - byte[] arr = new byte[size]; - IntPtr ptr = IntPtr.Zero; - try - { - ptr = Marshal.AllocHGlobal(size); - Marshal.StructureToPtr(data, ptr, true); - Marshal.Copy(ptr, arr, 0, size); - } - finally - { - Marshal.FreeHGlobal(ptr); - } - return arr; - } + public float AccX, AccY, AccZ; + public float GyrX, GyrY, GyrZ; + public uint TimeAcc, TimeGyr; + private static readonly object _readLock = new(); + private static readonly object _writeLock = new(); - public static object fromBytes(byte[] arr, Type type) - { - object mem = new object(); + public float qw, q1, q2, q3; - int size = Marshal.SizeOf(type); - IntPtr ptr = IntPtr.Zero; - try - { - ptr = Marshal.AllocHGlobal(size); + public float PosX, PosY; + public float LaserRange; + public uint TimeRange; - Marshal.Copy(arr, 0, ptr, size); + public Vector2 OF = Vector2.Zero; - mem = Marshal.PtrToStructure(ptr, type); - } - finally - { - Marshal.FreeHGlobal(ptr); - } + public float MotorUL, MotorUR, MotorDL, MotorDR; - return mem; - } - - private byte[] SendDataMotor4() - { - DroneData.DataMotor4 mot4 = new DroneData.DataMotor4(); - - mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4)); - mot4.Head.Mode = DroneData.DataMode.Response; - mot4.Head.Type = DroneData.DataType.DataMotor4; - - mot4.UL = MotorUL; - mot4.UR = MotorUR; - mot4.DL = MotorDL; - mot4.DR = MotorDR; - - return getBytes(mot4); - } - - private byte[]? RecvDataAcc(byte[] data) - { - DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc)); - - AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z; - - TimeAcc= imu.Time; - - return new byte[0]; - } - - private byte[]? RecvDataGyr(byte[] data) - { - DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr)); - - GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z; - - TimeGyr = imu.Time; - - return new byte[0]; - } - - private byte[]? RecvDataRange(byte[] data) - { - DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange)); - - LaserRange = pos.LiDAR; - - TimeRange = pos.Time; - - return new byte[0]; - } - - private byte[]? RecvDataLocal(byte[] data) - { - DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal)); - - PosX = pos.Local.X; PosY = pos.Local.Y; - - return new byte[0]; - } - - private byte[]? RecvDataOF(byte[] data) - { - DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF)); - - OF.X = of.X; - OF.Y = of.Y; - - return new byte[0]; - } - - private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body) - { - byte[] zero = new byte[0]; - - switch (head.Type) - { - case DroneData.DataType.DataAcc: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - // ... // - // - return zero; - } - else - { - // Пришли данные - return RecvDataAcc(body); - } - } - - case DroneData.DataType.DataGyr: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - // ... // - // - return zero; - } - else - { - // Пришли данные - return RecvDataGyr(body); - } - } - - case DroneData.DataType.DataRange: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - // ... // - // - return zero; - } - else - { - // Пришли данные - return RecvDataRange(body); - } - } - - case DroneData.DataType.DataLocal: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - // ... // - // - return zero; - } - else - { - // Пришли данные - return RecvDataLocal(body); - } - } - - case DroneData.DataType.DataOF: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - // ... // - // - return zero; - } - else - { - // Пришли данные - return RecvDataOF(body); - } - } - - case DroneData.DataType.DataMotor4: - { - if (head.Mode == DroneData.DataMode.Request) - { - // Запрос данных - return SendDataMotor4(); - } - else - { - // Пришли данные - // ... // - // - return zero; - } - } - } - - return zero; - } - - private const int DroneStreamCount = 512; - private byte[] DroneStreamData = new byte[DroneStreamCount]; - private int DroneStreamIndex = 0; - private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None }; - - public List? DataStream(byte[]? data, int size) - { - List ret = new List(); - - if (data == null) return ret; // Последовательность не сформирована, ждать данных - - if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец) - - Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size); - DroneStreamIndex += size; - - while (true) - { - if (DroneStreamHead.Size == 0) // Заголовок ещё не получен + public static byte[] getBytes(object data) { - if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать - DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead)); + int size = Marshal.SizeOf(data); + byte[] arr = new byte[size]; + + IntPtr ptr = IntPtr.Zero; + try + { + ptr = Marshal.AllocHGlobal(size); + Marshal.StructureToPtr(data, ptr, true); + Marshal.Copy(ptr, arr, 0, size); + } + finally + { + Marshal.FreeHGlobal(ptr); + } + return arr; } - if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный + public static object fromBytes(byte[] arr, Type type) + { + object mem = new object(); - byte[] body = new byte[DroneStreamHead.Size]; + int size = Marshal.SizeOf(type); + IntPtr ptr = IntPtr.Zero; + try + { + ptr = Marshal.AllocHGlobal(size); - Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size); + Marshal.Copy(arr, 0, ptr, size); - int shift = DroneStreamHead.Size; + mem = Marshal.PtrToStructure(ptr, type); + } + finally + { + Marshal.FreeHGlobal(ptr); + } - DroneStreamIndex -= shift; - Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные + return mem; + } - DroneStreamHead.Size = 0; // Отменяем прошлый заголовок + private byte[] SendDataMotor4() + { + DroneData.DataMotor4 mot4 = new DroneData.DataMotor4(); - ret.Add(ClientRequestResponse(DroneStreamHead, body)); - } + mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4)); + mot4.Head.Mode = DroneData.DataMode.Response; + mot4.Head.Type = DroneData.DataType.DataMotor4; - return ret; + mot4.UL = MotorUL; + mot4.UR = MotorUR; + mot4.DL = MotorDL; + mot4.DR = MotorDR; + + return getBytes(mot4); + } + + public void writeDataMotor4(float UL, float UR, float DL, float DR) + { + lock (_writeLock) + { + MotorUL = UL; + MotorUR = UR; + MotorDL = DL; + MotorDR = DR; + } + } + + private byte[]? RecvDataAcc(byte[] data) + { + DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc)); + lock (_readLock) + { + AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z; + + TimeAcc = imu.Time; + } + + return new byte[0]; + } + + public float[] readDataAcc() + { + lock (_readLock) + { + float[] acc = { AccX, AccY, AccZ, TimeAcc }; + return acc; + } + } + + private byte[]? RecvDataGyr(byte[] data) + { + DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr)); + + lock (_readLock) + { + GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z; + + TimeGyr = imu.Time; + } + + return new byte[0]; + } + + public float[] readDataGyr() + { + lock (_readLock) + { + float[] gyr = { GyrX, GyrY, GyrZ, TimeGyr }; + return gyr; + } + } + + private byte[]? RecvDataRange(byte[] data) + { + DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange)); + lock (_readLock) + { + LaserRange = pos.LiDAR; + + TimeRange = pos.Time; + } + return new byte[0]; + } + + public float[] readDataRange() + { + lock (_readLock) + { + float[] range = { LaserRange, TimeRange }; + return range; + } + } + + private byte[]? RecvDataLocal(byte[] data) + { + DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal)); + lock (_readLock) + { + PosX = pos.Local.X; PosY = pos.Local.Y; + } + return new byte[0]; + } + + public float[] readDataLocal() + { + lock (_readLock) + { + float[] local = { PosX, PosY }; + return local; + } + } + + private byte[]? RecvDataOF(byte[] data) + { + DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF)); + + OF.X = of.X; + OF.Y = of.Y; + + return new byte[0]; + } + + private Vector2 readDataOF() + { + lock (_readLock) + { + return new Vector2(OF.X, OF.Y); + } + } + + private byte[]? RecvDataQuat(byte[] data) + { + DroneData.DataQuat q = (DroneData.DataQuat)fromBytes(data, typeof(DroneData.DataQuat)); + + qw = q.W; q1 = q.X; q2 = q.Y; q3 = q.Z; + + return new byte[0]; + } + + private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body) + { + byte[] zero = new byte[0]; + + switch (head.Type) + { + case DroneData.DataType.DataAcc: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataAcc(body); + } + } + + case DroneData.DataType.DataGyr: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataGyr(body); + } + } + + case DroneData.DataType.DataRange: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataRange(body); + } + } + + case DroneData.DataType.DataLocal: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataLocal(body); + } + } + + case DroneData.DataType.DataOF: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataOF(body); + } + } + + case DroneData.DataType.DataMotor4: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + return SendDataMotor4(); + } + else + { + // Пришли данные + // ... // + // + return zero; + } + } + + case DroneData.DataType.DataQuat: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + return zero; + } + else + { + // Пришли данные + // ... // + // + return RecvDataQuat(body); + } + } + } + + return zero; + } + + private const int DroneStreamCount = 512; + private byte[] DroneStreamData = new byte[DroneStreamCount]; + private int DroneStreamIndex = 0; + private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None }; + + public List? DataStream(byte[]? data, int size) + { + List ret = new List(); + + if (data == null) return ret; // Последовательность не сформирована, ждать данных + + if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец) + + Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size); + DroneStreamIndex += size; + + while (true) + { + if (DroneStreamHead.Size == 0) // Заголовок ещё не получен + { + if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать + DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead)); + } + + if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный + + byte[] body = new byte[DroneStreamHead.Size]; + + Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size); + + int shift = DroneStreamHead.Size; + + DroneStreamIndex -= shift; + Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные + + DroneStreamHead.Size = 0; // Отменяем прошлый заголовок + + ret.Add(ClientRequestResponse(DroneStreamHead, body)); + } + + return ret; + } + + public byte[] SendReqest() + { + DroneData.DataHead acc = new DroneData.DataHead(); + acc.Size = DroneData.DataHead.StrLen; + acc.Mode = DroneData.DataMode.Request; + acc.Type = DroneData.DataType.DataAcc; + + DroneData.DataHead gyr = new DroneData.DataHead(); + gyr.Size = DroneData.DataHead.StrLen; + gyr.Mode = DroneData.DataMode.Request; + gyr.Type = DroneData.DataType.DataGyr; + + DroneData.DataHead range = new DroneData.DataHead(); + range.Size = DroneData.DataHead.StrLen; + range.Mode = DroneData.DataMode.Request; + range.Type = DroneData.DataType.DataRange; + + DroneData.DataHead local = new DroneData.DataHead(); + local.Size = DroneData.DataHead.StrLen; + local.Mode = DroneData.DataMode.Request; + local.Type = DroneData.DataType.DataLocal; + + DroneData.DataHead of = new DroneData.DataHead(); + of.Size = DroneData.DataHead.StrLen; + of.Mode = DroneData.DataMode.Request; + of.Type = DroneData.DataType.DataOF; + + DroneData.DataHead quat = new DroneData.DataHead(); + quat.Size = DroneData.DataHead.StrLen; + quat.Mode = DroneData.DataMode.Request; + quat.Type = DroneData.DataType.DataQuat; + + List list = new List(); + + list.Add(getBytes(acc)); + list.Add(getBytes(gyr)); + list.Add(getBytes(range)); + list.Add(getBytes(local)); + list.Add(getBytes(of)); + list.Add(getBytes(quat)); + list.Add(SendDataMotor4()); + + int count = 0; + + foreach (byte[] d in list) count += d.Length; + + byte[] send = new byte[count]; + + count = 0; + foreach (byte[] d in list) + { + d.CopyTo(send, count); + count += d.Length; + } + + return send; + } } - - public byte[] SendReqest() - { - DroneData.DataHead acc = new DroneData.DataHead(); - acc.Size = DroneData.DataHead.StrLen; - acc.Mode = DroneData.DataMode.Request; - acc.Type = DroneData.DataType.DataAcc; - - DroneData.DataHead gyr = new DroneData.DataHead(); - gyr.Size = DroneData.DataHead.StrLen; - gyr.Mode = DroneData.DataMode.Request; - gyr.Type = DroneData.DataType.DataGyr; - - DroneData.DataHead range = new DroneData.DataHead(); - range.Size = DroneData.DataHead.StrLen; - range.Mode = DroneData.DataMode.Request; - range.Type = DroneData.DataType.DataRange; - - DroneData.DataHead local = new DroneData.DataHead(); - local.Size = DroneData.DataHead.StrLen; - local.Mode = DroneData.DataMode.Request; - local.Type = DroneData.DataType.DataLocal; - - DroneData.DataHead of = new DroneData.DataHead(); - of.Size = DroneData.DataHead.StrLen; - of.Mode = DroneData.DataMode.Request; - of.Type = DroneData.DataType.DataOF; - - List list = new List(); - - list.Add(getBytes(acc)); - list.Add(getBytes(gyr)); - list.Add(getBytes(range)); - list.Add(getBytes(local)); - list.Add(getBytes(of)); - list.Add(SendDataMotor4()); - - int count = 0; - - foreach (byte[] d in list) count += d.Length; - - byte[] send = new byte[count]; - - count = 0; - foreach (byte[] d in list) - { - d.CopyTo(send, count); - count += d.Length; - } - - return send; - } - } } 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/DroneData.cs b/DroneClient/DroneData.cs index d58814b..063ef3b 100644 --- a/DroneClient/DroneData.cs +++ b/DroneClient/DroneData.cs @@ -2,122 +2,130 @@ namespace DroneData { - public enum DataMode : ushort - { - None = 0, Response = 1, Request = 2 - }; + public enum DataMode : ushort + { + None = 0, Response = 1, Request = 2 + }; - public enum DataType : ushort - { - None = 0, Head = 1, + public enum DataType : ushort + { + None = 0, Head = 1, - // Output - DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, + // Output + DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, - // Input - DataMotor4 = 2001, DataMotor6 = 2002, + // Input + DataMotor4 = 2001, DataMotor6 = 2002, - // State - DataQuat = 3001, - }; + // State + DataQuat = 3001, + }; - public struct DataHead - { - public int Size; + public struct DataHead + { + public int Size; - public DataMode Mode; - public DataType Type; + public DataMode Mode; + public DataType Type; - public uint Time; // Общее время + public uint Time; // Общее время - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); + } - public struct XYZ { public float X, Y, Z; } + public struct XYZ { public float X, Y, Z; } - public struct DataAcc - { - public DataHead Head; - public XYZ Acc; + public struct DataAcc + { + public DataHead Head; + public XYZ Acc; - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc)); + } - public struct DataGyr - { - public DataHead Head; - public XYZ Gyr; + public struct DataGyr + { + public DataHead Head; + public XYZ Gyr; - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr)); + } - public struct DataMag - { - public DataHead Head; - public XYZ Mag; + public struct DataMag + { + public DataHead Head; + public XYZ Mag; - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag)); + } - public struct DataRange - { - public DataHead Head; - public float LiDAR; // Датчик посадки + public struct DataRange + { + public DataHead Head; + public float LiDAR; // Датчик посадки - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange)); + } - public struct DataLocal - { - public DataHead Head; - public XYZ Local; // Локальные координаты + public struct DataLocal + { + public DataHead Head; + public XYZ Local; // Локальные координаты - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal)); + } - public struct DataBar - { - public DataHead Head; - public float Pressure; // Давление + public struct DataBar + { + public DataHead Head; + public float Pressure; // Давление - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); + } - public struct DataOF - { - public DataHead Head; - public float X, Y; // Угловой сдвиг + public struct DataOF + { + public DataHead Head; + public float X, Y; // Угловой сдвиг - public uint Time; // Последнее время изменения данных + public uint Time; // Последнее время изменения данных - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF)); + } - public struct DataMotor4 - { - public DataHead Head; - public float UL, UR, DL, DR; + public struct DataMotor4 + { + public DataHead Head; + public float UL, UR, DL, DR; - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4)); + } - public struct DataMotor6 - { - public DataHead Head; - public float UL, UR, LL, RR, DL, DR; + public struct DataMotor6 + { + public DataHead Head; + public float UL, UR, LL, RR, DL, DR; - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); + } + + public struct DataQuat + { + public DataHead Head; + public float X, Y, Z, W; + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat)); + } } \ No newline at end of file diff --git a/DroneClient/FormMain.Designer.cs b/DroneClient/FormMain.Designer.cs index 4604a5d..16f174f 100644 --- a/DroneClient/FormMain.Designer.cs +++ b/DroneClient/FormMain.Designer.cs @@ -20,588 +20,760 @@ 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(); + label15 = new Label(); + label16 = new Label(); + label_myq = new Label(); + label_simq = new Label(); + RC_roll_track = new TrackBar(); + RC_roll_label = new Label(); + RC_pitch_track = new TrackBar(); + RC_pitch_label = 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(); + ((System.ComponentModel.ISupportInitialize)RC_roll_track).BeginInit(); + ((System.ComponentModel.ISupportInitialize)RC_pitch_track).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.Name = "groupBox1"; + groupBox1.Size = new Size(752, 80); + groupBox1.TabIndex = 3; + groupBox1.TabStop = false; + groupBox1.Tag = ""; + groupBox1.Text = "Server"; + // + // TeleClientStatusPicture + // + TeleClientStatusPicture.Anchor = AnchorStyles.Top | AnchorStyles.Bottom | AnchorStyles.Left; + TeleClientStatusPicture.Location = new Point(408, 38); + TeleClientStatusPicture.Margin = new Padding(3, 2, 3, 2); + TeleClientStatusPicture.Name = "TeleClientStatusPicture"; + TeleClientStatusPicture.Size = new Size(21, 18); + TeleClientStatusPicture.TabIndex = 9; + TeleClientStatusPicture.TabStop = false; + // + // label12 + // + label12.AutoSize = true; + label12.Location = new Point(344, 38); + label12.Name = "label12"; + label12.Size = new Size(58, 15); + label12.TabIndex = 8; + label12.Tag = "#clients_port"; + label12.Text = "TeleClient"; + // + // button1 + // + button1.BackColor = Color.Transparent; + button1.Location = new Point(323, 12); + button1.Name = "button1"; + button1.Size = new Size(111, 23); + button1.TabIndex = 7; + button1.Tag = ""; + button1.Text = "Run TeleServer"; + button1.UseVisualStyleBackColor = false; + button1.Click += button1_Click; + // + // TeleServerPortCtrl + // + TeleServerPortCtrl.Location = new Point(255, 14); + 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(62, 23); + TeleServerPortCtrl.TabIndex = 6; + TeleServerPortCtrl.Value = new decimal(new int[] { 8888, 0, 0, 0 }); + // + // label8 + // + label8.AutoSize = true; + label8.Location = new Point(217, 16); + label8.Name = "label8"; + label8.Size = new Size(32, 15); + label8.TabIndex = 5; + label8.Tag = "#clients_port"; + label8.Text = "Port:"; + // + // 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"; + // + // label15 + // + label15.AutoSize = true; + label15.Font = new Font("Segoe UI", 11F); + label15.Location = new Point(11, 459); + label15.Name = "label15"; + label15.Size = new Size(108, 20); + label15.TabIndex = 27; + label15.Text = "SimQuaternion"; + // + // label16 + // + label16.AutoSize = true; + label16.Font = new Font("Segoe UI", 11F); + label16.Location = new Point(12, 497); + label16.Name = "label16"; + label16.Size = new Size(103, 20); + label16.TabIndex = 28; + label16.Text = "MyQuaternion"; + // + // label_myq + // + label_myq.AutoSize = true; + label_myq.Font = new Font("Segoe UI", 11F); + label_myq.Location = new Point(138, 497); + label_myq.Name = "label_myq"; + label_myq.Size = new Size(103, 20); + label_myq.TabIndex = 30; + label_myq.Text = "MyQuaternion"; + // + // label_simq + // + label_simq.AutoSize = true; + label_simq.Font = new Font("Segoe UI", 11F); + label_simq.Location = new Point(137, 459); + label_simq.Name = "label_simq"; + label_simq.Size = new Size(108, 20); + label_simq.TabIndex = 29; + label_simq.Text = "SimQuaternion"; + // + // RC_roll_track + // + RC_roll_track.Location = new Point(502, 226); + RC_roll_track.Maximum = 30; + RC_roll_track.Minimum = -30; + RC_roll_track.Name = "RC_roll_track"; + RC_roll_track.Size = new Size(200, 45); + RC_roll_track.TabIndex = 31; + RC_roll_track.Scroll += RC_roll_track_Scroll; + // + // RC_roll_label + // + RC_roll_label.AutoSize = true; + RC_roll_label.BackColor = Color.White; + RC_roll_label.Font = new Font("Segoe UI", 11.25F, FontStyle.Regular, GraphicsUnit.Point, 204); + RC_roll_label.Location = new Point(522, 203); + RC_roll_label.Name = "RC_roll_label"; + RC_roll_label.Size = new Size(50, 20); + RC_roll_label.TabIndex = 32; + RC_roll_label.Text = "Roll: 0"; + RC_roll_label.TextAlign = ContentAlignment.MiddleLeft; + RC_roll_label.Click += RC_roll_label_Click; + // + // RC_pitch_track + // + RC_pitch_track.Location = new Point(502, 308); + RC_pitch_track.Maximum = 30; + RC_pitch_track.Minimum = -30; + RC_pitch_track.Name = "RC_pitch_track"; + RC_pitch_track.Size = new Size(200, 45); + RC_pitch_track.TabIndex = 33; + RC_pitch_track.Scroll += RC_pitch_track_Scroll; + // + // RC_pitch_label + // + RC_pitch_label.AutoSize = true; + RC_pitch_label.BackColor = Color.White; + RC_pitch_label.Font = new Font("Segoe UI", 11.25F, FontStyle.Regular, GraphicsUnit.Point, 204); + RC_pitch_label.Location = new Point(522, 285); + RC_pitch_label.Name = "RC_pitch_label"; + RC_pitch_label.Size = new Size(56, 20); + RC_pitch_label.TabIndex = 34; + RC_pitch_label.Text = "Pitch: 0"; + RC_pitch_label.TextAlign = ContentAlignment.MiddleLeft; + RC_pitch_label.Click += RC_pitch_label_Click; + // + // Form_Main + // + AutoScaleDimensions = new SizeF(7F, 15F); + AutoScaleMode = AutoScaleMode.Font; + ClientSize = new Size(752, 533); + Controls.Add(RC_pitch_label); + Controls.Add(RC_pitch_track); + Controls.Add(RC_roll_label); + Controls.Add(RC_roll_track); + Controls.Add(label_myq); + Controls.Add(label_simq); + Controls.Add(label16); + Controls.Add(label15); + 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, 387); + 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(); + ((System.ComponentModel.ISupportInitialize)RC_roll_track).EndInit(); + ((System.ComponentModel.ISupportInitialize)RC_pitch_track).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 +820,18 @@ 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; + private Label label15; + private Label label16; + private Label label_myq; + private Label label_simq; + private TrackBar RC_roll_track; + private Label RC_roll_label; + private TrackBar RC_pitch_track; + private Label RC_pitch_label; + } } diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index f33d230..4ff928f 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -3,170 +3,357 @@ using System.Numerics; using System.Windows.Forms; using static DroneSimulator.NetClient; using DroneClient; +using TelemetryIO.Models; +using TelemetryIO; +using DroneClient.Models; +using DroneClient.utils; +using System; +using Vector = DroneClient.utils.Vector; 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"); + RC rc = new(); - 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; + } + + Task.Run(() => DroneOrientation.CalcAttitude(dataDrone, rc)); + } + + 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(); + + label_simq.Text = $"({dataDrone.qw}; {dataDrone.q1}; {dataDrone.q2}; {dataDrone.q3})"; + label_myq.Text = DroneOrientation.GetAttitude().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; + rc.SetThrottle(pow); + 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 RC_roll_track_Scroll(object sender, EventArgs e) + { + rc.SetRoll(RC_roll_track.Value * Vector.TO_RAD); + float hue = 0; + float sat = 1f; + float bri = 0; + RC_roll_label.Text = $"Roll: {RC_roll_track.Value}"; + if(RC_roll_track.Value > 0) + { + hue = 0; + sat = RC_roll_track.Value * 0.033f; + bri = 1f; + RC_roll_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri); + } + else if(RC_roll_track.Value < 0) + { + hue = 200; + sat = -RC_roll_track.Value * 0.033f; + bri = 1f; + RC_roll_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri); + } + else + { + RC_roll_label.BackColor = Color.White; + } + } + + private void RC_pitch_track_Scroll(object sender, EventArgs e) + { + rc.SetPitch(RC_pitch_track.Value * Vector.TO_RAD); + float hue = 0; + float sat = 1f; + float bri = 0; + RC_pitch_label.Text = $"Pitch: {RC_pitch_track.Value}"; + if (RC_pitch_track.Value > 0) + { + hue = 0; + sat = RC_pitch_track.Value * 0.033f; + bri = 1f; + RC_pitch_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri); + } + else if (RC_pitch_track.Value < 0) + { + hue = 200; + sat = -RC_pitch_track.Value * 0.033f; + bri = 1f; + RC_pitch_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri); + } + else + { + RC_pitch_label.BackColor = Color.White; + } + } + + private void RC_roll_label_Click(object sender, EventArgs e) + { + rc.SetRoll(RC_roll_track.Value * Vector.TO_RAD); + RC_roll_track.Value = 0; + RC_roll_label.Text = $"Roll: {RC_roll_track.Value}"; + RC_roll_label.BackColor = Color.White; + } + + private void RC_pitch_label_Click(object sender, EventArgs e) + { + rc.SetPitch(RC_pitch_track.Value * Vector.TO_RAD); + RC_pitch_track.Value = 0; + RC_pitch_label.Text = $"Pitch: {RC_pitch_track.Value}"; + RC_pitch_label.BackColor = Color.White; } - } - 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..60af270 --- /dev/null +++ b/DroneClient/Models/Telemetry.cs @@ -0,0 +1,354 @@ +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[] quaternion_target = 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("Qw_target", new VarAddress(QT_ADDRESS, 0)); + dictMonitor.Add("QTx_target", new VarAddress(QT_ADDRESS, 1)); + dictMonitor.Add("Qy_target", new VarAddress(QT_ADDRESS, 2)); + dictMonitor.Add("Qz_target", new VarAddress(QT_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 QT_ADDRESS://quaternion + set_float(quaternion_target, 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 QT_ADDRESS://quaternion target + return get_float(quaternion_target, 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 QT_ADDRESS = 12; + 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/DroneClient/utils/AdaptivePID.cs b/DroneClient/utils/AdaptivePID.cs new file mode 100644 index 0000000..fc22e3c --- /dev/null +++ b/DroneClient/utils/AdaptivePID.cs @@ -0,0 +1,88 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace DroneClient.utils +{ + internal class AdaptivePID + { + //Initial + private float P = 1; + private float I = 0.2f; + private float D = 0.005f; + + //Adaptive + private float aP = 1; + private float aI = 0.2f; + private float aD = 0.005f; + + private float P_min = 0; private float P_max = 10; + private float I_min = 0; private float I_max = 1; + private float D_min = 0; private float D_max = 0.1f; + + private float alpha = 0.001f; + private float beta = 0.001f; + private float gamma = 0.0005f; + + private float prev_error = float.NaN; + private float integral = 0; + + public AdaptivePID() + { + alpha = 0; beta = 0; gamma = 0; + } + + public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f) + { + P = p; aP = p; + I = i; aI = i; + D = d; aD = d; + this.alpha = alpha; + this.beta = beta; + this.gamma = gamma; + } + + public float process(float error, float T) + { + if (T <= 0) return 0; + integral += error * T; + float derivative = 0; + if (float.IsFinite(prev_error)) + { + derivative = (error - prev_error) / T; + } + prev_error = error; + + float dP = -alpha * error * error; + float dI = -beta * error * integral; + float dD = -gamma * error * derivative; + + aP += dP; aI += dI; aD += dD; + + aP = constrain(aP, P_min, P_max); + aI = constrain(aI, I_min, I_max); + aD = constrain(aD, D_min, D_max); + integral = constrain(integral, -10, 10); + float output = aP * error + aI * integral + aD * derivative; + + return output; + } + + public void reset() + { + integral = 0; + prev_error = 0; + aP = P; aI = I; aD = D; + } + + static float constrain(float val, float min, float max) + { + if (min >= max) return val; + if(val < min) return min; + if(val > max) return max; + return val; + } + } +} diff --git a/DroneClient/utils/DroneOrientation.cs b/DroneClient/utils/DroneOrientation.cs new file mode 100644 index 0000000..7d8fe3d --- /dev/null +++ b/DroneClient/utils/DroneOrientation.cs @@ -0,0 +1,198 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Text; +using System.Threading.Tasks; +using TelemetryIO.Models; + +namespace DroneClient.utils +{ + internal static class DroneOrientation + { + public static float AccX, AccY, AccZ; + public static float GyrX, GyrY, GyrZ; + public static uint TimeAcc, TimeGyr; + private static bool isFirstCycle = true; + public static uint prevTimeAcc, prevTimeGyr; + private static Telemetry telemetry; + + public static float PosX, PosY; + public static float LaserRange; + public static uint TimeRange; + public static uint prevTimeRange; + + public static Vector2 OF = Vector2.Zero; + + public static float MotorUL, MotorUR, MotorDL, MotorDR; + + public const float K_ACC = 0.1f; + + public static Quaternion attitude = new(); + + /***************************************************************/ + private static Quaternion attitudeTarget = new(); + private static Vector ratesTarget = new(); + private static Vector torqueTarget = new(); + private static float thrustTarget = 0; + /***************************************************************/ + private static AdaptivePID RollPid = new(); + private static AdaptivePID PitchPid = new(); + + //private static AdaptivePID YawPid = new(); + + private static AdaptivePID RatesRollPid = new(telemetry.pid0[0], telemetry.pid0[1], telemetry.pid0[2], 0.0f, 0, 0); + private static AdaptivePID RatesPitchPid = new(telemetry.pid0[3], telemetry.pid0[4], telemetry.pid0[5], 0.01f, 0.05f, 0.001f); + private static AdaptivePID RatesYawPid = new(telemetry.pid0[6], telemetry.pid0[7], telemetry.pid0[8], 0.01f, 0.05f, 0.001f); + /***************************************************************/ + private static float tim1s = 0; + + + /// + /// Вычисление текущей ориентации дрона + /// + public static void CalcAttitude(Drone droneData, RC rc) + { + readData(droneData); + + prevTimeAcc = TimeAcc; + prevTimeGyr = TimeGyr; + + + prevTimeRange = TimeRange; + while (true) + { + readData(droneData); + if(prevTimeGyr == 0) + { + prevTimeGyr = TimeGyr; + continue; + } + float gyro_dt = TimeGyr - prevTimeGyr; + tim1s += gyro_dt; + if ((prevTimeGyr > 0) && (gyro_dt > 0)) + { + gyro_dt /= 1000; + thrustTarget = rc.GetThrottle(); + attitudeTarget = Quaternion.FromEulerAngles(rc.GetControl()); + Vector rates = new Vector(-GyrY, -GyrX, GyrZ); + //Vector rates = new Vector(0, 0, 0); + rates *= Vector.TO_RAD; + //Quaternion q_rates = Quaternion.FromRotationVector(rates * (gyro_dt)); + //attitude = Quaternion.Mul(attitude, q_rates); + attitude.UpdateByRates(rates, gyro_dt); + attitude.Normalize(); + Vector errorPos = CalculateErrorAxisAngle(); + + if (tim1s > 1000) + { + //Debug.WriteLine($"Flix = {CalculateErrorFlix().ToString()}"); + Debug.WriteLine($"Axis = {CalculateErrorAxisAngle().ToString()}"); + Debug.WriteLine($"Current = {attitude.ToString()}"); + Debug.WriteLine($"Target = {attitudeTarget.ToString()}"); + tim1s = 0; + } + //ratesTarget.X = RollPid.process(errorPos.X, gyro_dt); + //ratesTarget.Y = PitchPid.process(errorPos.Y, gyro_dt); + + //Vector errorRates = ratesTarget - rates; + + torqueTarget.X = RatesRollPid.process(errorPos.X, gyro_dt); + torqueTarget.Y = RatesPitchPid.process(errorPos.Y, gyro_dt); + torqueTarget.Z = RatesYawPid.process(errorPos.Z, gyro_dt); + //torqueTarget.X = RatesRollPid.process(errorRates.X, gyro_dt); + //torqueTarget.Y = RatesPitchPid.process(errorRates.Y, gyro_dt); + //torqueTarget.Z = RatesYawPid.process(errorRates.Z, gyro_dt); + + /*MotorUL = thrustTarget + torqueTarget.X - torqueTarget.Y + torqueTarget.Z; + MotorUR = thrustTarget - torqueTarget.X - torqueTarget.Y - torqueTarget.Z; + MotorDL = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z; + MotorDR = thrustTarget - torqueTarget.X + torqueTarget.Y + torqueTarget.Z;*/ + MotorUL = thrustTarget - torqueTarget.X - torqueTarget.Y + torqueTarget.Z; + MotorUR = thrustTarget + torqueTarget.X - torqueTarget.Y - torqueTarget.Z; + MotorDL = thrustTarget - torqueTarget.X + torqueTarget.Y - torqueTarget.Z; + MotorDR = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z; + + MotorUL = (float)Vector.constraint(MotorUL, 0, 1); + MotorUR = (float)Vector.constraint(MotorUR, 0, 1); + MotorDL = (float)Vector.constraint(MotorDL, 0, 1); + MotorDR = (float)Vector.constraint(MotorDR, 0, 1); + + droneData.writeDataMotor4(MotorUL, MotorUR, MotorDL, MotorDR); + + prevTimeGyr = TimeGyr; + } + + + + /* + if (TimeAcc - prevTimeAcc > 0) + { + float AccNorm = (float)Math.Sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ); + if (Math.Abs(AccNorm - 1) < 0.05) + { + //Получение кватерниона, описывающего вращение вектора Z(0, 0, 1) в положение вектора ускорений + Quaternion accRotation = Quaternion.QuaternionBetweenVectors(new Vector(AccX, AccY, AccZ), new Vector(0, 0, 1)); + + //Коррекция по акселерометру + //accRotation *= K_ACC; + attitude = accRotation; + attitude.Normalize(); + } + prevTimeAcc = TimeAcc; + }*/ + } + } + + public static Quaternion GetAttitude() + { + return Quaternion.Copy(attitude); + } + + private static void CalculateControls(float throttle, float phi, float theta, float psi) + { + thrustTarget = throttle; + attitudeTarget = Quaternion.FromEulerAngles(new Vector(phi, theta, psi)); + } + + /// + /// Вычисление ошибки как в flix + /// + private static Vector CalculateErrorFlix() + { + Vector up = new Vector(0, 0, 1); + Vector current = Quaternion.RotateVector(up, attitude); + Vector target = Quaternion.RotateVector(up, attitudeTarget); + return Vector.getRotationVector(target, current); + } + + /// + /// Вычисление ошибки через форму ось-угол + /// + private static Vector CalculateErrorAxisAngle() + { + Quaternion error = Quaternion.Mul(attitudeTarget, attitude.getInverse()); + error.Normalize(); + float angle = 2 * (float)Math.Acos(error.W); + Vector n = new Vector(error.X, error.Y, error.Z); + n.Normalize(); + return n*angle; + } + + private static void readData(Drone droneData) + { + float[]acc = droneData.readDataAcc(); + AccX = acc[0]; AccY = acc[1]; AccZ = acc[2]; TimeAcc = (uint)acc[3]; + + float[] gyr = droneData.readDataGyr(); + GyrX = gyr[0]; GyrY = gyr[1]; GyrZ = gyr[2]; TimeGyr = (uint)gyr[3]; + + float[] pos = droneData.readDataLocal(); + PosX = pos[0]; PosY = pos[1]; + + float[] range = droneData.readDataRange(); + LaserRange = range[0]; TimeRange = (uint)range[1]; + } + } +} diff --git a/DroneClient/utils/Quaternion.cs b/DroneClient/utils/Quaternion.cs new file mode 100644 index 0000000..3e833ab --- /dev/null +++ b/DroneClient/utils/Quaternion.cs @@ -0,0 +1,178 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Text; +using System.Threading.Tasks; + +namespace DroneClient.utils +{ + internal class Quaternion + { + private float w; + private float x; + private float y; + private float z; + + public float W + { + get => w; + private set => w = value; + } + public float X + { + get => x; + private set => x = value; + } + public float Y + { + get => y; + private set => y = value; + } + public float Z + { + get => z; + private set => z = value; + } + + public Quaternion() + { + w = 1; + x = y = z = 0; + } + + public Quaternion(float w, float x, float y, float z) + { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public bool IsZero() + { + return w == 0 && x == 0 && y == 0 && z == 0; + } + + public float getNorm() + { + if (IsZero()) return 0; + return (float)Math.Sqrt(w * w + x * x + y * y + z * z); + } + + public void Normalize() + { + float norm = getNorm(); + if (norm != 0) + { + w /= norm; + x /= norm; + y /= norm; + z /= norm; + } + } + + static public Quaternion FromRotationVector(Vector Va) + { + float a = Va.getNorm(); + if (a == 0) + { + return new Quaternion(); + } + + Quaternion w = new Quaternion(); + float cos = (float)Math.Cos(a / 2); + float sin = (float)Math.Sin(a / 2); + + w.w = cos; + w.x = Va.X / a * sin; + w.y = Va.Y / a * sin; + w.z = Va.Z / a * sin; + return w; + } + + static public Quaternion FromEulerAngles(Vector euler) + { + float halfphi = euler.X / 2; + float halftheta = euler.Y / 2; + float halfpsi= euler.Z / 2; + float cosx = (float)Math.Cos(halfphi); + float sinx = (float)Math.Sin(halfphi); + float cosy = (float)Math.Cos(halftheta); + float siny = (float)Math.Sin(halftheta); + float cosz = (float)Math.Cos(halfpsi); + float sinz = (float)Math.Sin(halfpsi); + + return new Quaternion( + cosx * cosy * cosz + sinx * siny * sinz, + sinx * cosy * cosz - cosx * siny * sinz, + cosx * siny * cosz + sinx * cosy * sinz, + cosx * cosy * sinz - sinx * siny * cosz); + } + + static public Quaternion Copy(Quaternion q) + { + return new Quaternion(q.w, q.x, q.y, q.z); + } + + public Quaternion getInverse() + { + float n = getNorm(); + return new Quaternion(w / n, -x / n, -y / n, -z / n); + } + + static public Quaternion Mul(Quaternion q1, Quaternion q2) + { + Quaternion res = new Quaternion(); + res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; + res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y; + res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z; + res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x; + return res; + } + + public void UpdateByRates(Vector w, float dt) + { + Quaternion omega = new Quaternion(0, w.X, w.Y, w.Z); + Quaternion res = Mul(this, omega); + this.w += res.w * 0.5f * dt; + this.x += res.x * 0.5f * dt; + this.y += res.y * 0.5f * dt; + this.z += res.z * 0.5f * dt; + } + + static public Vector RotateVector(Vector a, Quaternion q) + { + Quaternion fromVector = new Quaternion(0, a.X, a.Y, a.Z); + Quaternion inverse = q.getInverse(); + Quaternion r = Mul(q, fromVector); + r = Mul(r, inverse); + return new Vector(r.x, r.y, r.z); + } + static public Quaternion QuaternionBetweenVectors(Vector a, Vector b) + { + Quaternion res = new Quaternion(); + Vector mul_v = Vector.Mul_Vector(a, b); + //Если векторы коллинеарны, то нет поворота + if (mul_v.IsZero()) return res; + + float mul_s = Vector.Mul_Scalar(a, b); + //Хотя бы один из векторов нулевой => кватернион неопределен, но мы возвращаем (1, 0, 0, 0) + if (mul_s == 0) return res; + + float norm_scalar = a.getNorm() * b.getNorm() + mul_s; + float normQ = (float)Math.Sqrt(norm_scalar * norm_scalar + mul_v.getNorm() * mul_v.getNorm()); + + res.w = norm_scalar / normQ; + res.x = mul_v.X / normQ; + res.y = mul_v.Y / normQ; + res.z = mul_v.Z / normQ; + return res; + } + + override public string ToString() + { + return $"({w}, {x}, {y}, {z})"; + } + } +} diff --git a/DroneClient/utils/RC.cs b/DroneClient/utils/RC.cs new file mode 100644 index 0000000..758b03a --- /dev/null +++ b/DroneClient/utils/RC.cs @@ -0,0 +1,45 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace DroneClient.utils +{ + internal class RC + { + private Vector control = new(); + private float throttle = 0; + + public void SetControl(float roll, float pitch, float yaw) + { + control.X = roll; + control.Y = pitch; + control.Z = yaw; + } + + public void SetRoll(float roll) + { + control.X = roll; + } + + public void SetPitch(float pitch) + { + control.Y = pitch; + } + + public void SetYaw(float yaw) + { + control.Z = yaw; + } + + public void SetThrottle(float throttle) + { + this.throttle = throttle; + } + + public Vector GetControl() { return control; } + + public float GetThrottle() { return throttle; } + } +} diff --git a/DroneClient/utils/UtilityUI.cs b/DroneClient/utils/UtilityUI.cs new file mode 100644 index 0000000..c1e07c3 --- /dev/null +++ b/DroneClient/utils/UtilityUI.cs @@ -0,0 +1,74 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace DroneClient.utils +{ + static class UtilityUI + { + public static Color ColorFromHsb(float hue, float saturation, float brightness) + { + // Ограничиваем значения параметров + hue = Math.Clamp(hue, 0, 360); + saturation = Math.Clamp(saturation, 0, 1); + brightness = Math.Clamp(brightness, 0, 1); + + float c = brightness * saturation; + float x = c * (1 - Math.Abs((hue / 60) % 2 - 1)); + float m = brightness - c; + + float r, g, b; + + if (hue < 60) { r = c; g = x; b = 0; } + else if (hue < 120) { r = x; g = c; b = 0; } + else if (hue < 180) { r = 0; g = c; b = x; } + else if (hue < 240) { r = 0; g = x; b = c; } + else if (hue < 300) { r = x; g = 0; b = c; } + else { r = c; g = 0; b = x; } + + return Color.FromArgb( + (int)((r + m) * 255), + (int)((g + m) * 255), + (int)((b + m) * 255)); + } + + public static void ColorToHsb(Color color, out float hue, out float saturation, out float brightness) + { + float r = color.R / 255f; + float g = color.G / 255f; + float b = color.B / 255f; + + float max = Math.Max(r, Math.Max(g, b)); + float min = Math.Min(r, Math.Min(g, b)); + float delta = max - min; + + // Hue calculation + if (delta == 0) + { + hue = 0; + } + else if (max == r) + { + hue = 60 * (((g - b) / delta) % 6); + } + else if (max == g) + { + hue = 60 * (((b - r) / delta) + 2); + } + else + { + hue = 60 * (((r - g) / delta) + 4); + } + + if (hue < 0) hue += 360; + + // Saturation calculation + saturation = max == 0 ? 0 : delta / max; + + // Brightness/Value + brightness = max; + } + } +} diff --git a/DroneClient/utils/Vector.cs b/DroneClient/utils/Vector.cs new file mode 100644 index 0000000..e4bd7af --- /dev/null +++ b/DroneClient/utils/Vector.cs @@ -0,0 +1,148 @@ +using System; +using System.Collections.Generic; +using System.Data; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows.Forms; + +namespace DroneClient.utils +{ + internal class Vector + { + public const float TO_GRAD = (float)(180 / Math.PI); + public const float TO_RAD = (float)(Math.PI / 180); + private float x, y, z; + + public float X + { + get => x; + set => x = value; + } + + public float Y + { + get => y; + set => y = value; + } + + public float Z + { + get => z; + set => z = value; + } + + public Vector() { } + + public Vector(float x, float y, float z) + { + this.x = x; + this.y = y; + this.z = z; + } + + public bool IsZero() + { + return x == 0 && y == 0 && z == 0; + } + + public float getNorm() + { + if (IsZero()) return 0; + return (float)Math.Sqrt(x * x + y * y + z * z); + } + + public void Normalize() + { + if (IsZero()) return; + float norm = getNorm(); + x /= norm; + y /= norm; + z /= norm; + } + + static public Vector operator *(float a, Vector v) + { + return new Vector(a * v.x, a * v.y, a * v.z); + } + + static public Vector operator *(Vector v, float a) + { + return new Vector(a * v.x, a * v.y, a * v.z); + } + + static public Vector operator -(Vector a, Vector b) + { + return new Vector(a.x - b.x, a.y - b.y, a.z - b.z); + } + + static public float Mul_Scalar(Vector a, Vector b) + { + return a.x * b.x + a.y * b.y + a.z * b.z; + } + + static public Vector Mul_Vector(Vector a, Vector b) + { + return new Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); + } + + static public float AngleBetweenVectors(Vector a, Vector b) + { + float mul_scalar = Mul_Scalar(a, b); + float normA = a.getNorm(); + float normB = b.getNorm(); + + if ((normA == 0) || (normB == 0)) + { + return 0; + } + else + { + return (float)Math.Acos(Vector.constraint(mul_scalar / normA / normB)); + } + } + + static public float AngleBetweenVectors2(Vector a, Vector b) + { + float normAB = Mul_Vector(a, b).getNorm(); + float normA = a.getNorm(); + float normB = b.getNorm(); + + if ((normA == 0) || (normB == 0)) + { + return 0; + } + else + { + return (float)Math.Asin(Vector.constraint(normAB / normA / normB)); + } + } + + static public Vector getRotationVector(Vector a, Vector b) + { + Vector n = Vector.Mul_Vector(a, b); + if (n.IsZero()) + { + return Vector.Mul_Vector(a, new Vector(1, 0, 0)); + } + n.Normalize(); + return Vector.AngleBetweenVectors(a, b) * n; + } + + /// + ///Ограничивает value в пределах между low и high. Если low >= high будет возвращено value. + /// + static public double constraint(double value, double low = -1, double high = 1) + { + if (low >= high) return value; + if (value < low) return low; + if (value > high) return high; + return value; + } + + override public string ToString() + { + return $"({x}, {y}, {z})"; + } + } +} 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