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

Compare commits

base: CPL:main
Branches Tags
CPL:main
CPL:sergey
CPL:Dana
150billionsteps:main
150billionsteps:sergey
150billionsteps:Dana
..
compare: 150billionsteps:main
Branches Tags
150billionsteps:main
150billionsteps:sergey
150billionsteps:Dana
CPL:main
CPL:sergey
CPL:Dana

No commits in common. "main" and "main" have entirely different histories.
main ... main

14 changed files with 190 additions and 2294 deletions
Show Stats Expand all files Collapse all files

130
DroneClient/Drone.cs
View File

@ -1,5 +1,4 @@
using System.Diagnostics.Metrics;
using System.Drawing;
using System.Drawing;
using System.Runtime.InteropServices;
namespace DroneClient
@ -9,11 +8,8 @@ namespace DroneClient
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public uint TimeAcc, TimeGyr;
public float PosX, PosY;
public float LaserRange;
public uint TimeRange;
public float MotorUL, MotorUR, MotorDL, MotorDR;
@ -74,44 +70,22 @@ namespace DroneClient
return getBytes(mot4);
}
private byte[]? RecvDataAcc(byte[] data)
private byte[]? RecvDataIMU(byte[] data)
{
DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc= imu.Time;
return new byte[0];
}
private byte[]? RecvDataGyr(byte[] data)
{
DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
return new byte[0];
}
private byte[]? RecvDataRange(byte[] data)
private byte[]? RecvDataPos(byte[] data)
{
DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
return new byte[0];
}
private byte[]? RecvDataLocal(byte[] data)
{
DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));
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];
}
@ -122,7 +96,7 @@ namespace DroneClient
switch (head.Type)
{
case DroneData.DataType.DataAcc:
case DroneData.DataType.DataIMU:
{
if (head.Mode == DroneData.DataMode.Request)
{
@ -134,11 +108,11 @@ namespace DroneClient
else
{
// Пришли данные
return RecvDataAcc(body);
return RecvDataIMU(body);
}
}
case DroneData.DataType.DataGyr:
case DroneData.DataType.DataPos:
{
if (head.Mode == DroneData.DataMode.Request)
{
@ -150,39 +124,7 @@ namespace DroneClient
else
{
// Пришли данные
return RecvDataGyr(body);
}
}
case DroneData.DataType.DataRange:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataRange(body);
}
}
case DroneData.DataType.DataLocal:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataLocal(body);
return RecvDataPos(body);
}
}
@ -251,46 +193,24 @@ namespace DroneClient
public byte[] SendReqest()
{
DroneData.DataHead acc = new DroneData.DataHead();
acc.Size = DroneData.DataHead.StrLen;
acc.Mode = DroneData.DataMode.Request;
acc.Type = DroneData.DataType.DataAcc;
DroneData.DataHead imu = new DroneData.DataHead();
imu.Size = DroneData.DataHead.StrLen;
imu.Mode = DroneData.DataMode.Request;
imu.Type = DroneData.DataType.DataIMU;
DroneData.DataHead gyr = new DroneData.DataHead();
gyr.Size = DroneData.DataHead.StrLen;
gyr.Mode = DroneData.DataMode.Request;
gyr.Type = DroneData.DataType.DataGyr;
DroneData.DataHead pos = new DroneData.DataHead();
pos.Size = DroneData.DataHead.StrLen;
pos.Mode = DroneData.DataMode.Request;
pos.Type = DroneData.DataType.DataPos;
DroneData.DataHead range = new DroneData.DataHead();
range.Size = DroneData.DataHead.StrLen;
range.Mode = DroneData.DataMode.Request;
range.Type = DroneData.DataType.DataRange;
byte[] si = getBytes(imu);
byte[] sp = getBytes(pos);
byte[] sm = SendDataMotor4();
DroneData.DataHead local = new DroneData.DataHead();
local.Size = DroneData.DataHead.StrLen;
local.Mode = DroneData.DataMode.Request;
local.Type = DroneData.DataType.DataLocal;
List<byte[]> list = new List<byte[]>();
list.Add(getBytes(acc));
list.Add(getBytes(gyr));
list.Add(getBytes(range));
list.Add(getBytes(local));
list.Add(SendDataMotor4());
int count = 0;
foreach (byte[] d in list) count += d.Length;
byte[] send = new byte[count];
count = 0;
foreach (byte[] d in list)
{
d.CopyTo(send, count);
count += d.Length;
}
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;
}

49
DroneClient/DroneData.cs
View File

@ -12,7 +12,7 @@ namespace DroneData
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
DataIMU = 1001, DataPos = 1002,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
@ -25,66 +25,32 @@ namespace DroneData
public DataMode Mode;
public DataType Type;
public uint Time; // Îáùåå âðåìÿ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct DataAcc
public struct DataIMU
{
public DataHead Head;
public XYZ Acc;
public XYZ Acc, Gyr, Mag;
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
}
public struct DataGyr
{
public DataHead Head;
public XYZ Gyr;
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataLocal
public struct DataPos
{
public DataHead Head;
public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // Äàò÷èê ïîñàäêè
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
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));
@ -93,6 +59,7 @@ namespace DroneData
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));

43
DroneClient/FormMain.Designer.cs generated
View File

@ -65,9 +65,6 @@
label_Pow = new Label();
button_ML = new Button();
button_MR = new Button();
label_time_acc = new Label();
label_time_range = new Label();
label_time_gyr = new Label();
groupBox1.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
groupBox2.SuspendLayout();
@ -159,7 +156,6 @@
//
// groupBox2
//
groupBox2.Controls.Add(label_time_acc);
groupBox2.Controls.Add(label_Acc_Z);
groupBox2.Controls.Add(label7);
groupBox2.Controls.Add(label_Acc_Y);
@ -168,7 +164,7 @@
groupBox2.Controls.Add(label1);
groupBox2.Location = new Point(6, 86);
groupBox2.Name = "groupBox2";
groupBox2.Size = new Size(78, 118);
groupBox2.Size = new Size(78, 100);
groupBox2.TabIndex = 5;
groupBox2.TabStop = false;
groupBox2.Text = "Acc";
@ -220,7 +216,6 @@
//
// groupBox3
//
groupBox3.Controls.Add(label_time_gyr);
groupBox3.Controls.Add(label_Gyr_Z);
groupBox3.Controls.Add(label9);
groupBox3.Controls.Add(label_Gyr_Y);
@ -229,7 +224,7 @@
groupBox3.Controls.Add(label13);
groupBox3.Location = new Point(95, 86);
groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(78, 118);
groupBox3.Size = new Size(78, 100);
groupBox3.TabIndex = 6;
groupBox3.TabStop = false;
groupBox3.Text = "Gyr";
@ -290,7 +285,6 @@
//
// groupBox4
//
groupBox4.Controls.Add(label_time_range);
groupBox4.Controls.Add(label_Pos_L);
groupBox4.Controls.Add(label6);
groupBox4.Controls.Add(label_Pos_Y);
@ -299,7 +293,7 @@
groupBox4.Controls.Add(label14);
groupBox4.Location = new Point(188, 86);
groupBox4.Name = "groupBox4";
groupBox4.Size = new Size(78, 118);
groupBox4.Size = new Size(78, 100);
groupBox4.TabIndex = 7;
groupBox4.TabStop = false;
groupBox4.Text = "Pos";
@ -443,33 +437,6 @@
button_MR.MouseDown += button_UU_MouseDown;
button_MR.MouseUp += button_UU_MouseUp;
//
// label_time_acc
//
label_time_acc.AutoSize = true;
label_time_acc.Location = new Point(6, 100);
label_time_acc.Name = "label_time_acc";
label_time_acc.Size = new Size(13, 15);
label_time_acc.TabIndex = 25;
label_time_acc.Text = "0";
//
// label_time_range
//
label_time_range.AutoSize = true;
label_time_range.Location = new Point(6, 100);
label_time_range.Name = "label_time_range";
label_time_range.Size = new Size(13, 15);
label_time_range.TabIndex = 27;
label_time_range.Text = "0";
//
// label_time_gyr
//
label_time_gyr.AutoSize = true;
label_time_gyr.Location = new Point(3, 100);
label_time_gyr.Name = "label_time_gyr";
label_time_gyr.Size = new Size(13, 15);
label_time_gyr.TabIndex = 26;
label_time_gyr.Text = "0";
//
// Form_Main
//
AutoScaleDimensions = new SizeF(7F, 15F);
@ -542,9 +509,5 @@
private Label label_Pow;
private Button button_ML;
private Button button_MR;
private Label label4;
private Label label_time_acc;
private Label label_time_range;
private Label label_time_gyr;
}
}

6
DroneClient/FormMain.cs
View File

@ -95,20 +95,14 @@ namespace DroneSimulator
label_Acc_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_time_acc.Text = dataDrone.TimeAcc.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
label_Pos_X.Text = dataDrone.PosX.ToString();
label_Pos_Y.Text = dataDrone.PosY.ToString();
label_Pos_L.Text = dataDrone.LaserRange.ToString();
label_time_range.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest());
}

118
DroneClientCpp/Drone.cpp
View File

@ -59,91 +59,55 @@ namespace DroneClient {
return GetBytes(mot4);
}
array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
// Реализация приватного метода RecvDataIMU
array<Byte>^ Drone::RecvDataIMU(array<Byte>^ data)
{
DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);
DroneData::DataIMU imu = (DroneData::DataIMU)FromBytes(data, DroneData::DataIMU::typeid);
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc = imu.Time;
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
// Реализация приватного метода RecvDataPos
array<Byte>^ Drone::RecvDataPos(array<Byte>^ data)
{
DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
{
DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
{
DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);
DroneData::DataPos pos = (DroneData::DataPos)FromBytes(data, DroneData::DataPos::typeid);
PosX = pos.Local.X; PosY = pos.Local.Y;
LaserRange = pos.LiDAR;
return gcnew array<Byte>(0);
}
// Реализация приватного метода ClientRequestResponse
array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
{
array<Byte>^ zero = gcnew array<Byte>(0);
switch (head.Type)
{
case DroneData::DataType::DataAcc:
case DroneData::DataType::DataIMU:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataAcc(body); // Пришли данные
return RecvDataIMU(body); // Пришли данные
}
case DroneData::DataType::DataGyr:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataGyr(body); // Пришли данные
}
case DroneData::DataType::DataRange:
case DroneData::DataType::DataPos:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataRange(body); // Пришли данные
return RecvDataPos(body); // Пришли данные
}
case DroneData::DataType::DataLocal:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataLocal(body); // Пришли данные
}
case DroneData::DataType::DataMotor4:
if (head.Mode == DroneData::DataMode::Request)
{
@ -169,7 +133,7 @@ namespace DroneClient {
}
// Реализация метода DataStream
List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
System::Collections::Generic::List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
{
System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();
@ -209,47 +173,25 @@ namespace DroneClient {
// Реализация метода SendRequest
array<Byte>^ Drone::SendRequest()
{
DroneData::DataHead^ acc = gcnew DroneData::DataHead();
acc->Size = DroneData::DataHead::StrLen;
acc->Mode = DroneData::DataMode::Request;
acc->Type = DroneData::DataType::DataAcc;
DroneData::DataHead imu;
imu.Size = DroneData::DataHead::StrLen;
imu.Mode = DroneData::DataMode::Request;
imu.Type = DroneData::DataType::DataIMU;
DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
gyr->Size = DroneData::DataHead::StrLen;
gyr->Mode = DroneData::DataMode::Request;
gyr->Type = DroneData::DataType::DataGyr;
DroneData::DataHead pos;
pos.Size = DroneData::DataHead::StrLen;
pos.Mode = DroneData::DataMode::Request;
pos.Type = DroneData::DataType::DataPos;
DroneData::DataHead^ range = gcnew DroneData::DataHead();
range->Size = DroneData::DataHead::StrLen;
range->Mode = DroneData::DataMode::Request;
range->Type = DroneData::DataType::DataRange;
array<Byte>^ si = GetBytes(imu);
array<Byte>^ sp = GetBytes(pos);
array<Byte>^ sm = SendDataMotor4();
DroneData::DataHead^ local = gcnew DroneData::DataHead();
local->Size = DroneData::DataHead::StrLen;
local->Mode = DroneData::DataMode::Request;
local->Type = DroneData::DataType::DataLocal;
array<Byte>^ send = gcnew array<Byte>(si->Length + sp->Length + sm->Length);
Array::Copy(si, 0, send, 0, si->Length);
Array::Copy(sp, 0, send, si->Length, sp->Length);
Array::Copy(sm, 0, send, si->Length + sp->Length, sm->Length);
List<array<byte>^>^ list = gcnew List<array<byte>^>();
list->Add(GetBytes(acc));
list->Add(GetBytes(gyr));
list->Add(GetBytes(range));
list->Add(GetBytes(local));
list->Add(SendDataMotor4());
int count = 0;
for each(array<byte>^ d in list) count += d->Length;
array<byte>^ send = gcnew array<byte>(count);
count = 0;
for each (array<byte> ^ d in list)
{
d->CopyTo(send, count);
count += d->Length;
}
return send;
return send;
}
}

9
DroneClientCpp/Drone.h
View File

@ -18,11 +18,8 @@ namespace DroneClient {
public:
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
unsigned int TimeAcc, TimeGyr;
float PosX, PosY;
float LaserRange;
unsigned int TimeRange;
float MotorUL, MotorUR, MotorDL, MotorDR;
@ -31,10 +28,8 @@ namespace DroneClient {
private:
array<Byte>^ SendDataMotor4();
array<Byte>^ RecvDataAcc(array<Byte>^ data);
array<Byte>^ RecvDataGyr(array<Byte>^ data);
array<Byte>^ RecvDataRange(array<Byte>^ data);
array<Byte>^ RecvDataLocal(array<Byte>^ data);
array<Byte>^ RecvDataIMU(array<Byte>^ data);
array<Byte>^ RecvDataPos(array<Byte>^ data);
array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
literal int DroneStreamCount = 512;

64
DroneClientCpp/DroneData.h
View File

@ -15,24 +15,18 @@ namespace DroneData {
public enum class DataType : unsigned short
{
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
None = 0, Head = 1,
DataIMU = 1001, DataPos = 1002,
DataMotor4 = 2001, DataMotor6 = 2002
};
public value struct DataHead
{
public:
long Size;
int Size;
DataMode Mode;
DataType Type;
unsigned long Time;
static const int StrLen = sizeof(DataHead);
};
@ -42,65 +36,30 @@ namespace DroneData {
float X, Y, Z;
};
public value struct DataAcc
public value struct DataIMU
{
public:
DataHead Head;
XYZ Acc;
XYZ Acc, Gyr, Mag;
unsigned long Time;
static const int StrLen = sizeof(DataAcc);
static const int StrLen = sizeof(DataIMU);
};
public value struct DataGyr
{
public:
DataHead Head;
XYZ Gyr;
unsigned long Time;
static const int StrLen = sizeof(DataGyr);
};
public value struct DataMag
{
public:
DataHead Head;
XYZ Mag;
unsigned long Time;
static const int StrLen = sizeof(DataMag);
};
public value struct DataRange
public value struct DataPos
{
public:
DataHead Head;
XYZ Local;
float LiDAR;
unsigned long Time;
static const int StrLen = sizeof(DataRange);
};
public value struct DataLocal
{
public:
DataHead Head;
XYZ Local;
unsigned long Time;
static const int StrLen = sizeof(DataLocal);
static const int StrLen = sizeof(DataPos);
};
public value struct DataMotor4
{
public:
DataHead Head;
unsigned long long Count;
float UL, UR, DL, DR;
static const int StrLen = sizeof(DataMotor4);
@ -110,6 +69,7 @@ namespace DroneData {
{
public:
DataHead Head;
unsigned long long Count;
float UL, UR, LL, RR, DL, DR;
static const int StrLen = sizeof(DataMotor6);

BIN
DroneClientCpp/FormMain.h
View File

Binary file not shown.

14
DroneClientCpp/NetClient.cpp
View File

@ -40,7 +40,7 @@ namespace DroneSimulator {
IPEndPoint^ ep = gcnew IPEndPoint(IPAddress::Parse(Addr), Port);
ServerSocket = gcnew Socket(AddressFamily::InterNetwork, SocketType::Stream, ProtocolType::Tcp);
try { if (ServerSocket) ServerSocket->Connect(ep); }
try { ServerSocket->Connect(ep); }
catch (...) { ServerSocket->Close(); return ClientState::Error; }
Connected = true;
@ -49,7 +49,7 @@ namespace DroneSimulator {
ReceiveData^ receiveData = gcnew ReceiveData(DataServer->buffer, ServerData::size, ServerSocket);
try { if (ServerSocket) ServerSocket->BeginReceive(DataServer->buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), receiveData); }
try { ServerSocket->BeginReceive(DataServer->buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), receiveData); }
catch (...) {}
return ClientState::Connected;
@ -58,9 +58,9 @@ namespace DroneSimulator {
// Реализация метода Close
void NetClient::Close()
{
try { if(ServerSocket) ServerSocket->Shutdown(SocketShutdown::Both); }
try { ServerSocket->Shutdown(SocketShutdown::Both); }
catch (...) {}
if(ServerSocket) ServerSocket->Close();
ServerSocket->Close();
Connected = false;
}
@ -69,7 +69,7 @@ namespace DroneSimulator {
{
if (ServerSocket != nullptr && Connected)
{
try { if (ServerSocket) ServerSocket->Send(data); }
try { ServerSocket->Send(data); }
catch (...) {}
}
}
@ -86,7 +86,7 @@ namespace DroneSimulator {
if (bytes == 0)
{
if (ServerSocket) ServerSocket->Close();
ServerSocket->Close();
Connected = false;
if (ServerSocket != nullptr)
@ -99,7 +99,7 @@ namespace DroneSimulator {
ReceiveCallback(gcnew ReceiveData(cd->Buffer, bytes, ServerSocket));
try { if (ServerSocket) ServerSocket->BeginReceive(cd->Buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), cd); }
try { ServerSocket->BeginReceive(cd->Buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), cd); }
catch (...) {}
}
}

188
DroneSimulator/Drone.cs
View File

@ -2,9 +2,6 @@
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
using static DroneSimulator.Drone;
using static System.Windows.Forms.VisualStyles.VisualStyleElement;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
namespace DroneSimulator
{
@ -24,8 +21,6 @@ namespace DroneSimulator
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
public float LaserRange; // Имитация: Дальномер
private uint DataTimer;
private const float Gravity = 9.8f;
private const float TO_GRAD = 180 / MathF.PI;
@ -34,13 +29,7 @@ namespace DroneSimulator
private Thread DroneThread;
private int Timer;
private Random MainRandom = new Random();
RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
RealMode.Position RealPos = new RealMode.Position();
RealMode.Barometer RealBar = new RealMode.Barometer();
RealMode.Range RealRange = new RealMode.Range();
private static int CounterID = 0;
public static byte[] getBytes(object data)
{
@ -83,6 +72,33 @@ namespace DroneSimulator
return mem;
}
/*public struct DataOut
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public float PosX, PosY;
public float LaserRange;
}
public DataOut GetDataOut()
{
DataOut data = new DataOut();
data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
data.LaserRange = LaserRange;
return data;
}
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
}*/
public struct DataVisual
{
public int ID; // Идентификатор
@ -248,14 +264,6 @@ namespace DroneSimulator
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
else LaserRange = float.MaxValue;
}
RealAcc.Update(Acc, (uint)tick);
RealGyr.Update(Gyr, (uint)tick);
RealRange.Update(LaserRange, (uint)tick);
RealBar.Update(PosXYZ.Z * 11, (uint)tick);
RealPos.Update(PosXYZ, (uint)tick);
DataTimer = (uint)tick;
}
private float Range(float pow)
@ -284,94 +292,33 @@ namespace DroneSimulator
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
}
private byte[] SendDataAcc()
private byte[] SendDataIMU()
{
DroneData.DataAcc acc = new DroneData.DataAcc();
DroneData.DataIMU imu = new DroneData.DataIMU();
acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
acc.Head.Mode = DroneData.DataMode.Response;
acc.Head.Type = DroneData.DataType.DataAcc;
acc.Head.Time = (uint)Environment.TickCount;
imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
imu.Head.Mode = DroneData.DataMode.Response;
imu.Head.Type = DroneData.DataType.DataIMU;
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
acc.Time = RealAcc.timer;
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(acc);
return getBytes(imu);
}
private byte[] SendDataGyr()
private byte[] SendDataPos()
{
DroneData.DataGyr gyr = new DroneData.DataGyr();
DroneData.DataPos pos = new DroneData.DataPos();
gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
gyr.Head.Mode = DroneData.DataMode.Response;
gyr.Head.Type = DroneData.DataType.DataGyr;
gyr.Head.Time = (uint)Environment.TickCount;
pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
pos.Head.Mode = DroneData.DataMode.Response;
pos.Head.Type = DroneData.DataType.DataPos;
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
gyr.Time = RealGyr.timer;
pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
pos.LiDAR = LaserRange;
return getBytes(gyr);
}
private byte[] SendDataMag()
{
DroneData.DataMag mag = new DroneData.DataMag();
mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
mag.Head.Mode = DroneData.DataMode.Response;
mag.Head.Type = DroneData.DataType.DataMag;
mag.Head.Time = (uint)Environment.TickCount;
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
mag.Time = DataTimer;
return getBytes(mag);
}
private byte[] SendDataRange()
{
DroneData.DataRange range = new DroneData.DataRange();
range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
range.Head.Mode = DroneData.DataMode.Response;
range.Head.Type = DroneData.DataType.DataRange;
range.Head.Time = (uint)Environment.TickCount;
range.LiDAR = RealRange.result;
range.Time = RealRange.timer;
return getBytes(range);
}
private byte[] SendDataLocal()
{
DroneData.DataLocal local = new DroneData.DataLocal();
local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
local.Head.Mode = DroneData.DataMode.Response;
local.Head.Type = DroneData.DataType.DataLocal;
local.Head.Time = (uint)Environment.TickCount;
local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
local.Time = RealPos.timer;
return getBytes(local);
}
private byte[] SendDataBarometer()
{
DroneData.DataBar bar = new DroneData.DataBar();
bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
bar.Head.Mode = DroneData.DataMode.Response;
bar.Head.Type = DroneData.DataType.DataBar;
bar.Head.Time = (uint)Environment.TickCount;
bar.Pressure = RealBar.result;
bar.Time = RealBar.timer;
return getBytes(bar);
return getBytes(pos);
}
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
@ -380,12 +327,12 @@ namespace DroneSimulator
switch (head.Type)
{
case DroneData.DataType.DataAcc:
case DroneData.DataType.DataIMU:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataAcc();
return SendDataIMU();
}
else
{
@ -396,17 +343,38 @@ namespace DroneSimulator
}
}
case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;
case DroneData.DataType.DataPos:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataPos();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataMag: if (head.Mode == DroneData.DataMode.Request) return SendDataMag(); else return zero;
case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero;
case DroneData.DataType.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else return zero;
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
RecvDataMotor4(body);
return zero;
}
}
}
return zero;

65
DroneSimulator/DroneData.cs
View File

@ -1,4 +1,4 @@
using System.Runtime.InteropServices;
using System.Runtime.InteropServices;
namespace DroneData
{
@ -12,7 +12,7 @@ namespace DroneData
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
DataIMU = 1001, DataPos = 1002,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
@ -25,76 +25,32 @@ namespace DroneData
public DataMode Mode;
public DataType Type;
public uint Time; // Общее время
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct DataAcc
public struct DataIMU
{
public DataHead Head;
public XYZ Acc;
public XYZ Acc, Gyr, Mag;
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
}
public struct DataGyr
public struct DataPos
{
public DataHead Head;
public XYZ Gyr;
public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû
public float LiDAR; // Äàò÷èê ïîñàäêè
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // Датчик посадки
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // Локальные координаты
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataBar
{
public DataHead Head;
public float Pressure; // Давление
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
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));
@ -103,6 +59,7 @@ namespace DroneData
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));

1390
DroneSimulator/FormMain.Designer.cs generated
View File

File diff suppressed because it is too large Load Diff

69
DroneSimulator/FormMain.cs
View File

@ -1,4 +1,4 @@
using System.Text;
using System.Text;
using System.Numerics;
using System.Windows.Forms;
using static System.Net.Mime.MediaTypeNames;
@ -22,13 +22,6 @@ namespace DroneSimulator
public Form_Main()
{
InitializeComponent();
RealMode.RealSimulation = checkBox_Mode_Real.Checked;
numericUpDown_Acc_Update(null, null);
numericUpDown_Gyr_Update(null, null);
numericUpDown_Pos_Update(null, null);
numericUpDown_Bar_Update(null, null);
numericUpDown_Range_Update(null, null);
}
private void ClientConnectionCallback(object o)
@ -211,65 +204,5 @@ namespace DroneSimulator
}
}
}
private void numericUpDown_Bar_Update(object sender, EventArgs e)
{
try { RealMode.Barometer.Pressure = uint.Parse(textBox_Bar_Pressure.Text); }
catch
{
RealMode.Barometer.Pressure = 102258;
MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value;
RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value;
RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value;
RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked;
}
private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e)
{
RealMode.RealSimulation = checkBox_Mode_Real.Checked;
}
private void numericUpDown_Acc_Update(object sender, EventArgs e)
{
RealMode.Accelerometer.Freq = (uint)numericUpDown_Acc_Freq.Value;
RealMode.Accelerometer.Noise = (float)numericUpDown_Acc_Noise.Value;
RealMode.Accelerometer.Lateness = (float)numericUpDown_Acc_Laten.Value;
RealMode.Accelerometer.ScaleLeft = (float)numericUpDown_Acc_Scale_Left.Value;
RealMode.Accelerometer.ScaleRight = (float)numericUpDown_Acc_Scale_Rigth.Value;
}
private void numericUpDown_Gyr_Update(object sender, EventArgs e)
{
RealMode.Gyroscope.Freq = (uint)numericUpDown_Gyr_Freq.Value;
RealMode.Gyroscope.Noise = (float)numericUpDown_Gyr_Noise.Value;
RealMode.Gyroscope.Lateness = (float)numericUpDown_Gyr_Laten.Value;
RealMode.Gyroscope.Shift.X = (float)numericUpDown_Gyr_Shift_X.Value;
RealMode.Gyroscope.Shift.Y = (float)numericUpDown_Gyr_Shift_Y.Value;
RealMode.Gyroscope.Shift.Z = (float)numericUpDown_Gyr_Shift_Z.Value;
}
private void numericUpDown_Pos_Update(object sender, EventArgs e)
{
RealMode.Position.Freq = (uint)numericUpDown_Pos_Freq.Value;
RealMode.Position.Noise = (float)numericUpDown_Pos_Noise.Value;
RealMode.Position.Lateness = (float)numericUpDown_Pos_Laten.Value;
RealMode.Position.Enable = checkBox_Pos_Enable.Checked;
}
private void numericUpDown_Range_Update(object sender, EventArgs e)
{
RealMode.Range.Freq = (uint)numericUpDown_Range_Freq.Value;
RealMode.Range.Noise = (float)numericUpDown_Range_Noise.Value;
RealMode.Range.Lateness = (float)numericUpDown_Range_Laten.Value;
RealMode.Range.Enable = checkBox_Range_Enable.Checked;
RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
}
}
}

333
DroneSimulator/RealMode.cs
View File

@ -1,333 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Numerics;
using System.Reflection;
namespace DroneSimulator
{
internal class RealMode
{
public static bool RealSimulation;
internal class Accelerometer
{
public static uint Freq;
public static float Noise;
public static float ScaleLeft;
public static float ScaleRight;
public static float Lateness;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private uint index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
float scale = (ScaleRight - ScaleLeft) / 2;
float shift = scale + ScaleLeft;
value.X = (value.X * scale) + shift;
value.Y = (value.Y * scale) + shift;
value.Z = (value.Z * scale) + shift;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
internal class Gyroscope
{
public static uint Freq;
public static float Noise;
public static Vector3 Shift;
public static float Lateness;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private uint index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
value.X += Shift.X;
value.Y += Shift.Y;
value.Z += Shift.Z;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
internal class Magnetometer
{
}
internal class Position
{
public static bool Enable;
public static uint Freq;
public static float Noise;
public static float Lateness;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private uint index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
if (!Enable)
{
result = Vector3.NaN;
return;
}
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
internal class Barometer
{
public static bool Enable;
public static float Pressure;
public static uint Freq;
public static float Noise;
public static float Lateness;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private uint index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
value = Pressure - value;
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
if (!Enable)
{
result = float.NaN;
return;
}
int noise = (int)(Noise * 1000);
value += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
internal class OpticalFlow
{
}
internal class Range
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private uint index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
if (!Enable)
{
result = float.NaN;
return;
}
if (value > MaxHeight) value = -1;
else
{
int noise = (int)(Noise * 1000);
value += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
}
}