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

update

Browse Source
This commit is contained in:
Sergey Sklyarov 2025-04-06 17:17:15 +03:00
parent fd2318cbf6
commit da7f5a8404
7 changed files with 233 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
Common/DroneData.cs Normal file
View File

@ -0,0 +1,42 @@
namespace DroneData
{
enum StructType : ulong
{
// Output
DataIMU = 1, DataPos = 2,
// Input
DataMotor4 = 1001, DataMotor6 = 1002
};
public struct DataInfo
{
StructType Type;
ulong Size;
}
public struct DataXYZ { float X, Y, Z; }
public struct DataIMU
{
DataXYZ Acc, Gyr, Mag;
}
public struct DataPos
{
DataXYZ Local; // Ëîêàëüíûå êîîðäèíàòû
float LiDAR; // Äàò÷èê ïîñàäêè
}
public struct DataMotor4
{
ulong Count;
float M1, M2, M3, M4;
}
public struct DataMotor6
{
ulong Count;
float M1, M2, M3, M4, M5, M6;
}
}

42
Common/DroneData.h Normal file
View File

@ -0,0 +1,42 @@
namespace DroneData
{
enum class StructType : unsigned long
{
// Output
DataIMU = 1, DataPos = 2,
// Input
DataMotor4 = 1001, DataMotor6 = 1002
};
public struct DataInfo
{
StructType Type;
unsigned long Size;
};
public struct DataXYZ { float X, Y, Z; };
public struct DataIMU
{
DataXYZ Acc, Gyr, Mag;
};
public struct DataPos
{
DataXYZ Local; // Ëîêàëüíûå êîîðäèíàòû
float LiDAR; // Äàò÷èê ïîñàäêè
};
public struct DataMotor4
{
ulong Count;
float M1, M2, M3, M4;
};
public struct DataMotor6
{
ulong Count;
float M1, M2, M3, M4, M5, M6;
};
}

4
DroneClient/NetClient.cs
View File

@ -41,9 +41,7 @@ namespace DroneSimulator
{
if (Connected)
{
try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
ServerSocket?.Close();
Connected = false;
Close();
return ClientState.Stop;
}

87
DroneSimulator/Drone.cs
View File

@ -1,5 +1,6 @@
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
namespace DroneSimulator
{
@ -12,8 +13,8 @@ namespace DroneSimulator
public const float Dynamic = 10; // Динамика вращения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион
public float Power = 0; // Тяга всех двигателей
public float MaxPower; // Тяга всех двигателей
public float Power = 0; // Тяга всех двигателей (0-1)
public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
@ -70,7 +71,7 @@ namespace DroneSimulator
return mem;
}
public struct DataOut
/*public struct DataOut
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
@ -95,7 +96,7 @@ namespace DroneSimulator
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
}
}*/
public struct DataVisual
{
@ -113,9 +114,7 @@ namespace DroneSimulator
{
while (DroneThread != null)
{
float time = Environment.TickCount - Timer;
Timer = Environment.TickCount;
Action(time / 1000);
Action(Environment.TickCount);
Thread.Sleep(1);
}
}
@ -191,8 +190,11 @@ namespace DroneSimulator
return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
}
public void Action(float time)
public void Action(int tick)
{
float time = (tick - Timer) / 1000.0f;
Timer = tick;
if (!Active) return;
float flow = Power;
@ -206,7 +208,7 @@ namespace DroneSimulator
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
PosXYZ += SpdXYZ * time;
@ -281,5 +283,72 @@ namespace DroneSimulator
AccPRY.X = ((ul + ur) - (dl + dr));
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)
{
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));
}
public byte[] SendDataStream(DroneData.StructType type)
{
switch(type)
{
case DroneData.StructType.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);
}
case DroneData.StructType.DataPos:
{
DroneData.DataPos pos = new DroneData.DataPos();
pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
pos.LiDAR = LaserRange;
return getBytes(pos);
}
}
return null;
}
}
}

44
DroneSimulator/DroneData.cs Normal file
View File

@ -0,0 +1,44 @@
namespace DroneData
{
public enum StructType : ulong
{
None = 0, Head = 1,
// Output
DataIMU = 1001, DataPos = 1002,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
};
public struct DataHead
{
public StructType Type;
public ulong Size;
}
public struct XYZ { public float X, Y, Z; }
public struct DataIMU
{
public XYZ Acc, Gyr, Mag;
}
public struct DataPos
{
public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû
public float LiDAR; // Äàò÷èê ïîñàäêè
}
public struct DataMotor4
{
public ulong Count;
public float UL, UR, DL, DR;
}
public struct DataMotor6
{
public ulong Count;
public float UL, UR, LL, RR, DL, DR;
}
}

24
DroneSimulator/FormMain.cs
View File

@ -4,6 +4,8 @@ using System.Windows.Forms;
using static System.Net.Mime.MediaTypeNames;
using static System.Runtime.InteropServices.JavaScript.JSType;
using System.Security.Policy;
using System.Runtime.InteropServices;
using System.CodeDom;
namespace DroneSimulator
{
@ -21,7 +23,7 @@ namespace DroneSimulator
InitializeComponent();
}
private void ClientConnectionCallback(object o)
private int ClientConnectionCallback(object o)
{
NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
@ -52,9 +54,11 @@ namespace DroneSimulator
break;
}
}
return Marshal.SizeOf(typeof(DroneData.DataHead));
}
private void ClientReceiveCallback(object o)
private int ClientReceiveCallback(object o)
{
NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
@ -67,16 +71,18 @@ namespace DroneSimulator
break;
}
if (drone == null) return;
if (drone == null) return 0;
Drone.DataIn id = (Drone.DataIn)Drone.fromBytes(data.Buffer, typeof(Drone.DataIn));
int size=drone.RecvDataStream(data.Buffer);
drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
Drone.DataOut od = drone.GetDataOut();
try { data.Client.Send(Drone.getBytes(od)); }
try
{
data.Client.Send(drone.SendDataStream(DroneData.StructType.DataIMU));
data.Client.Send(drone.SendDataStream(DroneData.StructType.DataPos));
}
catch { }
return size;
}
private void button_Client_Start_Click(object sender, EventArgs e)

18
DroneSimulator/NetServerClients.cs
View File

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