diff --git a/DroneClient/BaseCommHandler.cs b/DroneClient/BaseCommHandler.cs deleted file mode 100644 index d5bbbf3..0000000 --- a/DroneClient/BaseCommHandler.cs +++ /dev/null @@ -1,478 +0,0 @@ -using System; -using System.Collections.Concurrent; -using System.Collections.Generic; -using System.Diagnostics; -using System.Linq; -using System.Text; -using System.Threading; -using System.Threading.Tasks; -using TelemetryIO.Models; - -namespace TelemetryIO -{ - public interface iCommParams - { - - } - - internal class TCPCommParams : iCommParams - { - public string IP = ""; - public int Port = 0; - public TCPCommParams(string addr, int port) - { - this.IP = addr; - this.Port = port; - } - } - - public class SerialCommParams : iCommParams - { - public string PortName = ""; - public int BaudRate = 9600; - public SerialCommParams(string portName, int baudRate) - { - PortName = portName; - BaudRate = baudRate; - } - } - - public abstract class BaseCommHandler - { - public const int TELE_CMD_RD_ONCE = 1; - public const int TELE_CMD_RD_MON_ON = 2; - public const int TELE_CMD_RD_MON_OFF = 3; - public const int TELE_CMD_RD_MON_ADD = 4; - public const int TELE_CMD_RD_MON_REMOVE = 5; - public const int TELE_CMD_RD_MON_REMOVEALL = 6; - public const int TELE_CMD_WR = 10; - public const int TELE_CMD_MOTORS_CTRL = 100; - public const int TELE_CMD_ABORT = 999; - public const int TELE_CMD_HELLO = 9999; - public const byte CRC8_POLYNOMIAL = 0x07; - public const int CRC32_POLY = 0x04C11DB7; - public const byte ESCAPE_BEGIN = 0xBE; - public const byte ESCAPE_END = 0xED; - public const byte ESCAPE_CHAR = 0x0E; - - - private volatile bool _isReading = true; - private readonly ConcurrentQueue 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 632b365..324f4ab 100644 --- a/DroneClient/Drone.cs +++ b/DroneClient/Drone.cs @@ -6,426 +6,329 @@ using System.Runtime.InteropServices; namespace DroneClient { - internal class Drone + 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) { - - 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 float qw, q1, q2, q3; - - 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) - { - 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 static object fromBytes(byte[] arr, Type type) - { - object mem = new object(); - - int size = Marshal.SizeOf(type); - IntPtr ptr = IntPtr.Zero; - try - { - ptr = Marshal.AllocHGlobal(size); - - Marshal.Copy(arr, 0, ptr, size); - - mem = Marshal.PtrToStructure(ptr, type); - } - finally - { - Marshal.FreeHGlobal(ptr); - } - - 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); - } - - 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; - } + 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 static object fromBytes(byte[] arr, Type type) + { + object mem = new object(); + + int size = Marshal.SizeOf(type); + IntPtr ptr = IntPtr.Zero; + try + { + ptr = Marshal.AllocHGlobal(size); + + Marshal.Copy(arr, 0, ptr, size); + + mem = Marshal.PtrToStructure(ptr, type); + } + finally + { + Marshal.FreeHGlobal(ptr); + } + + 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) // Заголовок ещё не получен + { + 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; + + 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 1654505..c27cd77 100644 --- a/DroneClient/DroneClient.csproj +++ b/DroneClient/DroneClient.csproj @@ -8,28 +8,4 @@ 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 063ef3b..d58814b 100644 --- a/DroneClient/DroneData.cs +++ b/DroneClient/DroneData.cs @@ -2,130 +2,122 @@ 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)); - } - - public struct DataQuat - { - public DataHead Head; - public float X, Y, Z, W; - - static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat)); - } + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); + } } \ No newline at end of file diff --git a/DroneClient/FormMain.Designer.cs b/DroneClient/FormMain.Designer.cs index 16f174f..4604a5d 100644 --- a/DroneClient/FormMain.Designer.cs +++ b/DroneClient/FormMain.Designer.cs @@ -20,760 +20,588 @@ 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(); - 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(); - } + /// + /// 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(); + } - #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; @@ -820,18 +648,5 @@ 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 4ff928f..f33d230 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -3,357 +3,170 @@ 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 + public partial class Form_Main : Form + { + private NetClient netClient = new NetClient(); + + public Form_Main() { - 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(); - - public Form_Main() - { - InitializeComponent(); - tele_server = new TelemetryServer(); - if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); - } - - private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e) - { - if (TeleClientStatusPicture.InvokeRequired) - { - TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); })); - } - else - { - if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); - } - monitoring.resetMonitorContainer(); - } - - private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e) - { - if (TeleClientStatusPicture.InvokeRequired) - { - TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); })); - } - else - { - if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); - } - } - - private void MonitoringTimer_Tick(object? sender, EventArgs e) - { - if (monitoring.isMonitor == 1) - { - byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)]; - for (int i = 0; i < monitoring.monitorFillLevel; i++) - { - int slot = monitoring.monitorItems[i].slotNo; - int offset = monitoring.monitorItems[i].offset; - int len = monitoring.monitorItems[i].len; - if (len == 0) continue; - Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float)); - } - if (tele_server != null) - { - tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length)); - } - } - } - - private void CopyDataTimer_Tick(object? sender, EventArgs e) - { - /*if (dataDrone != null) - { - float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ }; - Array.Copy(imu, telemetry.imu, 6); - }*/ - - if (monitoring.isMonitor == 1) MonitoringTimer.Start(); - else MonitoringTimer.Stop(); - } - - private void ConnectionCallback(object o) - { - ConnectData data = (ConnectData)o; - - if (!data.Connect) - { - try - { - Invoke((MethodInvoker)delegate - { - button_Connect.Text = "Connect"; - button_Connect.BackColor = Color.Transparent; - MessageBox.Show("Connection closed"); - }); - } - catch { } - - return; - } - - 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; - } + InitializeComponent(); } + + private void ConnectionCallback(object o) + { + ConnectData data = (ConnectData)o; + + if (!data.Connect) + { + try + { + Invoke((MethodInvoker)delegate + { + button_Connect.Text = "Connect"; + button_Connect.BackColor = Color.Transparent; + MessageBox.Show("Connection closed"); + }); + } + catch { } + + return; + } + } + + Drone dataDrone = new Drone(); + + private void ReceiveCallback(object o) + { + ReceiveData data = (ReceiveData)o; + + List? send = dataDrone.DataStream(data.Buffer, data.Size); + + if (send == null) return; + try + { + foreach (byte[]? b in send) + { + if (b != null) data.Server?.Send(b); + } + } + catch { } + } + + private void button_Connect_Click(object sender, EventArgs e) + { + var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback); + + switch (done) + { + case NetClient.ClientState.Error: + { + MessageBox.Show("Error connecting to server"); + break; + } + case NetClient.ClientState.Connected: + { + button_Connect.Text = "Disconnect"; + button_Connect.BackColor = Color.LimeGreen; + break; + } + case NetClient.ClientState.Stop: + { + button_Connect.Text = "Connect"; + button_Connect.BackColor = Color.Transparent; + break; + } + } + + if (done != NetClient.ClientState.Connected) return; + } + + 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 deleted file mode 100644 index 57fc853..0000000 --- a/DroneClient/Models/MonitorContainer.cs +++ /dev/null @@ -1,126 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using TelemetryIO; - -namespace DroneClient.Models -{ - public class monitorItem - { - public int slotNo = 0; - public int offset = 0; - public int len = 0; - public int id = 0; - - public monitorItem() - { - - } - public monitorItem(int slotNo, int offset, int len, int id) - { - this.slotNo = slotNo; - this.offset = offset; - this.len = len; - this.id = id; - } - } - - public class MonitorContainer - { - public const int monitorMaxFill = 32; - public int monitorFillLevel = 0; - public monitorItem[] monitorItems = new monitorItem[monitorMaxFill]; - public int isMonitor = 0; - private static volatile MonitorContainer instance; - private static readonly object _lock = new object(); - - public static MonitorContainer Instance - { - get - { - if (instance == null) - { - lock (_lock) - { - if (instance == null) - { - instance = new MonitorContainer(); - } - } - } - return instance; - } - - } - - private MonitorContainer() - { - for (int i = 0; i < monitorMaxFill; i++) - { - monitorItems[i] = new monitorItem(); - } - } - - public void resetMonitorContainer() - { - for (int i = 0; i < monitorMaxFill; i++) - { - monitorItems[i] = new monitorItem(); - } - } - - public int monitorPut(int slot, int offset, int len) - { - int ret_id = 0; - if (monitorFillLevel < monitorMaxFill) - { - monitorItems[monitorFillLevel].slotNo = slot; - monitorItems[monitorFillLevel].offset = offset; - monitorItems[monitorFillLevel].len = len; - monitorItems[monitorFillLevel].id = (slot << 8) + offset; - ret_id = monitorItems[monitorFillLevel].id; - monitorFillLevel++; - isMonitor = 1; - } - - return ret_id; - } - - public int monitorRemove(int id) - { - int match = 0; - int ret_id = 0; - for (int i = 0; i < monitorMaxFill; i++) - { - if (monitorItems[i].id == id) - { - match = 1; - if (monitorFillLevel > 0) monitorFillLevel--; - ret_id = id; - } - - if (match > 0) - { - if (monitorItems[i].len == 0) return ret_id; - if (i < 31) - { - monitorItems[i].id = monitorItems[i + 1].id; - monitorItems[i].offset = monitorItems[i + 1].offset; - monitorItems[i].slotNo = monitorItems[i + 1].slotNo; - monitorItems[i].len = monitorItems[i + 1].len; - } - else - { - monitorItems[i].id = 0; - monitorItems[i].offset = 0; - monitorItems[i].slotNo = 0; - monitorItems[i].len = 0; - } - } - } - return ret_id; - } - } -} diff --git a/DroneClient/Models/Telemetry.cs b/DroneClient/Models/Telemetry.cs deleted file mode 100644 index 60af270..0000000 --- a/DroneClient/Models/Telemetry.cs +++ /dev/null @@ -1,354 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Drawing; -using System.Linq; -using System.Text; -using System.Threading.Tasks; - -namespace TelemetryIO.Models -{ - public sealed class Telemetry - { - //Класс синглтон для хранения данных телеметрии - private static volatile Telemetry instance; - private static readonly object _lock = new object(); - private Dictionary 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 deleted file mode 100644 index 2771671..0000000 --- a/DroneClient/Properties/Resources.Designer.cs +++ /dev/null @@ -1,63 +0,0 @@ -//------------------------------------------------------------------------------ -// -// Этот код создан программой. -// Исполняемая версия: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 deleted file mode 100644 index 1af7de1..0000000 --- a/DroneClient/Properties/Resources.resx +++ /dev/null @@ -1,120 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 deleted file mode 100644 index f619039..0000000 --- a/DroneClient/TelemetryServer.cs +++ /dev/null @@ -1,258 +0,0 @@ -using DroneClient.Models; -using System; -using System.Collections.Concurrent; -using System.Collections.Generic; -using System.Diagnostics; -using System.IO; -using System.Linq; -using System.Net; -using System.Net.Sockets; -using System.Text; -using System.Threading; -using System.Threading.Tasks; -using TelemetryIO; -using TelemetryIO.Models; - -namespace TelemetryIO -{ - internal class TelemetryServer : TelemetryIO.BaseCommHandler - { - ConcurrentQueue 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 deleted file mode 100644 index f3d3c3d..0000000 Binary files a/DroneClient/images/connect.ico and /dev/null differ diff --git a/DroneClient/images/disconnect.ico b/DroneClient/images/disconnect.ico deleted file mode 100644 index 37f053f..0000000 Binary files a/DroneClient/images/disconnect.ico and /dev/null differ diff --git a/DroneClient/utils/AdaptivePID.cs b/DroneClient/utils/AdaptivePID.cs deleted file mode 100644 index fc22e3c..0000000 --- a/DroneClient/utils/AdaptivePID.cs +++ /dev/null @@ -1,88 +0,0 @@ -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 deleted file mode 100644 index 7d8fe3d..0000000 --- a/DroneClient/utils/DroneOrientation.cs +++ /dev/null @@ -1,198 +0,0 @@ -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 deleted file mode 100644 index 3e833ab..0000000 --- a/DroneClient/utils/Quaternion.cs +++ /dev/null @@ -1,178 +0,0 @@ -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 deleted file mode 100644 index 758b03a..0000000 --- a/DroneClient/utils/RC.cs +++ /dev/null @@ -1,45 +0,0 @@ -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 deleted file mode 100644 index c1e07c3..0000000 --- a/DroneClient/utils/UtilityUI.cs +++ /dev/null @@ -1,74 +0,0 @@ -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 deleted file mode 100644 index e4bd7af..0000000 --- a/DroneClient/utils/Vector.cs +++ /dev/null @@ -1,148 +0,0 @@ -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 deleted file mode 100644 index 23b2df5..0000000 Binary files a/connect.png and /dev/null differ diff --git a/disconnect.png b/disconnect.png deleted file mode 100644 index b12a154..0000000 Binary files a/disconnect.png and /dev/null differ