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

revert 1f8bde2370

Browse Source 
revert Vector, Quaternion - математика для ориентации. AdaptivePID - ПИД-регулятор. DroneOrientation.CalcAttitude() - цикл вычисления положения.
This commit is contained in:
sasklyarov
2025-08-04 18:02:30 +00:00
parent e30f1f0f35
commit a67275b402
21 changed files with 1149 additions and 3780 deletions
Download Patch File Download Diff File Expand all files Collapse all files

743
DroneClient/Drone.cs
View File

@ -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<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;
}
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<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;
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;
}
}
}