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

Compare commits

base: CPL:1558429ad081f153d2a771cd6a239d1847e9d06a
Branches Tags
CPL:main
CPL:sergey
CPL:Dana
..
compare: CPL:e0af20b46d3b92488e8669eefcf26a5f90688cbe
Branches Tags
CPL:main
CPL:sergey
CPL:Dana

No commits in common. "1558429ad081f153d2a771cd6a239d1847e9d06a" and "e0af20b46d3b92488e8669eefcf26a5f90688cbe" have entirely different histories.
1558429ad0 ... e0af20b46d

9 changed files with 109 additions and 555 deletions
Show Stats Expand all files Collapse all files

170
DroneClient/Drone.cs
View File

@ -1,17 +1,21 @@
using System.Drawing;
using System.Runtime.InteropServices;
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 static byte[] getBytes(object data)
{
@ -54,165 +58,5 @@ 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<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 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;
}
}
}

67
DroneClient/DroneData.cs
View File

@ -1,67 +0,0 @@
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));
}
}

71
DroneClient/FormMain.cs
View File

@ -20,8 +20,6 @@ namespace DroneSimulator
ConnectData data = (ConnectData)o;
if (!data.Connect)
{
try
{
Invoke((MethodInvoker)delegate
{
@ -29,29 +27,28 @@ namespace DroneSimulator
button_Connect.BackColor = Color.Transparent;
MessageBox.Show("Connection closed");
});
}
catch { }
return;
}
byte[] send = Drone.getBytes(sendDrone);
data.Server.Send(send);
}
Drone dataDrone = new Drone();
Drone.DataIn sendDrone;
Drone.DataOut recvDrone;
private void ReceiveCallback(object o)
{
ReceiveData data = (ReceiveData)o;
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
recvDrone = (Drone.DataOut)Drone.fromBytes(data.Buffer, typeof(Drone.DataOut));
if (send == null) return;
try
{
foreach (byte[]? b in send)
{
if (b != null) data.Server?.Send(b);
}
}
byte[] send = Drone.getBytes(sendDrone);
try { data.Server.Send(send); }
catch { }
}
@ -91,19 +88,17 @@ namespace DroneSimulator
private void timer_Test_Tick(object sender, EventArgs e)
{
label_Acc_X.Text = dataDrone.AccX.ToString();
label_Acc_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_Acc_X.Text = recvDrone.AccX.ToString();
label_Acc_Y.Text = recvDrone.AccY.ToString();
label_Acc_Z.Text = recvDrone.AccZ.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_Gyr_X.Text = recvDrone.GyrX.ToString();
label_Gyr_Y.Text = recvDrone.GyrY.ToString();
label_Gyr_Z.Text = recvDrone.GyrZ.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());
label_Pos_X.Text = recvDrone.PosX.ToString();
label_Pos_Y.Text = recvDrone.PosY.ToString();
label_Pos_L.Text = recvDrone.LaserRange.ToString();
}
private void trackBar_Power_Scroll(object sender, EventArgs e)
@ -112,7 +107,7 @@ namespace DroneSimulator
label_Pow.Text = pow.ToString();
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
sendDrone.MotorUL = sendDrone.MotorUR = sendDrone.MotorDL = sendDrone.MotorDR = pow;
}
private void button_UU_MouseDown(object sender, MouseEventArgs e)
@ -121,35 +116,35 @@ namespace DroneSimulator
if (sender == button_UU)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR += pow;
}
if (sender == button_DD)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
sendDrone.MotorUL += pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow;
}
if (sender == button_LL)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
}
if (sender == button_RR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
}
if (sender == button_ML)
{
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
}
if (sender == button_MR)
{
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
}
}

9
DroneClient/NetClient.cs
View File

@ -41,7 +41,9 @@ namespace DroneSimulator
{
if (Connected)
{
Close();
try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
ServerSocket?.Close();
Connected = false;
return ClientState.Stop;
}
@ -97,10 +99,5 @@ 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 { }
}
}
}

159
DroneSimulator/Drone.cs
View File

@ -1,7 +1,5 @@
using System.CodeDom;
using System.Numerics;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
namespace DroneSimulator
{
@ -14,8 +12,8 @@ namespace DroneSimulator
public const float Dynamic = 10; // Динамика вращения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион
public float Power = 0; // Тяга всех двигателей (0-1)
public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
public float Power = 0; // Тяга всех двигателей
public float MaxPower; // Тяга всех двигателей
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
@ -72,7 +70,7 @@ namespace DroneSimulator
return mem;
}
/*public struct DataOut
public struct DataOut
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
@ -97,7 +95,7 @@ namespace DroneSimulator
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
}*/
}
public struct DataVisual
{
@ -115,7 +113,9 @@ namespace DroneSimulator
{
while (DroneThread != null)
{
Action(Environment.TickCount);
float time = Environment.TickCount - Timer;
Timer = Environment.TickCount;
Action(time / 1000);
Thread.Sleep(1);
}
}
@ -191,11 +191,8 @@ 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(int tick)
public void Action(float time)
{
float time = (tick - Timer) / 1000.0f;
Timer = tick;
if (!Active) return;
float flow = Power;
@ -284,143 +281,5 @@ namespace DroneSimulator
AccPRY.X = ((ul + ur) - (dl + dr));
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
}
private void RecvDataMotor4(byte[] data)
{
DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
/* обработка */
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
}
private byte[] SendDataIMU()
{
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.DataType.DataIMU:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataIMU();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataPos:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataPos();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
RecvDataMotor4(body);
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(ServerRequestResponse(DroneStreamHead, body));
}
return ret;
}
}
}

67
DroneSimulator/DroneData.cs
View File

@ -1,67 +0,0 @@
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));
}
}

18
DroneSimulator/FormMain.cs
View File

@ -4,9 +4,6 @@ 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;
using System.Linq;
namespace DroneSimulator
{
@ -72,16 +69,13 @@ namespace DroneSimulator
if (drone == null) return;
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
Drone.DataIn id = (Drone.DataIn)Drone.fromBytes(data.Buffer, typeof(Drone.DataIn));
if (send == null) return;
try
{
foreach (byte[]? b in send)
{
if (b != null) data.Client?.Send(b);
}
}
drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
Drone.DataOut od = drone.GetDataOut();
try { data.Client.Send(Drone.getBytes(od)); }
catch { }
}

12
DroneSimulator/NetServerClients.cs
View File

@ -1,6 +1,5 @@
using System.Net.Sockets;
using System.Net;
using System.Drawing;
namespace DroneSimulator
{
@ -17,9 +16,8 @@ namespace DroneSimulator
public class ReceiveData
{
public int ID;
public byte[] Buffer;
public byte[]? Buffer;
public int Size;
public Socket? Client;
@ -29,8 +27,8 @@ namespace DroneSimulator
{
public int ID;
public Socket? workSocket = null;
public const int count = 1024;
public byte[] buffer = new byte[count];
public const int BufferSize = 1024;
public byte[] buffer = new byte[BufferSize];
}
private int SocketID = 0;
@ -103,7 +101,7 @@ namespace DroneSimulator
ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
handler.BeginReceive(clientData.buffer, 0, ClientData.count, 0, new AsyncCallback(ReadCallback), clientData);
handler.BeginReceive(clientData.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), clientData);
}
else handler.Close();
@ -134,7 +132,7 @@ namespace DroneSimulator
try
{
cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.count, 0, new AsyncCallback(ReadCallback), cd);
cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), cd);
}
catch { }
}

1
www.txt Normal file
View File

@ -0,0 +1 @@
Hi!!!