CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Vector, Quaternion - математика для ориентации. AdaptivePID - ПИД-регулятор. DroneOrientation.CalcAttitude() - цикл вычисления положения.

Browse Source
This commit is contained in:
Dmitry Vorobyev
2025-08-04 20:52:20 +03:00
parent 15d4fa5011
commit 1f8bde2370
21 changed files with 3758 additions and 1127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
DroneClient/BaseCommHandler.cs Normal file
View File

@ -0,0 +1,478 @@
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TelemetryIO.Models;
namespace TelemetryIO
{
public interface iCommParams
{
}
internal class TCPCommParams : iCommParams
{
public string IP = "";
public int Port = 0;
public TCPCommParams(string addr, int port)
{
this.IP = addr;
this.Port = port;
}
}
public class SerialCommParams : iCommParams
{
public string PortName = "";
public int BaudRate = 9600;
public SerialCommParams(string portName, int baudRate)
{
PortName = portName;
BaudRate = baudRate;
}
}
public abstract class BaseCommHandler
{
public const int TELE_CMD_RD_ONCE = 1;
public const int TELE_CMD_RD_MON_ON = 2;
public const int TELE_CMD_RD_MON_OFF = 3;
public const int TELE_CMD_RD_MON_ADD = 4;
public const int TELE_CMD_RD_MON_REMOVE = 5;
public const int TELE_CMD_RD_MON_REMOVEALL = 6;
public const int TELE_CMD_WR = 10;
public const int TELE_CMD_MOTORS_CTRL = 100;
public const int TELE_CMD_ABORT = 999;
public const int TELE_CMD_HELLO = 9999;
public const byte CRC8_POLYNOMIAL = 0x07;
public const int CRC32_POLY = 0x04C11DB7;
public const byte ESCAPE_BEGIN = 0xBE;
public const byte ESCAPE_END = 0xED;
public const byte ESCAPE_CHAR = 0x0E;
private volatile bool _isReading = true;
private readonly ConcurrentQueue<byte> dataQueue = new ConcurrentQueue<byte>();
static private List<byte> rx_buf = new List<byte>();
private Task _readingTask;
private object lock_obj = new object();
private object lock_obj_put = new object();
private bool waitingForResponse = false;
private bool isTimeout = false;
private readonly int responseTimeout = 2000;
private System.Timers.Timer timeout = new System.Timers.Timer(3000);
private bool exitPending = false;
private float[] monitor = new float[32];
public string view_str = "";
protected string IP = "127.0.0.1";
protected int Port = 8888;
protected string PortName = "COM1";
protected int BaudRate = 9600;
private Queue<byte[]> req_buffer = new Queue<byte[]>();
//Generate event when answer is received
public delegate void AnswerEventHandler<SerialEventArgs>(object sender, SerialEventArgs e);
public event AnswerEventHandler<FeedbackEventArgs> AnswerReceived;//событие получены данные в ответ на запрос
//Generate event when handshake is occurred
public delegate void HandShakeHandler(object sender, EventArgs e);
public event HandShakeHandler HandShakeOccurred;//событие полетник ответил на запрос HELLO
//Generate event when monitoring telegram is received
public delegate void MonitoringEventHandler<MonitoringEventArgs>(object sender, MonitoringEventArgs e);
public event MonitoringEventHandler<MonitoringEventArgs> MonitoringItemsReceived;//событие получены данные мониторинга
Models.Telemetry telemetry = Models.Telemetry.Instance;
abstract public Task Open();
abstract public void Close();
abstract protected void sendData(byte[] data);
abstract public bool IsOpen();
abstract public void CloseConnection();
abstract public Task StartReadingAsync(object? client = null);
abstract public void setCommParams(iCommParams commParams);
abstract protected void ProcessCommand(int cmd, int slot, byte[] data, int offset, int len);
/// <summary>
/// Обрабатывает входящий поток данных, при обнаружении ECSAPE_END байта декодирует данные, проверяет контрольную сумму и запускает обработку команды
/// </summary>
protected void data_extract()
{
byte b = new byte();
while (dataQueue.TryDequeue(out b))
{
if (b == ESCAPE_END)
{//END BYTE IS RECEIVED
isTimeout = false;
byte[] unscape = EscapeSeqToBytes(rx_buf.ToArray());
uint checksum = crc32(unscape, unscape.Length - 4);
uint re_checksum = BitConverter.ToUInt32(unscape, unscape.Length - 4);
if (re_checksum == checksum)
{
//Parse telegram
int cmd = BitConverter.ToInt32(unscape, 0);
int slot = BitConverter.ToInt32(unscape, 4);
int len = BitConverter.ToInt32(unscape, 8);
int offset = BitConverter.ToInt32(unscape, 12);
byte[] data = new byte[len];
Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** offset = {offset} *** len = {len}");
if(cmd == TELE_CMD_WR)Array.Copy(unscape, 16, data, 0, len);
ProcessCommand(cmd, slot, data, offset, len);
waitingForResponse = false;
timeout.Stop();
sendNextRequest();
}
}
else if (b == ESCAPE_BEGIN)
{//START BYTE IS RECEIVED
rx_buf.Clear();
}
else
{//FILLING BUFFER
rx_buf.Add(b);
}
}
}
//**********************************************************************************************
//*************************************** REQUESTS BLOCK ***************************************
//**********************************************************************************************
public void getPIDs()
{
//отправить наборы ПИДов
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1000, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1001, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1002, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1003, new byte[0], 0, 4 * 9));
}
public void stopMonitoring()
{
//остановить мониторинг
putRequest(prepareTelegram(TELE_CMD_RD_MON_OFF, 0, new byte[0], 0, 0));
}
public void startMonitoring()
{
//начать мониторинг
putRequest(prepareTelegram(TELE_CMD_RD_MON_ON, 0, new byte[0], 0, 0));
}
public void AddMonitoringItem(int slot, int offset)
{
//добавить элемент из массива мониторинга по адресу
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, slot, new byte[0], offset));
}
public void AddMonitoringItem(string name)
{
//добавить элемент из массива мониторинга по имени
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, new byte[0], name));
}
public void RemoveMonitoringItem(int id)
{
//удалить элемент из массива мониторинга (len == id)
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVE, 0, new byte[0], id));
}
public void RemoveMonitoringItems()
{
//удалить все элементы из массива мониторинга
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVEALL, 0, new byte[0], 0));
}
public void sendFloats(int slot, float[] sp)
{
//Записать массив чисел с плавающей точкой
putRequest(prepareTelegram(TELE_CMD_WR, slot, sp, 0, sp.Length * 4));
}
public void sendMotorsControl()
{
//Отправляет задание на моторы в полетник. Тот в ответ посылает актуальные скорости на моторах
//В offset передается количество моторов
putRequest(prepareTelegram(TELE_CMD_MOTORS_CTRL, Telemetry.MOTORS_SP_ADDRESS, telemetry.motor_sp, 8));
}
/// <summary>
/// Считает контрольную сумму CRC32
/// </summary>
public uint crc32(byte[] data, int length)
{
uint crc = 0xFFFFFFFF; // Начальное значение CRC
for (int i = 0; i < length; i++)
{
crc ^= (uint)data[i] << 24; // XOR с текущим байтом
for (int j = 0; j < 8; j++)
{
if ((uint)(crc & 0x80000000) != 0)
{
crc = (crc << 1) ^ CRC32_POLY;
}
else
{
crc <<= 1;
}
}
}
return crc;
}
public byte[] prepareTelegram<T>(int cmd, T[] load, string var_name)
{
VarAddress va = telemetry.getVarAdress(var_name);
return prepareTelegram(cmd, va.slot, load, va.offset, va.length);
}
/// <summary>
/// Подготавливает данные для отправки, кодируя их в ESCAPE-последовательность
/// </summary>
public byte[] prepareTelegram<T>(int cmd, int slot, T[] load, int offset, int len = 0)
{
byte[] byteload = DataArrayToBytes(load);
int total_len = 20 + byteload.Length;//cmd[4 bytes] + slot[4 bytes] + len[4 bytes] + offset[4 bytes] + load[len bytes]
byte[] data = new byte[total_len];
//Construct telegram
data[0] = (byte)(0xFF & cmd);
data[1] = (byte)(0xFF & (cmd >> 8));
data[2] = (byte)(0xFF & (cmd >> 16));
data[3] = (byte)(0xFF & (cmd >> 24));
data[4] = (byte)(0xFF & slot);
data[5] = (byte)(0xFF & (slot >> 8));
data[6] = (byte)(0xFF & (slot >> 16));
data[7] = (byte)(0xFF & (slot >> 24));
int l = 0;
if (cmd == TELE_CMD_WR) l = byteload.Length;
else l = len;
data[8] = (byte)(0xFF & l);
data[9] = (byte)(0xFF & (l >> 8));
data[10] = (byte)(0xFF & (l >> 16));
data[11] = (byte)(0xFF & (l >> 24));
data[12] = (byte)(0xFF & offset);
data[13] = (byte)(0xFF & (offset >> 8));
data[14] = (byte)(0xFF & (offset >> 16));
data[15] = (byte)(0xFF & (offset >> 24));
if (byteload.Length > 0)
{
//Copy data
Array.Copy(byteload, 0, data, 16, byteload.Length);
}
//CRC32
uint checksum = crc32(data, total_len - 4);
data[total_len - 4] = (byte)(0xFF & checksum);
data[total_len - 3] = (byte)(0xFF & (checksum >> 8));
data[total_len - 2] = (byte)(0xFF & (checksum >> 16));
data[total_len - 1] = (byte)(0xFF & (checksum >> 24));
byte[] escape = BytesToEscapeSeq(data);
byte[] ret = new byte[escape.Length + 2];
Array.Copy(escape, 0, ret, 1, escape.Length);
ret[0] = ESCAPE_BEGIN;
ret[ret.Length - 1] = ESCAPE_END;
//Array.Copy(ret, saving_request, ret.Length);
return ret;
}
/// <summary>
/// Конвертирует массив bool/byte/int/float в массив байт. Создает новый массив.
/// </summary>
byte[] DataArrayToBytes<T>(T[] data)
{
if (data == null) return new byte[0];
List<byte> ret = new List<byte>();
for (int i = 0; i < data.Length; i++)
{
if (typeof(T) == typeof(float))
{
ret.AddRange(BitConverter.GetBytes((float)(object)data[i]));
}
else if (typeof(T) == typeof(int))
{
ret.AddRange(BitConverter.GetBytes((int)(object)data[i]));
}
else if (typeof(T) == typeof(byte))
{
ret.Add((byte)(object)data[i]);
}
else if (typeof(T) == typeof(bool))
{
bool t = (bool)(object)data[i];
if (t) ret.Add(1);
else ret.Add(0);
}
}
return ret.ToArray();
}
/// <summary>
/// Конвертирует массив байт в ESCAPE-последовательность.
/// </summary>
public byte[] BytesToEscapeSeq(byte[] data)
{
List<byte> ret = new List<byte>();
for (int i = 0; i < data.Length; i++)
{
if ((data[i] == ESCAPE_BEGIN) || (data[i] == ESCAPE_CHAR) || (data[i] == ESCAPE_END))
{
ret.Add(ESCAPE_CHAR);
ret.Add((byte)(data[i] - 0x0E));
}
else
{
ret.Add(data[i]);
}
}
return ret.ToArray();
}
/// <summary>
/// Конвертирует ESCAPE-последовательность в массив байт.
/// </summary>
public byte[] EscapeSeqToBytes(byte[] EscSeq)
{
List<byte> ret = new List<byte>();
for (int i = 0; i < EscSeq.Length; i++)
{
//if ((EscSeq[i] == ESCAPE_BEGIN) || (EscSeq[i] == ESCAPE_CHAR) || (EscSeq[i] == ESCAPE_END))
if (EscSeq[i] == ESCAPE_CHAR)
{
i++;
ret.Add((byte)(EscSeq[i] + 0x0E));
}
else
{
ret.Add(EscSeq[i]);
}
}
return ret.ToArray();
}
public void requestExit()
{
//запрос на закрытие приложения, отправляем команду на очистку массива мониторинга и ждем ответ, либо таймаут,
//после чего приложение закрывается
RemoveMonitoringItems();
exitPending = true;
}
/// <summary>
/// Добавляет данные в очередь ожидания отправки.
/// </summary>
public void putRequest(byte[] request)
{
//добавить в очередь или отправить(если очередь пуста) пакет в порт
lock (lock_obj_put)
{
if (waitingForResponse)
{
req_buffer.Enqueue(request);
return;
}
sendRequest(request);
}
}
/// <summary>
/// Толкает данные непосредственно в очередь отправки.
/// </summary>
protected void sendRequest(byte[] request)
{
//отправка пакета
//waitingForResponse = true;
if (IsOpen())
{
sendData(request);
}
timeout.Stop();
timeout.Start();
}
/// <summary>
/// Вытягивает данные из очереди ожидания непосредственно в очередь отправки.
/// </summary>
protected void sendNextRequest()
{
//вытащить из очереди пакет и отправить в порт
waitingForResponse = false;
if (req_buffer.Count > 0)
{
sendRequest(req_buffer.Dequeue());
}
}
/// <summary>
/// Копирует входящие данные в очередь.
/// </summary>
protected void EnqueueData(byte[] data, int len)
{
for (int i = 0; i < len; i++)
{
dataQueue.Enqueue(data[i]);
}
}
public void ClearReqQueue()
{
req_buffer.Clear();
}
protected virtual void OnAnswerReceived(string answer, int id)
{
AnswerReceived?.Invoke(this, new FeedbackEventArgs(answer, id));
}
protected virtual void OnHandShakeOccurred()
{
HandShakeOccurred?.Invoke(this, new EventArgs());
}
protected virtual void OnMonitoringItemsReceived(float[] data)
{
MonitoringItemsReceived?.Invoke(this, new MonitoringEventArgs(data));
}
}
public class FeedbackEventArgs : EventArgs
{
public string Answer { get; set; }
public int Id { get; set; }
public FeedbackEventArgs(string answer, int id)
{
Answer = answer;
Id = id;
}
}
public class MonitoringEventArgs : EventArgs
{
public float[] Data { get; set; }
public MonitoringEventArgs(float[] data)
{
Data = data;
}
}
}

711
DroneClient/Drone.cs
View File

@ -6,329 +6,426 @@ using System.Runtime.InteropServices;
namespace DroneClient
{
internal class Drone
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public uint TimeAcc, TimeGyr;
public float PosX, PosY;
public float LaserRange;
public uint TimeRange;
public Vector2 OF = Vector2.Zero;
public float MotorUL, MotorUR, MotorDL, MotorDR;
public static byte[] getBytes(object data)
internal class Drone
{
int size = Marshal.SizeOf(data);
byte[] arr = new byte[size];
IntPtr ptr = IntPtr.Zero;
try
{
ptr = Marshal.AllocHGlobal(size);
Marshal.StructureToPtr(data, ptr, true);
Marshal.Copy(ptr, arr, 0, size);
}
finally
{
Marshal.FreeHGlobal(ptr);
}
return arr;
}
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public uint TimeAcc, TimeGyr;
private static readonly object _readLock = new();
private static readonly object _writeLock = new();
public static object fromBytes(byte[] arr, Type type)
{
object mem = new object();
public float qw, q1, q2, q3;
int size = Marshal.SizeOf(type);
IntPtr ptr = IntPtr.Zero;
try
{
ptr = Marshal.AllocHGlobal(size);
public float PosX, PosY;
public float LaserRange;
public uint TimeRange;
Marshal.Copy(arr, 0, ptr, size);
public Vector2 OF = Vector2.Zero;
mem = Marshal.PtrToStructure(ptr, type);
}
finally
{
Marshal.FreeHGlobal(ptr);
}
public float MotorUL, MotorUR, MotorDL, MotorDR;
return mem;
}
private byte[] SendDataMotor4()
{
DroneData.DataMotor4 mot4 = new DroneData.DataMotor4();
mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4));
mot4.Head.Mode = DroneData.DataMode.Response;
mot4.Head.Type = DroneData.DataType.DataMotor4;
mot4.UL = MotorUL;
mot4.UR = MotorUR;
mot4.DL = MotorDL;
mot4.DR = MotorDR;
return getBytes(mot4);
}
private byte[]? RecvDataAcc(byte[] data)
{
DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc= imu.Time;
return new byte[0];
}
private byte[]? RecvDataGyr(byte[] data)
{
DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
return new byte[0];
}
private byte[]? RecvDataRange(byte[] data)
{
DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
return new byte[0];
}
private byte[]? RecvDataLocal(byte[] data)
{
DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));
PosX = pos.Local.X; PosY = pos.Local.Y;
return new byte[0];
}
private byte[]? RecvDataOF(byte[] data)
{
DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF));
OF.X = of.X;
OF.Y = of.Y;
return new byte[0];
}
private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
switch (head.Type)
{
case DroneData.DataType.DataAcc:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataAcc(body);
}
}
case DroneData.DataType.DataGyr:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataGyr(body);
}
}
case DroneData.DataType.DataRange:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataRange(body);
}
}
case DroneData.DataType.DataLocal:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataLocal(body);
}
}
case DroneData.DataType.DataOF:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataOF(body);
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataMotor4();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
}
return zero;
}
private const int DroneStreamCount = 512;
private byte[] DroneStreamData = new byte[DroneStreamCount];
private int DroneStreamIndex = 0;
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
public List<byte[]?>? DataStream(byte[]? data, int size)
{
List<byte[]?> ret = new List<byte[]?>();
if (data == null) return ret; // Последовательность не сформирована, ждать данных
if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец)
Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
DroneStreamIndex += size;
while (true)
{
if (DroneStreamHead.Size == 0) // Заголовок ещё не получен
public static byte[] getBytes(object data)
{
if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать
DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
int size = Marshal.SizeOf(data);
byte[] arr = new byte[size];
IntPtr ptr = IntPtr.Zero;
try
{
ptr = Marshal.AllocHGlobal(size);
Marshal.StructureToPtr(data, ptr, true);
Marshal.Copy(ptr, arr, 0, size);
}
finally
{
Marshal.FreeHGlobal(ptr);
}
return arr;
}
if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный
public static object fromBytes(byte[] arr, Type type)
{
object mem = new object();
byte[] body = new byte[DroneStreamHead.Size];
int size = Marshal.SizeOf(type);
IntPtr ptr = IntPtr.Zero;
try
{
ptr = Marshal.AllocHGlobal(size);
Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);
Marshal.Copy(arr, 0, ptr, size);
int shift = DroneStreamHead.Size;
mem = Marshal.PtrToStructure(ptr, type);
}
finally
{
Marshal.FreeHGlobal(ptr);
}
DroneStreamIndex -= shift;
Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные
return mem;
}
DroneStreamHead.Size = 0; // Отменяем прошлый заголовок
private byte[] SendDataMotor4()
{
DroneData.DataMotor4 mot4 = new DroneData.DataMotor4();
ret.Add(ClientRequestResponse(DroneStreamHead, body));
}
mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4));
mot4.Head.Mode = DroneData.DataMode.Response;
mot4.Head.Type = DroneData.DataType.DataMotor4;
return ret;
mot4.UL = MotorUL;
mot4.UR = MotorUR;
mot4.DL = MotorDL;
mot4.DR = MotorDR;
return getBytes(mot4);
}
public void writeDataMotor4(float UL, float UR, float DL, float DR)
{
lock (_writeLock)
{
MotorUL = UL;
MotorUR = UR;
MotorDL = DL;
MotorDR = DR;
}
}
private byte[]? RecvDataAcc(byte[] data)
{
DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
lock (_readLock)
{
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc = imu.Time;
}
return new byte[0];
}
public float[] readDataAcc()
{
lock (_readLock)
{
float[] acc = { AccX, AccY, AccZ, TimeAcc };
return acc;
}
}
private byte[]? RecvDataGyr(byte[] data)
{
DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));
lock (_readLock)
{
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
}
return new byte[0];
}
public float[] readDataGyr()
{
lock (_readLock)
{
float[] gyr = { GyrX, GyrY, GyrZ, TimeGyr };
return gyr;
}
}
private byte[]? RecvDataRange(byte[] data)
{
DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));
lock (_readLock)
{
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
}
return new byte[0];
}
public float[] readDataRange()
{
lock (_readLock)
{
float[] range = { LaserRange, TimeRange };
return range;
}
}
private byte[]? RecvDataLocal(byte[] data)
{
DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));
lock (_readLock)
{
PosX = pos.Local.X; PosY = pos.Local.Y;
}
return new byte[0];
}
public float[] readDataLocal()
{
lock (_readLock)
{
float[] local = { PosX, PosY };
return local;
}
}
private byte[]? RecvDataOF(byte[] data)
{
DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF));
OF.X = of.X;
OF.Y = of.Y;
return new byte[0];
}
private Vector2 readDataOF()
{
lock (_readLock)
{
return new Vector2(OF.X, OF.Y);
}
}
private byte[]? RecvDataQuat(byte[] data)
{
DroneData.DataQuat q = (DroneData.DataQuat)fromBytes(data, typeof(DroneData.DataQuat));
qw = q.W; q1 = q.X; q2 = q.Y; q3 = q.Z;
return new byte[0];
}
private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
switch (head.Type)
{
case DroneData.DataType.DataAcc:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataAcc(body);
}
}
case DroneData.DataType.DataGyr:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataGyr(body);
}
}
case DroneData.DataType.DataRange:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataRange(body);
}
}
case DroneData.DataType.DataLocal:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataLocal(body);
}
}
case DroneData.DataType.DataOF:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataOF(body);
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataMotor4();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataQuat:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return zero;
}
else
{
// Пришли данные
// ... //
//
return RecvDataQuat(body);
}
}
}
return zero;
}
private const int DroneStreamCount = 512;
private byte[] DroneStreamData = new byte[DroneStreamCount];
private int DroneStreamIndex = 0;
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
public List<byte[]?>? DataStream(byte[]? data, int size)
{
List<byte[]?> ret = new List<byte[]?>();
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<byte[]> list = new List<byte[]>();
list.Add(getBytes(acc));
list.Add(getBytes(gyr));
list.Add(getBytes(range));
list.Add(getBytes(local));
list.Add(getBytes(of));
list.Add(getBytes(quat));
list.Add(SendDataMotor4());
int count = 0;
foreach (byte[] d in list) count += d.Length;
byte[] send = new byte[count];
count = 0;
foreach (byte[] d in list)
{
d.CopyTo(send, count);
count += d.Length;
}
return send;
}
}
public byte[] SendReqest()
{
DroneData.DataHead acc = new DroneData.DataHead();
acc.Size = DroneData.DataHead.StrLen;
acc.Mode = DroneData.DataMode.Request;
acc.Type = DroneData.DataType.DataAcc;
DroneData.DataHead gyr = new DroneData.DataHead();
gyr.Size = DroneData.DataHead.StrLen;
gyr.Mode = DroneData.DataMode.Request;
gyr.Type = DroneData.DataType.DataGyr;
DroneData.DataHead range = new DroneData.DataHead();
range.Size = DroneData.DataHead.StrLen;
range.Mode = DroneData.DataMode.Request;
range.Type = DroneData.DataType.DataRange;
DroneData.DataHead local = new DroneData.DataHead();
local.Size = DroneData.DataHead.StrLen;
local.Mode = DroneData.DataMode.Request;
local.Type = DroneData.DataType.DataLocal;
DroneData.DataHead of = new DroneData.DataHead();
of.Size = DroneData.DataHead.StrLen;
of.Mode = DroneData.DataMode.Request;
of.Type = DroneData.DataType.DataOF;
List<byte[]> list = new List<byte[]>();
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;
}
}
}

24
DroneClient/DroneClient.csproj
View File

@ -8,4 +8,28 @@
<ImplicitUsings>enable</ImplicitUsings>
</PropertyGroup>
<ItemGroup>
<Compile Update="Properties\Resources.Designer.cs">
<DesignTime>True</DesignTime>
<AutoGen>True</AutoGen>
<DependentUpon>Resources.resx</DependentUpon>
</Compile>
</ItemGroup>
<ItemGroup>
<EmbeddedResource Update="Properties\Resources.resx">
<Generator>ResXFileCodeGenerator</Generator>
<LastGenOutput>Resources.Designer.cs</LastGenOutput>
</EmbeddedResource>
</ItemGroup>
<ItemGroup>
<None Update="images\connect.ico">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
<None Update="images\disconnect.ico">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>

176
DroneClient/DroneData.cs
View File

@ -2,122 +2,130 @@
namespace DroneData
{
public enum DataMode : ushort
{
None = 0, Response = 1, Request = 2
};
public enum DataMode : ushort
{
None = 0, Response = 1, Request = 2
};
public enum DataType : ushort
{
None = 0, Head = 1,
public enum DataType : ushort
{
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Input
DataMotor4 = 2001, DataMotor6 = 2002,
// Input
DataMotor4 = 2001, DataMotor6 = 2002,
// State
DataQuat = 3001,
};
// State
DataQuat = 3001,
};
public struct DataHead
{
public int Size;
public struct DataHead
{
public int Size;
public DataMode Mode;
public DataType Type;
public DataMode Mode;
public DataType Type;
public uint Time; // Общее время
public uint Time; // Общее время
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct XYZ { public float X, Y, Z; }
public struct DataAcc
{
public DataHead Head;
public XYZ Acc;
public struct DataAcc
{
public DataHead Head;
public XYZ Acc;
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
}
public struct DataGyr
{
public DataHead Head;
public XYZ Gyr;
public struct DataGyr
{
public DataHead Head;
public XYZ Gyr;
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // Датчик посадки
public struct DataRange
{
public DataHead Head;
public float LiDAR; // Датчик посадки
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // Локальные координаты
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // Локальные координаты
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataBar
{
public DataHead Head;
public float Pressure; // Давление
public struct DataBar
{
public DataHead Head;
public float Pressure; // Давление
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
}
public struct DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public struct DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public uint Time; // Последнее время изменения данных
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
public struct DataMotor4
{
public DataHead Head;
public float UL, UR, DL, DR;
public struct DataMotor4
{
public DataHead Head;
public float UL, UR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
}
public struct DataMotor6
{
public DataHead Head;
public float UL, UR, LL, RR, DL, DR;
public struct DataMotor6
{
public DataHead Head;
public float UL, UR, LL, RR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
}
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
}
public struct DataQuat
{
public DataHead Head;
public float X, Y, Z, W;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat));
}
}

1347
DroneClient/FormMain.Designer.cs generated
View File

File diff suppressed because it is too large Load Diff

497
DroneClient/FormMain.cs
View File

@ -3,170 +3,357 @@ using System.Numerics;
using System.Windows.Forms;
using static DroneSimulator.NetClient;
using DroneClient;
using TelemetryIO.Models;
using TelemetryIO;
using DroneClient.Models;
using DroneClient.utils;
using System;
using Vector = DroneClient.utils.Vector;
namespace DroneSimulator
{
public partial class Form_Main : Form
{
private NetClient netClient = new NetClient();
public Form_Main()
public partial class Form_Main : Form
{
InitializeComponent();
}
private NetClient netClient = new NetClient();
private Telemetry telemetry = Telemetry.Instance;
private MonitorContainer monitoring = MonitorContainer.Instance;
private System.Windows.Forms.Timer copyDataTimer = new System.Windows.Forms.Timer();
private System.Windows.Forms.Timer MonitoringTimer = new System.Windows.Forms.Timer();
TelemetryServer tele_server;
string connectIco = Path.Combine(Application.StartupPath, "Images", "connect.ico");
string disconnectIco = Path.Combine(Application.StartupPath, "Images", "disconnect.ico");
RC rc = new();
private void ConnectionCallback(object o)
{
ConnectData data = (ConnectData)o;
if (!data.Connect)
{
try
public Form_Main()
{
Invoke((MethodInvoker)delegate
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
MessageBox.Show("Connection closed");
});
InitializeComponent();
tele_server = new TelemetryServer();
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
}
catch { }
return;
}
}
Drone dataDrone = new Drone();
private void ReceiveCallback(object o)
{
ReceiveData data = (ReceiveData)o;
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
if (send == null) return;
try
{
foreach (byte[]? b in send)
private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e)
{
if (b != null) data.Server?.Send(b);
if (TeleClientStatusPicture.InvokeRequired)
{
TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); }));
}
else
{
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
}
monitoring.resetMonitorContainer();
}
private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e)
{
if (TeleClientStatusPicture.InvokeRequired)
{
TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); }));
}
else
{
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco);
}
}
private void MonitoringTimer_Tick(object? sender, EventArgs e)
{
if (monitoring.isMonitor == 1)
{
byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)];
for (int i = 0; i < monitoring.monitorFillLevel; i++)
{
int slot = monitoring.monitorItems[i].slotNo;
int offset = monitoring.monitorItems[i].offset;
int len = monitoring.monitorItems[i].len;
if (len == 0) continue;
Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float));
}
if (tele_server != null)
{
tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length));
}
}
}
private void CopyDataTimer_Tick(object? sender, EventArgs e)
{
/*if (dataDrone != null)
{
float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
Array.Copy(imu, telemetry.imu, 6);
}*/
if (monitoring.isMonitor == 1) MonitoringTimer.Start();
else MonitoringTimer.Stop();
}
private void ConnectionCallback(object o)
{
ConnectData data = (ConnectData)o;
if (!data.Connect)
{
try
{
Invoke((MethodInvoker)delegate
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
MessageBox.Show("Connection closed");
});
}
catch { }
return;
}
Task.Run(() => DroneOrientation.CalcAttitude(dataDrone, rc));
}
Drone dataDrone = new Drone();
private void ReceiveCallback(object o)
{
ReceiveData data = (ReceiveData)o;
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
if (send == null) return;
try
{
foreach (byte[]? b in send)
{
if (b != null) data.Server?.Send(b);
}
}
catch { }
}
private void button_Connect_Click(object sender, EventArgs e)
{
var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
switch (done)
{
case NetClient.ClientState.Error:
{
MessageBox.Show("Error connecting to server");
break;
}
case NetClient.ClientState.Connected:
{
button_Connect.Text = "Disconnect";
button_Connect.BackColor = Color.LimeGreen;
break;
}
case NetClient.ClientState.Stop:
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
break;
}
}
if (done != NetClient.ClientState.Connected) return;
copyDataTimer.Interval = 10;
copyDataTimer.Tick += CopyDataTimer_Tick;
tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
var t = Task.Run(() => tele_server.Open());
tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
MonitoringTimer.Interval = 25;
MonitoringTimer.Tick += MonitoringTimer_Tick;
copyDataTimer.Start();
button1.BackColor = Color.LimeGreen;
}
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
{
netClient?.Close();
netClient = null;
}
private void timer_Test_Tick(object sender, EventArgs e)
{
label_Acc_X.Text = dataDrone.AccX.ToString();
label_Acc_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_time_acc.Text = dataDrone.TimeAcc.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
Array.Copy(imu, telemetry.imu, 6);
label_Pos_X.Text = dataDrone.PosX.ToString();
label_Pos_Y.Text = dataDrone.PosY.ToString();
label_Pos_L.Text = dataDrone.LaserRange.ToString();
label_time_range.Text = dataDrone.TimeRange.ToString();
float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange };
Array.Copy(pos, telemetry.pos, 3);
label_OF_X.Text = dataDrone.OF.X.ToString();
label_OF_Y.Text = dataDrone.OF.Y.ToString();
label_time_of.Text = dataDrone.TimeRange.ToString();
label_simq.Text = $"({dataDrone.qw}; {dataDrone.q1}; {dataDrone.q2}; {dataDrone.q3})";
label_myq.Text = DroneOrientation.GetAttitude().ToString();
float[] of = { dataDrone.OF.X, dataDrone.OF.Y };
Array.Copy(of, telemetry.of, 2);
if (monitoring.isMonitor == 1) MonitoringTimer.Start();
else MonitoringTimer.Stop();
netClient.SendData(dataDrone.SendReqest());
}
private void trackBar_Power_Scroll(object sender, EventArgs e)
{
float pow = (float)trackBar_Power.Value / 100;
rc.SetThrottle(pow);
label_Pow.Text = pow.ToString();
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
}
private void button_UU_MouseDown(object sender, MouseEventArgs e)
{
float pow = ((float)trackBar_Value.Value) / 10.0f;
if (sender == button_UU)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
}
if (sender == button_DD)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
}
if (sender == button_LL)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
if (sender == button_RR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_ML)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_MR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
}
private void button_UU_MouseUp(object sender, MouseEventArgs e)
{
trackBar_Power_Scroll(null, null);
}
private void button1_Click(object sender, EventArgs e)
{
copyDataTimer.Interval = 10;
copyDataTimer.Tick += CopyDataTimer_Tick;
tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
var t = Task.Run(() => tele_server.Open());
tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
MonitoringTimer.Interval = 25;
MonitoringTimer.Tick += MonitoringTimer_Tick;
copyDataTimer.Start();
button1.BackColor = Color.LimeGreen;
}
private void RC_roll_track_Scroll(object sender, EventArgs e)
{
rc.SetRoll(RC_roll_track.Value * Vector.TO_RAD);
float hue = 0;
float sat = 1f;
float bri = 0;
RC_roll_label.Text = $"Roll: {RC_roll_track.Value}";
if(RC_roll_track.Value > 0)
{
hue = 0;
sat = RC_roll_track.Value * 0.033f;
bri = 1f;
RC_roll_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri);
}
else if(RC_roll_track.Value < 0)
{
hue = 200;
sat = -RC_roll_track.Value * 0.033f;
bri = 1f;
RC_roll_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri);
}
else
{
RC_roll_label.BackColor = Color.White;
}
}
private void RC_pitch_track_Scroll(object sender, EventArgs e)
{
rc.SetPitch(RC_pitch_track.Value * Vector.TO_RAD);
float hue = 0;
float sat = 1f;
float bri = 0;
RC_pitch_label.Text = $"Pitch: {RC_pitch_track.Value}";
if (RC_pitch_track.Value > 0)
{
hue = 0;
sat = RC_pitch_track.Value * 0.033f;
bri = 1f;
RC_pitch_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri);
}
else if (RC_pitch_track.Value < 0)
{
hue = 200;
sat = -RC_pitch_track.Value * 0.033f;
bri = 1f;
RC_pitch_label.BackColor = UtilityUI.ColorFromHsb(hue, sat, bri);
}
else
{
RC_pitch_label.BackColor = Color.White;
}
}
private void RC_roll_label_Click(object sender, EventArgs e)
{
rc.SetRoll(RC_roll_track.Value * Vector.TO_RAD);
RC_roll_track.Value = 0;
RC_roll_label.Text = $"Roll: {RC_roll_track.Value}";
RC_roll_label.BackColor = Color.White;
}
private void RC_pitch_label_Click(object sender, EventArgs e)
{
rc.SetPitch(RC_pitch_track.Value * Vector.TO_RAD);
RC_pitch_track.Value = 0;
RC_pitch_label.Text = $"Pitch: {RC_pitch_track.Value}";
RC_pitch_label.BackColor = Color.White;
}
}
catch { }
}
private void button_Connect_Click(object sender, EventArgs e)
{
var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
switch (done)
{
case NetClient.ClientState.Error:
{
MessageBox.Show("Error connecting to server");
break;
}
case NetClient.ClientState.Connected:
{
button_Connect.Text = "Disconnect";
button_Connect.BackColor = Color.LimeGreen;
break;
}
case NetClient.ClientState.Stop:
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
break;
}
}
if (done != NetClient.ClientState.Connected) return;
}
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
{
netClient?.Close();
netClient = null;
}
private void timer_Test_Tick(object sender, EventArgs e)
{
label_Acc_X.Text = dataDrone.AccX.ToString();
label_Acc_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_time_acc.Text = dataDrone.TimeAcc.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
label_Pos_X.Text = dataDrone.PosX.ToString();
label_Pos_Y.Text = dataDrone.PosY.ToString();
label_Pos_L.Text = dataDrone.LaserRange.ToString();
label_time_range.Text = dataDrone.TimeRange.ToString();
label_OF_X.Text = dataDrone.OF.X.ToString();
label_OF_Y.Text = dataDrone.OF.Y.ToString();
label_time_of.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest());
}
private void trackBar_Power_Scroll(object sender, EventArgs e)
{
float pow = (float)trackBar_Power.Value / 100;
label_Pow.Text = pow.ToString();
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
}
private void button_UU_MouseDown(object sender, MouseEventArgs e)
{
float pow = ((float)trackBar_Value.Value) / 10.0f;
if (sender == button_UU)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
}
if (sender == button_DD)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
}
if (sender == button_LL)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
if (sender == button_RR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_ML)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_MR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
}
private void button_UU_MouseUp(object sender, MouseEventArgs e)
{
trackBar_Power_Scroll(null, null);
}
}
}

126
DroneClient/Models/MonitorContainer.cs Normal file
View File

@ -0,0 +1,126 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using TelemetryIO;
namespace DroneClient.Models
{
public class monitorItem
{
public int slotNo = 0;
public int offset = 0;
public int len = 0;
public int id = 0;
public monitorItem()
{
}
public monitorItem(int slotNo, int offset, int len, int id)
{
this.slotNo = slotNo;
this.offset = offset;
this.len = len;
this.id = id;
}
}
public class MonitorContainer
{
public const int monitorMaxFill = 32;
public int monitorFillLevel = 0;
public monitorItem[] monitorItems = new monitorItem[monitorMaxFill];
public int isMonitor = 0;
private static volatile MonitorContainer instance;
private static readonly object _lock = new object();
public static MonitorContainer Instance
{
get
{
if (instance == null)
{
lock (_lock)
{
if (instance == null)
{
instance = new MonitorContainer();
}
}
}
return instance;
}
}
private MonitorContainer()
{
for (int i = 0; i < monitorMaxFill; i++)
{
monitorItems[i] = new monitorItem();
}
}
public void resetMonitorContainer()
{
for (int i = 0; i < monitorMaxFill; i++)
{
monitorItems[i] = new monitorItem();
}
}
public int monitorPut(int slot, int offset, int len)
{
int ret_id = 0;
if (monitorFillLevel < monitorMaxFill)
{
monitorItems[monitorFillLevel].slotNo = slot;
monitorItems[monitorFillLevel].offset = offset;
monitorItems[monitorFillLevel].len = len;
monitorItems[monitorFillLevel].id = (slot << 8) + offset;
ret_id = monitorItems[monitorFillLevel].id;
monitorFillLevel++;
isMonitor = 1;
}
return ret_id;
}
public int monitorRemove(int id)
{
int match = 0;
int ret_id = 0;
for (int i = 0; i < monitorMaxFill; i++)
{
if (monitorItems[i].id == id)
{
match = 1;
if (monitorFillLevel > 0) monitorFillLevel--;
ret_id = id;
}
if (match > 0)
{
if (monitorItems[i].len == 0) return ret_id;
if (i < 31)
{
monitorItems[i].id = monitorItems[i + 1].id;
monitorItems[i].offset = monitorItems[i + 1].offset;
monitorItems[i].slotNo = monitorItems[i + 1].slotNo;
monitorItems[i].len = monitorItems[i + 1].len;
}
else
{
monitorItems[i].id = 0;
monitorItems[i].offset = 0;
monitorItems[i].slotNo = 0;
monitorItems[i].len = 0;
}
}
}
return ret_id;
}
}
}

354
DroneClient/Models/Telemetry.cs Normal file
View File

@ -0,0 +1,354 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace TelemetryIO.Models
{
public sealed class Telemetry
{
//Класс синглтон для хранения данных телеметрии
private static volatile Telemetry instance;
private static readonly object _lock = new object();
private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();//Словарь адресов, для целей мониторинга
public string name = "_150";
public float[] raw_imu = new float[6];
public float[] imu = new float[6];
public float[] filtered_imu = new float[6];
public float[] g_integration = new float[3];
public float[] euler = new float[3];
public float[] quaternion = new float[4];
public float[] 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;
}
}
}

63
DroneClient/Properties/Resources.Designer.cs generated Normal file
View File

@ -0,0 +1,63 @@
//------------------------------------------------------------------------------
// <auto-generated>
// Этот код создан программой.
// Исполняемая версия:4.0.30319.42000
//
// Изменения в этом файле могут привести к неправильной работе и будут потеряны в случае
// повторной генерации кода.
// </auto-generated>
//------------------------------------------------------------------------------
namespace DroneClient.Properties {
using System;
/// <summary>
/// Класс ресурса со строгой типизацией для поиска локализованных строк и т.д.
/// </summary>
// Этот класс создан автоматически классом StronglyTypedResourceBuilder
// с помощью такого средства, как ResGen или Visual Studio.
// Чтобы добавить или удалить член, измените файл .ResX и снова запустите ResGen
// с параметром /str или перестройте свой проект VS.
[global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "17.0.0.0")]
[global::System.Diagnostics.DebuggerNonUserCodeAttribute()]
[global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
internal class Resources {
private static global::System.Resources.ResourceManager resourceMan;
private static global::System.Globalization.CultureInfo resourceCulture;
[global::System.Diagnostics.CodeAnalysis.SuppressMessageAttribute("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
internal Resources() {
}
/// <summary>
/// Возвращает кэшированный экземпляр ResourceManager, использованный этим классом.
/// </summary>
[global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
internal static global::System.Resources.ResourceManager ResourceManager {
get {
if (object.ReferenceEquals(resourceMan, null)) {
global::System.Resources.ResourceManager temp = new global::System.Resources.ResourceManager("DroneClient.Properties.Resources", typeof(Resources).Assembly);
resourceMan = temp;
}
return resourceMan;
}
}
/// <summary>
/// Перезаписывает свойство CurrentUICulture текущего потока для всех
/// обращений к ресурсу с помощью этого класса ресурса со строгой типизацией.
/// </summary>
[global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
internal static global::System.Globalization.CultureInfo Culture {
get {
return resourceCulture;
}
set {
resourceCulture = value;
}
}
}
}

120
DroneClient/Properties/Resources.resx Normal file
View File

@ -0,0 +1,120 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
</root>

258
DroneClient/TelemetryServer.cs Normal file
View File

@ -0,0 +1,258 @@
using DroneClient.Models;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TelemetryIO;
using TelemetryIO.Models;
namespace TelemetryIO
{
internal class TelemetryServer : TelemetryIO.BaseCommHandler
{
ConcurrentQueue<byte[]> sendQueue = new ConcurrentQueue<byte[]>();
Telemetry telemetry = Telemetry.Instance;
MonitorContainer monitoring = MonitorContainer.Instance;
private bool isClientConnected = false;
public event EventHandler OnTeleClientConnected = delegate { };
public event EventHandler OnTeleClientDisconnected = delegate { };
public bool isClientAvailable { get { return isClientConnected; } }
public TelemetryServer()
{
}
public override void Close()
{
throw new NotImplementedException();
}
public override void CloseConnection()
{
throw new NotImplementedException();
}
public override bool IsOpen()
{
return true;
}
public async override Task Open()
{
await StartServerAsync();
//Console.ReadLine();
}
public async Task StartServerAsync()
{
var listener = new TcpListener(IPAddress.Any, Port);
listener.Start();
Console.WriteLine("Server is running");
try
{
while (true)
{
//if (isClientConnected) await Task.Delay(25);
var client = await listener.AcceptTcpClientAsync();
StrikeOnTeleClientConnected();
isClientConnected = true;
Debug.WriteLine("Client is connected");
//this.client = client;
var readTask = StartReadingAsync(client);
var writeTask = SendMessageAsync(client);
await readTask;
await writeTask;
}
}
catch (IOException ex) when (IsNetworkError(ex))
{
// Специальная обработка сетевых ошибок
Debug.WriteLine($"Ошибка при запуске сервера: {ex.Message}");
}
catch (Exception ex)
{
Debug.WriteLine("An error occurred: " + ex.Message);
}
finally
{
listener.Stop();
}
}
public async Task SendMessageAsync(TcpClient c)
{
if (c.GetStream() == null || !c.Connected)
{
Console.WriteLine("Not connected to server");
return;
}
while (c.Connected)
{
if (sendQueue.TryDequeue(out byte[] buffer))
{
try
{
await c.GetStream().WriteAsync(buffer, 0, buffer.Length);
Debug.WriteLine($"Sent");
}
catch (IOException ex) when (IsNetworkError(ex))
{
// Специальная обработка сетевых ошибок
Debug.WriteLine($"Клиент отключился: {ex.Message}");
HandleDisconnectedClient(c);
}
catch (Exception ex)
{
Debug.WriteLine($"Error sending message: {ex.Message}");
}
}
}
}
public async override Task StartReadingAsync(object C)
{
TcpClient client;
if (C is TcpClient) client = (TcpClient)C;
else
{
return;
}
try
{
using NetworkStream stream = client.GetStream();
while (client.Connected)
{
byte[] buffer = new byte[1024];
int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length);
//string response = await reader.ReadLineAsync();
if (bytesRead > 0)
{
EnqueueData(buffer, bytesRead);
}
data_extract();
}
}
catch (IOException ex) when (IsNetworkError(ex))
{
// Специальная обработка сетевых ошибок
Debug.WriteLine($"Клиент отключился: {ex.Message}");
HandleDisconnectedClient(client);
}
catch (Exception ex)
{
Debug.WriteLine($"Error sending message: {ex.Message}");
}
}
protected override void ProcessCommand(int cmd, int slot, byte[] data, int offset, int len)
{
Debug.WriteLine(@"server cmd = " + cmd);
switch (cmd)
{
case TELE_CMD_HELLO:
byte[] load = telemetry.getSlot(cmd, offset, len);
putRequest(prepareTelegram(cmd, 0, load, 0, load.Length));
break;
case TELE_CMD_RD_ONCE:
byte[] load1 = telemetry.getSlot(slot, offset, len);
putRequest(prepareTelegram(cmd, slot, load1, offset, load1.Length));
break;
case TELE_CMD_WR:
setData(data, slot, offset, len);
sendVoidAnswer(TELE_CMD_WR);
break;
case TELE_CMD_RD_MON_ON:
monitoring.isMonitor = 1;
sendVoidAnswer(TELE_CMD_RD_MON_ON);
break;
case TELE_CMD_RD_MON_OFF:
monitoring.isMonitor = 0;
sendVoidAnswer(TELE_CMD_RD_MON_OFF);
break;
case TELE_CMD_RD_MON_ADD:
int id = monitoring.monitorPut(slot, offset, 4);
sendIntAnswer(TELE_CMD_RD_MON_ADD, id);
monitoring.isMonitor = 1;
break;
case TELE_CMD_RD_MON_REMOVE:
id = monitoring.monitorRemove(offset);//in this case offset == id
if(monitoring.monitorFillLevel == 0)monitoring.isMonitor = 0;//if monitor_fill_level == 0 there is no item to monitor
if (monitoring.isMonitor > 0) sendIntAnswer(TELE_CMD_RD_MON_REMOVE, id);
else sendVoidAnswer(TELE_CMD_RD_MON_REMOVEALL);
break;
}
}
private void setData(byte[] load, int slot, int offset, int len)
{
telemetry.setSlot(slot, load, offset, len);
}
private void sendVoidAnswer(int command)
{
putRequest(prepareTelegram(command, 0, new byte[0], 0));
}
private void sendIntAnswer(int command, int param)
{
putRequest(prepareTelegram(command, 0, new byte[0], param));
}
protected override void sendData(byte[] data)
{
sendQueue.Enqueue(data);
}
public override void setCommParams(iCommParams commParams)
{
if (commParams.GetType() == typeof(TCPCommParams))
{
var comm = (TCPCommParams)commParams;
IP = comm.IP;
Port = comm.Port;
}
}
private void HandleDisconnectedClient(TcpClient client)
{
isClientConnected = false;
StrikeOnTeleClientDisconnected();
}
private bool IsNetworkError(Exception ex)
{
// Проверяем типичные сетевые ошибки при разрыве соединения
return ex is IOException
|| ex is SocketException
|| ex.InnerException is SocketException;
}
public void StrikeOnTeleClientConnected()
{
OnTeleClientConnected?.Invoke(this, EventArgs.Empty);
}
public void StrikeOnTeleClientDisconnected()
{
OnTeleClientDisconnected?.Invoke(this, EventArgs.Empty);
}
}
}

BIN
DroneClient/images/connect.ico Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 102 KiB

BIN
DroneClient/images/disconnect.ico Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 101 KiB

88
DroneClient/utils/AdaptivePID.cs Normal file
View File

@ -0,0 +1,88 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace DroneClient.utils
{
internal class AdaptivePID
{
//Initial
private float P = 1;
private float I = 0.2f;
private float D = 0.005f;
//Adaptive
private float aP = 1;
private float aI = 0.2f;
private float aD = 0.005f;
private float P_min = 0; private float P_max = 10;
private float I_min = 0; private float I_max = 1;
private float D_min = 0; private float D_max = 0.1f;
private float alpha = 0.001f;
private float beta = 0.001f;
private float gamma = 0.0005f;
private float prev_error = float.NaN;
private float integral = 0;
public AdaptivePID()
{
alpha = 0; beta = 0; gamma = 0;
}
public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f)
{
P = p; aP = p;
I = i; aI = i;
D = d; aD = d;
this.alpha = alpha;
this.beta = beta;
this.gamma = gamma;
}
public float process(float error, float T)
{
if (T <= 0) return 0;
integral += error * T;
float derivative = 0;
if (float.IsFinite(prev_error))
{
derivative = (error - prev_error) / T;
}
prev_error = error;
float dP = -alpha * error * error;
float dI = -beta * error * integral;
float dD = -gamma * error * derivative;
aP += dP; aI += dI; aD += dD;
aP = constrain(aP, P_min, P_max);
aI = constrain(aI, I_min, I_max);
aD = constrain(aD, D_min, D_max);
integral = constrain(integral, -10, 10);
float output = aP * error + aI * integral + aD * derivative;
return output;
}
public void reset()
{
integral = 0;
prev_error = 0;
aP = P; aI = I; aD = D;
}
static float constrain(float val, float min, float max)
{
if (min >= max) return val;
if(val < min) return min;
if(val > max) return max;
return val;
}
}
}

198
DroneClient/utils/DroneOrientation.cs Normal file
View File

@ -0,0 +1,198 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using TelemetryIO.Models;
namespace DroneClient.utils
{
internal static class DroneOrientation
{
public static float AccX, AccY, AccZ;
public static float GyrX, GyrY, GyrZ;
public static uint TimeAcc, TimeGyr;
private static bool isFirstCycle = true;
public static uint prevTimeAcc, prevTimeGyr;
private static Telemetry telemetry;
public static float PosX, PosY;
public static float LaserRange;
public static uint TimeRange;
public static uint prevTimeRange;
public static Vector2 OF = Vector2.Zero;
public static float MotorUL, MotorUR, MotorDL, MotorDR;
public const float K_ACC = 0.1f;
public static Quaternion attitude = new();
/***************************************************************/
private static Quaternion attitudeTarget = new();
private static Vector ratesTarget = new();
private static Vector torqueTarget = new();
private static float thrustTarget = 0;
/***************************************************************/
private static AdaptivePID RollPid = new();
private static AdaptivePID PitchPid = new();
//private static AdaptivePID YawPid = new();
private static AdaptivePID RatesRollPid = new(telemetry.pid0[0], telemetry.pid0[1], telemetry.pid0[2], 0.0f, 0, 0);
private static AdaptivePID RatesPitchPid = new(telemetry.pid0[3], telemetry.pid0[4], telemetry.pid0[5], 0.01f, 0.05f, 0.001f);
private static AdaptivePID RatesYawPid = new(telemetry.pid0[6], telemetry.pid0[7], telemetry.pid0[8], 0.01f, 0.05f, 0.001f);
/***************************************************************/
private static float tim1s = 0;
/// <summary>
/// Вычисление текущей ориентации дрона
/// </summary>
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));
}
/// <summary>
/// Вычисление ошибки как в flix
/// </summary>
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);
}
/// <summary>
/// Вычисление ошибки через форму ось-угол
/// </summary>
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];
}
}
}

178
DroneClient/utils/Quaternion.cs Normal file
View File

@ -0,0 +1,178 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
namespace DroneClient.utils
{
internal class Quaternion
{
private float w;
private float x;
private float y;
private float z;
public float W
{
get => w;
private set => w = value;
}
public float X
{
get => x;
private set => x = value;
}
public float Y
{
get => y;
private set => y = value;
}
public float Z
{
get => z;
private set => z = value;
}
public Quaternion()
{
w = 1;
x = y = z = 0;
}
public Quaternion(float w, float x, float y, float z)
{
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
public bool IsZero()
{
return w == 0 && x == 0 && y == 0 && z == 0;
}
public float getNorm()
{
if (IsZero()) return 0;
return (float)Math.Sqrt(w * w + x * x + y * y + z * z);
}
public void Normalize()
{
float norm = getNorm();
if (norm != 0)
{
w /= norm;
x /= norm;
y /= norm;
z /= norm;
}
}
static public Quaternion FromRotationVector(Vector Va)
{
float a = Va.getNorm();
if (a == 0)
{
return new Quaternion();
}
Quaternion w = new Quaternion();
float cos = (float)Math.Cos(a / 2);
float sin = (float)Math.Sin(a / 2);
w.w = cos;
w.x = Va.X / a * sin;
w.y = Va.Y / a * sin;
w.z = Va.Z / a * sin;
return w;
}
static public Quaternion FromEulerAngles(Vector euler)
{
float halfphi = euler.X / 2;
float halftheta = euler.Y / 2;
float halfpsi= euler.Z / 2;
float cosx = (float)Math.Cos(halfphi);
float sinx = (float)Math.Sin(halfphi);
float cosy = (float)Math.Cos(halftheta);
float siny = (float)Math.Sin(halftheta);
float cosz = (float)Math.Cos(halfpsi);
float sinz = (float)Math.Sin(halfpsi);
return new Quaternion(
cosx * cosy * cosz + sinx * siny * sinz,
sinx * cosy * cosz - cosx * siny * sinz,
cosx * siny * cosz + sinx * cosy * sinz,
cosx * cosy * sinz - sinx * siny * cosz);
}
static public Quaternion Copy(Quaternion q)
{
return new Quaternion(q.w, q.x, q.y, q.z);
}
public Quaternion getInverse()
{
float n = getNorm();
return new Quaternion(w / n, -x / n, -y / n, -z / n);
}
static public Quaternion Mul(Quaternion q1, Quaternion q2)
{
Quaternion res = new Quaternion();
res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
return res;
}
public void UpdateByRates(Vector w, float dt)
{
Quaternion omega = new Quaternion(0, w.X, w.Y, w.Z);
Quaternion res = Mul(this, omega);
this.w += res.w * 0.5f * dt;
this.x += res.x * 0.5f * dt;
this.y += res.y * 0.5f * dt;
this.z += res.z * 0.5f * dt;
}
static public Vector RotateVector(Vector a, Quaternion q)
{
Quaternion fromVector = new Quaternion(0, a.X, a.Y, a.Z);
Quaternion inverse = q.getInverse();
Quaternion r = Mul(q, fromVector);
r = Mul(r, inverse);
return new Vector(r.x, r.y, r.z);
}
static public Quaternion QuaternionBetweenVectors(Vector a, Vector b)
{
Quaternion res = new Quaternion();
Vector mul_v = Vector.Mul_Vector(a, b);
//Если векторы коллинеарны, то нет поворота
if (mul_v.IsZero()) return res;
float mul_s = Vector.Mul_Scalar(a, b);
//Хотя бы один из векторов нулевой => кватернион неопределен, но мы возвращаем (1, 0, 0, 0)
if (mul_s == 0) return res;
float norm_scalar = a.getNorm() * b.getNorm() + mul_s;
float normQ = (float)Math.Sqrt(norm_scalar * norm_scalar + mul_v.getNorm() * mul_v.getNorm());
res.w = norm_scalar / normQ;
res.x = mul_v.X / normQ;
res.y = mul_v.Y / normQ;
res.z = mul_v.Z / normQ;
return res;
}
override public string ToString()
{
return $"({w}, {x}, {y}, {z})";
}
}
}

45
DroneClient/utils/RC.cs Normal file
View File

@ -0,0 +1,45 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace DroneClient.utils
{
internal class RC
{
private Vector control = new();
private float throttle = 0;
public void SetControl(float roll, float pitch, float yaw)
{
control.X = roll;
control.Y = pitch;
control.Z = yaw;
}
public void SetRoll(float roll)
{
control.X = roll;
}
public void SetPitch(float pitch)
{
control.Y = pitch;
}
public void SetYaw(float yaw)
{
control.Z = yaw;
}
public void SetThrottle(float throttle)
{
this.throttle = throttle;
}
public Vector GetControl() { return control; }
public float GetThrottle() { return throttle; }
}
}

74
DroneClient/utils/UtilityUI.cs Normal file
View File

@ -0,0 +1,74 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace DroneClient.utils
{
static class UtilityUI
{
public static Color ColorFromHsb(float hue, float saturation, float brightness)
{
// Ограничиваем значения параметров
hue = Math.Clamp(hue, 0, 360);
saturation = Math.Clamp(saturation, 0, 1);
brightness = Math.Clamp(brightness, 0, 1);
float c = brightness * saturation;
float x = c * (1 - Math.Abs((hue / 60) % 2 - 1));
float m = brightness - c;
float r, g, b;
if (hue < 60) { r = c; g = x; b = 0; }
else if (hue < 120) { r = x; g = c; b = 0; }
else if (hue < 180) { r = 0; g = c; b = x; }
else if (hue < 240) { r = 0; g = x; b = c; }
else if (hue < 300) { r = x; g = 0; b = c; }
else { r = c; g = 0; b = x; }
return Color.FromArgb(
(int)((r + m) * 255),
(int)((g + m) * 255),
(int)((b + m) * 255));
}
public static void ColorToHsb(Color color, out float hue, out float saturation, out float brightness)
{
float r = color.R / 255f;
float g = color.G / 255f;
float b = color.B / 255f;
float max = Math.Max(r, Math.Max(g, b));
float min = Math.Min(r, Math.Min(g, b));
float delta = max - min;
// Hue calculation
if (delta == 0)
{
hue = 0;
}
else if (max == r)
{
hue = 60 * (((g - b) / delta) % 6);
}
else if (max == g)
{
hue = 60 * (((b - r) / delta) + 2);
}
else
{
hue = 60 * (((r - g) / delta) + 4);
}
if (hue < 0) hue += 360;
// Saturation calculation
saturation = max == 0 ? 0 : delta / max;
// Brightness/Value
brightness = max;
}
}
}

148
DroneClient/utils/Vector.cs Normal file
View File

@ -0,0 +1,148 @@
using System;
using System.Collections.Generic;
using System.Data;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
namespace DroneClient.utils
{
internal class Vector
{
public const float TO_GRAD = (float)(180 / Math.PI);
public const float TO_RAD = (float)(Math.PI / 180);
private float x, y, z;
public float X
{
get => x;
set => x = value;
}
public float Y
{
get => y;
set => y = value;
}
public float Z
{
get => z;
set => z = value;
}
public Vector() { }
public Vector(float x, float y, float z)
{
this.x = x;
this.y = y;
this.z = z;
}
public bool IsZero()
{
return x == 0 && y == 0 && z == 0;
}
public float getNorm()
{
if (IsZero()) return 0;
return (float)Math.Sqrt(x * x + y * y + z * z);
}
public void Normalize()
{
if (IsZero()) return;
float norm = getNorm();
x /= norm;
y /= norm;
z /= norm;
}
static public Vector operator *(float a, Vector v)
{
return new Vector(a * v.x, a * v.y, a * v.z);
}
static public Vector operator *(Vector v, float a)
{
return new Vector(a * v.x, a * v.y, a * v.z);
}
static public Vector operator -(Vector a, Vector b)
{
return new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
}
static public float Mul_Scalar(Vector a, Vector b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
static public Vector Mul_Vector(Vector a, Vector b)
{
return new Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
static public float AngleBetweenVectors(Vector a, Vector b)
{
float mul_scalar = Mul_Scalar(a, b);
float normA = a.getNorm();
float normB = b.getNorm();
if ((normA == 0) || (normB == 0))
{
return 0;
}
else
{
return (float)Math.Acos(Vector.constraint(mul_scalar / normA / normB));
}
}
static public float AngleBetweenVectors2(Vector a, Vector b)
{
float normAB = Mul_Vector(a, b).getNorm();
float normA = a.getNorm();
float normB = b.getNorm();
if ((normA == 0) || (normB == 0))
{
return 0;
}
else
{
return (float)Math.Asin(Vector.constraint(normAB / normA / normB));
}
}
static public Vector getRotationVector(Vector a, Vector b)
{
Vector n = Vector.Mul_Vector(a, b);
if (n.IsZero())
{
return Vector.Mul_Vector(a, new Vector(1, 0, 0));
}
n.Normalize();
return Vector.AngleBetweenVectors(a, b) * n;
}
///<summary>
///Ограничивает value в пределах между low и high. Если low >= high будет возвращено value.
///</summary>
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})";
}
}
}

BIN
connect.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.0 KiB

BIN
disconnect.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.0 KiB