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

update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-04-06 20:56:02 +03:00
parent da7f5a8404
commit 21b60b7f4c
9 changed files with 498 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

180
DroneSimulator/Drone.cs
View File

@ -1,4 +1,5 @@
using System.Numerics;
using System.CodeDom;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
@ -284,71 +285,142 @@ namespace DroneSimulator
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
}
private DroneData.DataHead StreamHead = new DroneData.DataHead() { Type = DroneData.StructType.None, Size = 0 };
public int RecvDataStream(byte[] data)
private void RecvDataMotor4(byte[] data)
{
if (StreamHead.Type == DroneData.StructType.None) return Marshal.SizeOf(typeof(DroneData.DataHead));
if (StreamHead.Type == DroneData.StructType.Head)
{
StreamHead = (DroneData.DataHead)fromBytes(data, typeof(DroneData.DataHead));
return (int)StreamHead.Size;
}
switch(StreamHead.Type)
{
case DroneData.StructType.DataIMU:
{
DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
/* обработка */
break;
}
case DroneData.StructType.DataPos:
{
DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos));
/* обработка */
break;
}
case DroneData.StructType.DataMotor4:
{
DroneData.DataMotor4 pos = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
/* обработка */
SetQadroPow(pos.UL, pos.UR, pos.DL, pos.DR);
break;
}
}
return Marshal.SizeOf(typeof(DroneData.DataHead));
DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
/* обработка */
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
}
public byte[] SendDataStream(DroneData.StructType type)
private byte[] SendDataIMU()
{
switch(type)
DroneData.DataIMU imu = new DroneData.DataIMU();
imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
imu.Head.Mode = DroneData.DataMode.Response;
imu.Head.Type = DroneData.DataType.DataIMU;
imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;
return getBytes(imu);
}
private byte[] SendDataPos()
{
DroneData.DataPos pos = new DroneData.DataPos();
pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
pos.Head.Mode = DroneData.DataMode.Response;
pos.Head.Type = DroneData.DataType.DataPos;
pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
pos.LiDAR = LaserRange;
return getBytes(pos);
}
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
switch (head.Type)
{
case DroneData.StructType.DataIMU:
case DroneData.DataType.DataIMU:
{
DroneData.DataIMU imu=new DroneData.DataIMU();
imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;
return getBytes(imu);
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataIMU();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.StructType.DataPos:
case DroneData.DataType.DataPos:
{
DroneData.DataPos pos = new DroneData.DataPos();
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataPos();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
pos.LiDAR = LaserRange;
return getBytes(pos);
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
RecvDataMotor4(body);
return zero;
}
}
}
return null;
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(ServerRequestResponse(DroneStreamHead, body));
}
return ret;
}
}
}

29
DroneSimulator/DroneData.cs
View File

@ -1,6 +1,13 @@
using System.Runtime.InteropServices;
namespace DroneData
{
public enum StructType : ulong
public enum DataMode : ushort
{
None = 0, Response = 1, Request = 2
};
public enum DataType : ushort
{
None = 0, Head = 1,
@ -13,32 +20,48 @@ namespace DroneData
public struct DataHead
{
public StructType Type;
public ulong Size;
public int Size;
public DataMode Mode;
public DataType Type;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct DataIMU
{
public DataHead Head;
public XYZ Acc, Gyr, Mag;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
}
public struct DataPos
{
public DataHead Head;
public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataPos));
}
public struct DataMotor4
{
public DataHead Head;
public ulong Count;
public float UL, UR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
}
public struct DataMotor6
{
public DataHead Head;
public ulong Count;
public float UL, UR, LL, RR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
}
}

22
DroneSimulator/FormMain.cs
View File

@ -6,6 +6,7 @@ using static System.Runtime.InteropServices.JavaScript.JSType;
using System.Security.Policy;
using System.Runtime.InteropServices;
using System.CodeDom;
using System.Linq;
namespace DroneSimulator
{
@ -23,7 +24,7 @@ namespace DroneSimulator
InitializeComponent();
}
private int ClientConnectionCallback(object o)
private void ClientConnectionCallback(object o)
{
NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
@ -54,11 +55,9 @@ namespace DroneSimulator
break;
}
}
return Marshal.SizeOf(typeof(DroneData.DataHead));
}
private int ClientReceiveCallback(object o)
private void ClientReceiveCallback(object o)
{
NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
@ -71,18 +70,19 @@ namespace DroneSimulator
break;
}
if (drone == null) return 0;
if (drone == null) return;
int size=drone.RecvDataStream(data.Buffer);
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
if (send == null) return;
try
{
data.Client.Send(drone.SendDataStream(DroneData.StructType.DataIMU));
data.Client.Send(drone.SendDataStream(DroneData.StructType.DataPos));
{
foreach (byte[]? b in send)
{
if (b != null) data.Client?.Send(b);
}
}
catch { }
return size;
}
private void button_Client_Start_Click(object sender, EventArgs e)

20
DroneSimulator/NetServerClients.cs
View File

@ -17,8 +17,9 @@ namespace DroneSimulator
public class ReceiveData
{
public int ID;
public byte[]? Buffer;
public byte[] Buffer;
public int Size;
public Socket? Client;
@ -28,7 +29,8 @@ namespace DroneSimulator
{
public int ID;
public Socket? workSocket = null;
public byte[] buffer;
public const int count = 1024;
public byte[] buffer = new byte[count];
}
private int SocketID = 0;
@ -36,7 +38,7 @@ namespace DroneSimulator
private Socket? ServerSocket;
private List<ClientData> ClientSockets = new List<ClientData>();
public delegate int ServerCallback(object o);
public delegate void ServerCallback(object o);
private ServerCallback? ConnectionCallback;
private ServerCallback? ReceiveCallback;
@ -99,11 +101,9 @@ namespace DroneSimulator
ClientSockets.Add(clientData);
int size = ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
clientData.buffer=new byte[size];
handler.BeginReceive(clientData.buffer, 0, size, 0, new AsyncCallback(ReadCallback), clientData);
handler.BeginReceive(clientData.buffer, 0, ClientData.count, 0, new AsyncCallback(ReadCallback), clientData);
}
else handler.Close();
@ -130,13 +130,11 @@ namespace DroneSimulator
return;
}
int size = ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
cd.buffer = new byte[size];
ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
try
{
cd.workSocket?.BeginReceive(cd.buffer, 0, size, 0, new AsyncCallback(ReadCallback), cd);
cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.count, 0, new AsyncCallback(ReadCallback), cd);
}
catch { }
}