diff --git a/Common/DroneData.cs b/Common/DroneData.cs new file mode 100644 index 0000000..b442bb9 --- /dev/null +++ b/Common/DroneData.cs @@ -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; + } +} \ No newline at end of file diff --git a/Common/DroneData.h b/Common/DroneData.h new file mode 100644 index 0000000..14d3512 --- /dev/null +++ b/Common/DroneData.h @@ -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; + }; +} \ No newline at end of file diff --git a/DroneClient/NetClient.cs b/DroneClient/NetClient.cs index f343708..fc93baf 100644 --- a/DroneClient/NetClient.cs +++ b/DroneClient/NetClient.cs @@ -41,9 +41,7 @@ namespace DroneSimulator { if (Connected) { - try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { } - ServerSocket?.Close(); - Connected = false; + Close(); return ClientState.Stop; } diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 45d0e39..612505a 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -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; + } } } diff --git a/DroneSimulator/DroneData.cs b/DroneSimulator/DroneData.cs new file mode 100644 index 0000000..448299b --- /dev/null +++ b/DroneSimulator/DroneData.cs @@ -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; + } +} \ No newline at end of file diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 862c70c..84c70ab 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -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) diff --git a/DroneSimulator/NetServerClients.cs b/DroneSimulator/NetServerClients.cs index 8c28f73..1e1d30d 100644 --- a/DroneSimulator/NetServerClients.cs +++ b/DroneSimulator/NetServerClients.cs @@ -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 ClientSockets = new List(); - 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 { } }