From 21b60b7f4c4ec6193e23e4a30748d8dfda977e38 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Sun, 6 Apr 2025 20:56:02 +0300 Subject: [PATCH] update --- Common/DroneData.cs | 61 +++++++--- DroneClient/Drone.cs | 180 +++++++++++++++++++++++++++-- DroneClient/DroneData.cs | 67 +++++++++++ DroneClient/FormMain.cs | 81 +++++++------ DroneClient/NetClient.cs | 5 + DroneSimulator/Drone.cs | 180 ++++++++++++++++++++--------- DroneSimulator/DroneData.cs | 29 ++++- DroneSimulator/FormMain.cs | 22 ++-- DroneSimulator/NetServerClients.cs | 20 ++-- 9 files changed, 498 insertions(+), 147 deletions(-) create mode 100644 DroneClient/DroneData.cs diff --git a/Common/DroneData.cs b/Common/DroneData.cs index b442bb9..8c14e2c 100644 --- a/Common/DroneData.cs +++ b/Common/DroneData.cs @@ -1,42 +1,67 @@ +using System.Runtime.InteropServices; + namespace DroneData { - enum StructType : ulong - { - // Output - DataIMU = 1, DataPos = 2, - - // Input - DataMotor4 = 1001, DataMotor6 = 1002 + public enum DataMode : ushort + { + None = 0, Response = 1, Request = 2 }; - public struct DataInfo + public enum DataType : ushort + { + None = 0, Head = 1, + + // Output + DataIMU = 1001, DataPos = 1002, + + // Input + DataMotor4 = 2001, DataMotor6 = 2002 + }; + + public struct DataHead { - StructType Type; - ulong Size; + public int Size; + + public DataMode Mode; + public DataType Type; + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); } - public struct DataXYZ { float X, Y, Z; } + public struct XYZ { public float X, Y, Z; } public struct DataIMU { - DataXYZ Acc, Gyr, Mag; + public DataHead Head; + public XYZ Acc, Gyr, Mag; + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU)); } public struct DataPos { - DataXYZ Local; // - float LiDAR; // + public DataHead Head; + public XYZ Local; // + public float LiDAR; // + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataPos)); } public struct DataMotor4 { - ulong Count; - float M1, M2, M3, M4; + public DataHead Head; + public ulong Count; + public float UL, UR, DL, DR; + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4)); } public struct DataMotor6 { - ulong Count; - float M1, M2, M3, M4, M5, M6; + public DataHead Head; + public ulong Count; + public float UL, UR, LL, RR, DL, DR; + + static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); } } \ No newline at end of file diff --git a/DroneClient/Drone.cs b/DroneClient/Drone.cs index 87eac05..74faa8b 100644 --- a/DroneClient/Drone.cs +++ b/DroneClient/Drone.cs @@ -1,21 +1,17 @@ -using System.Runtime.InteropServices; +using System.Drawing; +using System.Runtime.InteropServices; namespace DroneClient { internal class Drone { - public struct DataOut - { - public float AccX, AccY, AccZ; - public float GyrX, GyrY, GyrZ; - public float PosX, PosY; - public float LaserRange; - } - public struct DataIn - { - public float MotorUL, MotorUR, MotorDL, MotorDR; - } + public float AccX, AccY, AccZ; + public float GyrX, GyrY, GyrZ; + public float PosX, PosY; + public float LaserRange; + + public float MotorUL, MotorUR, MotorDL, MotorDR; public static byte[] getBytes(object data) { @@ -58,5 +54,165 @@ namespace DroneClient 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[]? RecvDataIMU(byte[] data) + { + DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU)); + + AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z; + GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z; + + return new byte[0]; + } + + private byte[]? RecvDataPos(byte[] data) + { + DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos)); + + PosX = pos.Local.X; PosY = pos.Local.Y; + LaserRange = pos.LiDAR; + + return new byte[0]; + } + + private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body) + { + byte[] zero = new byte[0]; + + switch (head.Type) + { + case DroneData.DataType.DataIMU: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataIMU(body); + } + } + + case DroneData.DataType.DataPos: + { + if (head.Mode == DroneData.DataMode.Request) + { + // Запрос данных + // ... // + // + return zero; + } + else + { + // Пришли данные + return RecvDataPos(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? DataStream(byte[]? data, int size) + { + List ret = new List(); + + 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 imu = new DroneData.DataHead(); + imu.Size = DroneData.DataHead.StrLen; + imu.Mode = DroneData.DataMode.Request; + imu.Type = DroneData.DataType.DataIMU; + + DroneData.DataHead pos = new DroneData.DataHead(); + pos.Size = DroneData.DataHead.StrLen; + pos.Mode = DroneData.DataMode.Request; + pos.Type = DroneData.DataType.DataPos; + + byte[] si = getBytes(imu); + byte[] sp = getBytes(pos); + byte[] sm = SendDataMotor4(); + + byte[] send = new byte[si.Length + sp.Length + sm.Length]; + si.CopyTo(send, 0); + sp.CopyTo(send, si.Length); + sm.CopyTo(send, si.Length + sp.Length); + + return send; + } } } diff --git a/DroneClient/DroneData.cs b/DroneClient/DroneData.cs new file mode 100644 index 0000000..8c14e2c --- /dev/null +++ b/DroneClient/DroneData.cs @@ -0,0 +1,67 @@ +using System.Runtime.InteropServices; + +namespace DroneData +{ + public enum DataMode : ushort + { + None = 0, Response = 1, Request = 2 + }; + + public enum DataType : ushort + { + None = 0, Head = 1, + + // Output + DataIMU = 1001, DataPos = 1002, + + // Input + DataMotor4 = 2001, DataMotor6 = 2002 + }; + + public struct DataHead + { + 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; // + public float LiDAR; // + + 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)); + } +} \ No newline at end of file diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index dee166e..bc249ac 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -21,34 +21,37 @@ namespace DroneSimulator if (!data.Connect) { - Invoke((MethodInvoker)delegate + try { - button_Connect.Text = "Connect"; - button_Connect.BackColor = Color.Transparent; - MessageBox.Show("Connection closed"); - }); + Invoke((MethodInvoker)delegate + { + button_Connect.Text = "Connect"; + button_Connect.BackColor = Color.Transparent; + MessageBox.Show("Connection closed"); + }); + } + catch { } return; } - - byte[] send = Drone.getBytes(sendDrone); - - data.Server.Send(send); } - Drone.DataIn sendDrone; - - Drone.DataOut recvDrone; + Drone dataDrone = new Drone(); private void ReceiveCallback(object o) { ReceiveData data = (ReceiveData)o; - recvDrone = (Drone.DataOut)Drone.fromBytes(data.Buffer, typeof(Drone.DataOut)); + List? send = dataDrone.DataStream(data.Buffer, data.Size); - byte[] send = Drone.getBytes(sendDrone); - - try { data.Server.Send(send); } + if (send == null) return; + try + { + foreach (byte[]? b in send) + { + if (b != null) data.Server?.Send(b); + } + } catch { } } @@ -88,17 +91,19 @@ namespace DroneSimulator private void timer_Test_Tick(object sender, EventArgs e) { - label_Acc_X.Text = recvDrone.AccX.ToString(); - label_Acc_Y.Text = recvDrone.AccY.ToString(); - label_Acc_Z.Text = recvDrone.AccZ.ToString(); + label_Acc_X.Text = dataDrone.AccX.ToString(); + label_Acc_Y.Text = dataDrone.AccY.ToString(); + label_Acc_Z.Text = dataDrone.AccZ.ToString(); - label_Gyr_X.Text = recvDrone.GyrX.ToString(); - label_Gyr_Y.Text = recvDrone.GyrY.ToString(); - label_Gyr_Z.Text = recvDrone.GyrZ.ToString(); + label_Gyr_X.Text = dataDrone.GyrX.ToString(); + label_Gyr_Y.Text = dataDrone.GyrY.ToString(); + label_Gyr_Z.Text = dataDrone.GyrZ.ToString(); - label_Pos_X.Text = recvDrone.PosX.ToString(); - label_Pos_Y.Text = recvDrone.PosY.ToString(); - label_Pos_L.Text = recvDrone.LaserRange.ToString(); + label_Pos_X.Text = dataDrone.PosX.ToString(); + label_Pos_Y.Text = dataDrone.PosY.ToString(); + label_Pos_L.Text = dataDrone.LaserRange.ToString(); + + netClient.SendData(dataDrone.SendReqest()); } private void trackBar_Power_Scroll(object sender, EventArgs e) @@ -107,7 +112,7 @@ namespace DroneSimulator label_Pow.Text = pow.ToString(); - sendDrone.MotorUL = sendDrone.MotorUR = sendDrone.MotorDL = sendDrone.MotorDR = pow; + dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow; } private void button_UU_MouseDown(object sender, MouseEventArgs e) @@ -116,35 +121,35 @@ namespace DroneSimulator if (sender == button_UU) { - sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow; - sendDrone.MotorDL += pow; sendDrone.MotorDR += pow; + dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR += pow; } if (sender == button_DD) { - sendDrone.MotorUL += pow; sendDrone.MotorUR += pow; - sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow; + dataDrone.MotorUL += pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow; } if (sender == button_LL) { - sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; - sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; + dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; } if (sender == button_RR) { - sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; - sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; + dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; } if (sender == button_ML) { - sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; - sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; + dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; + dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; } if (sender == button_MR) { - sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; - sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; + dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; + dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; } } diff --git a/DroneClient/NetClient.cs b/DroneClient/NetClient.cs index fc93baf..12aa8cb 100644 --- a/DroneClient/NetClient.cs +++ b/DroneClient/NetClient.cs @@ -97,5 +97,10 @@ namespace DroneSimulator try { ServerSocket?.BeginReceive(cd.Buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), cd); } catch { } } + + public void SendData(byte[] data) + { + try { ServerSocket?.Send(data); } catch { } + } } } diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 612505a..9a50742 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -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? DataStream(byte[]? data, int size) + { + List ret = new List(); + + 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; } } } diff --git a/DroneSimulator/DroneData.cs b/DroneSimulator/DroneData.cs index 448299b..8c14e2c 100644 --- a/DroneSimulator/DroneData.cs +++ b/DroneSimulator/DroneData.cs @@ -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; // public float LiDAR; // + + 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)); } } \ No newline at end of file diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 84c70ab..469e08b 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -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? 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) diff --git a/DroneSimulator/NetServerClients.cs b/DroneSimulator/NetServerClients.cs index 1e1d30d..8bde955 100644 --- a/DroneSimulator/NetServerClients.cs +++ b/DroneSimulator/NetServerClients.cs @@ -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 ClientSockets = new List(); - 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 { } }