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

51
DroneClient/DroneData.cs
View File

@ -8,11 +8,11 @@ namespace DroneData
}; };
public enum DataType : ushort public enum DataType : ushort
{ {
None = 0, Head = 1, None = 0, Head = 1,
// Output // Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataIMU = 1001, DataPos = 1002,
// Input // Input
DataMotor4 = 2001, DataMotor6 = 2002 DataMotor4 = 2001, DataMotor6 = 2002
@ -25,66 +25,32 @@ namespace DroneData
public DataMode Mode; public DataMode Mode;
public DataType Type; public DataType Type;
public uint Time; // Îáùåå âðåìÿ
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
} }
public struct XYZ { public float X, Y, Z; } public struct XYZ { public float X, Y, Z; }
public struct DataAcc public struct DataIMU
{ {
public DataHead Head; public DataHead Head;
public XYZ Acc; public XYZ Acc, Gyr, Mag;
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataIMU));
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
} }
public struct DataGyr public struct DataPos
{
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 DataHead Head; public DataHead Head;
public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû 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 float LiDAR; // Äàò÷èê ïîñàäêè
public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataPos));
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
} }
public struct DataMotor4 public struct DataMotor4
{ {
public DataHead Head; public DataHead Head;
public ulong Count;
public float UL, UR, DL, DR; public float UL, UR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
@ -93,6 +59,7 @@ namespace DroneData
public struct DataMotor6 public struct DataMotor6
{ {
public DataHead Head; public DataHead Head;
public ulong Count;
public float UL, UR, LL, RR, DL, DR; public float UL, UR, LL, RR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6)); 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(); label_Pow = new Label();
button_ML = new Button(); button_ML = new Button();
button_MR = new Button(); button_MR = new Button();
label_time_acc = new Label();
label_time_range = new Label();
label_time_gyr = new Label();
groupBox1.SuspendLayout(); groupBox1.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
groupBox2.SuspendLayout(); groupBox2.SuspendLayout();
@ -159,7 +156,6 @@
// //
// groupBox2 // groupBox2
// //
groupBox2.Controls.Add(label_time_acc);
groupBox2.Controls.Add(label_Acc_Z); groupBox2.Controls.Add(label_Acc_Z);
groupBox2.Controls.Add(label7); groupBox2.Controls.Add(label7);
groupBox2.Controls.Add(label_Acc_Y); groupBox2.Controls.Add(label_Acc_Y);
@ -168,7 +164,7 @@
groupBox2.Controls.Add(label1); groupBox2.Controls.Add(label1);
groupBox2.Location = new Point(6, 86); groupBox2.Location = new Point(6, 86);
groupBox2.Name = "groupBox2"; groupBox2.Name = "groupBox2";
groupBox2.Size = new Size(78, 118); groupBox2.Size = new Size(78, 100);
groupBox2.TabIndex = 5; groupBox2.TabIndex = 5;
groupBox2.TabStop = false; groupBox2.TabStop = false;
groupBox2.Text = "Acc"; groupBox2.Text = "Acc";
@ -220,7 +216,6 @@
// //
// groupBox3 // groupBox3
// //
groupBox3.Controls.Add(label_time_gyr);
groupBox3.Controls.Add(label_Gyr_Z); groupBox3.Controls.Add(label_Gyr_Z);
groupBox3.Controls.Add(label9); groupBox3.Controls.Add(label9);
groupBox3.Controls.Add(label_Gyr_Y); groupBox3.Controls.Add(label_Gyr_Y);
@ -229,7 +224,7 @@
groupBox3.Controls.Add(label13); groupBox3.Controls.Add(label13);
groupBox3.Location = new Point(95, 86); groupBox3.Location = new Point(95, 86);
groupBox3.Name = "groupBox3"; groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(78, 118); groupBox3.Size = new Size(78, 100);
groupBox3.TabIndex = 6; groupBox3.TabIndex = 6;
groupBox3.TabStop = false; groupBox3.TabStop = false;
groupBox3.Text = "Gyr"; groupBox3.Text = "Gyr";
@ -290,7 +285,6 @@
// //
// groupBox4 // groupBox4
// //
groupBox4.Controls.Add(label_time_range);
groupBox4.Controls.Add(label_Pos_L); groupBox4.Controls.Add(label_Pos_L);
groupBox4.Controls.Add(label6); groupBox4.Controls.Add(label6);
groupBox4.Controls.Add(label_Pos_Y); groupBox4.Controls.Add(label_Pos_Y);
@ -299,7 +293,7 @@
groupBox4.Controls.Add(label14); groupBox4.Controls.Add(label14);
groupBox4.Location = new Point(188, 86); groupBox4.Location = new Point(188, 86);
groupBox4.Name = "groupBox4"; groupBox4.Name = "groupBox4";
groupBox4.Size = new Size(78, 118); groupBox4.Size = new Size(78, 100);
groupBox4.TabIndex = 7; groupBox4.TabIndex = 7;
groupBox4.TabStop = false; groupBox4.TabStop = false;
groupBox4.Text = "Pos"; groupBox4.Text = "Pos";
@ -443,33 +437,6 @@
button_MR.MouseDown += button_UU_MouseDown; button_MR.MouseDown += button_UU_MouseDown;
button_MR.MouseUp += button_UU_MouseUp; 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 // Form_Main
// //
AutoScaleDimensions = new SizeF(7F, 15F); AutoScaleDimensions = new SizeF(7F, 15F);
@ -542,9 +509,5 @@
private Label label_Pow; private Label label_Pow;
private Button button_ML; private Button button_ML;
private Button button_MR; 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_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString(); label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_time_acc.Text = dataDrone.TimeAcc.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString(); label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString(); label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString(); label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
label_Pos_X.Text = dataDrone.PosX.ToString(); label_Pos_X.Text = dataDrone.PosX.ToString();
label_Pos_Y.Text = dataDrone.PosY.ToString(); label_Pos_Y.Text = dataDrone.PosY.ToString();
label_Pos_L.Text = dataDrone.LaserRange.ToString(); label_Pos_L.Text = dataDrone.LaserRange.ToString();
label_time_range.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest()); netClient.SendData(dataDrone.SendReqest());
} }

118
DroneClientCpp/Drone.cpp
View File

@ -59,91 +59,55 @@ namespace DroneClient {
return GetBytes(mot4); 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; 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); 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); DroneData::DataPos pos = (DroneData::DataPos)FromBytes(data, DroneData::DataPos::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);
PosX = pos.Local.X; PosY = pos.Local.Y; PosX = pos.Local.X; PosY = pos.Local.Y;
LaserRange = pos.LiDAR;
return gcnew array<Byte>(0); return gcnew array<Byte>(0);
} }
// Реализация приватного метода ClientRequestResponse
array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body) array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
{ {
array<Byte>^ zero = gcnew array<Byte>(0); array<Byte>^ zero = gcnew array<Byte>(0);
switch (head.Type) switch (head.Type)
{ {
case DroneData::DataType::DataAcc: case DroneData::DataType::DataIMU:
if (head.Mode == DroneData::DataMode::Request) if (head.Mode == DroneData::DataMode::Request)
{ {
return zero; // Запрос данных (не реализовано) return zero; // Запрос данных (не реализовано)
} }
else else
{ {
return RecvDataAcc(body); // Пришли данные return RecvDataIMU(body); // Пришли данные
} }
case DroneData::DataType::DataGyr: case DroneData::DataType::DataPos:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataGyr(body); // Пришли данные
}
case DroneData::DataType::DataRange:
if (head.Mode == DroneData::DataMode::Request) if (head.Mode == DroneData::DataMode::Request)
{ {
return zero; // Запрос данных (не реализовано) return zero; // Запрос данных (не реализовано)
} }
else 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: case DroneData::DataType::DataMotor4:
if (head.Mode == DroneData::DataMode::Request) if (head.Mode == DroneData::DataMode::Request)
{ {
@ -169,7 +133,7 @@ namespace DroneClient {
} }
// Реализация метода DataStream // Реализация метода 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>^>(); System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();
@ -209,47 +173,25 @@ namespace DroneClient {
// Реализация метода SendRequest // Реализация метода SendRequest
array<Byte>^ Drone::SendRequest() array<Byte>^ Drone::SendRequest()
{ {
DroneData::DataHead^ acc = gcnew DroneData::DataHead(); DroneData::DataHead imu;
acc->Size = DroneData::DataHead::StrLen; imu.Size = DroneData::DataHead::StrLen;
acc->Mode = DroneData::DataMode::Request; imu.Mode = DroneData::DataMode::Request;
acc->Type = DroneData::DataType::DataAcc; imu.Type = DroneData::DataType::DataIMU;
DroneData::DataHead^ gyr = gcnew DroneData::DataHead(); DroneData::DataHead pos;
gyr->Size = DroneData::DataHead::StrLen; pos.Size = DroneData::DataHead::StrLen;
gyr->Mode = DroneData::DataMode::Request; pos.Mode = DroneData::DataMode::Request;
gyr->Type = DroneData::DataType::DataGyr; pos.Type = DroneData::DataType::DataPos;
DroneData::DataHead^ range = gcnew DroneData::DataHead(); array<Byte>^ si = GetBytes(imu);
range->Size = DroneData::DataHead::StrLen; array<Byte>^ sp = GetBytes(pos);
range->Mode = DroneData::DataMode::Request; array<Byte>^ sm = SendDataMotor4();
range->Type = DroneData::DataType::DataRange;
DroneData::DataHead^ local = gcnew DroneData::DataHead(); array<Byte>^ send = gcnew array<Byte>(si->Length + sp->Length + sm->Length);
local->Size = DroneData::DataHead::StrLen; Array::Copy(si, 0, send, 0, si->Length);
local->Mode = DroneData::DataMode::Request; Array::Copy(sp, 0, send, si->Length, sp->Length);
local->Type = DroneData::DataType::DataLocal; Array::Copy(sm, 0, send, si->Length + sp->Length, sm->Length);
List<array<byte>^>^ list = gcnew List<array<byte>^>(); return send;
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;
} }
} }

9
DroneClientCpp/Drone.h
View File

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

64
DroneClientCpp/DroneData.h
View File

@ -15,24 +15,18 @@ namespace DroneData {
public enum class DataType : unsigned short public enum class DataType : unsigned short
{ {
None = 0, Head = 1, None = 0, Head = 1,
DataIMU = 1001, DataPos = 1002,
// Output DataMotor4 = 2001, DataMotor6 = 2002
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
}; };
public value struct DataHead public value struct DataHead
{ {
public: public:
long Size; int Size;
DataMode Mode; DataMode Mode;
DataType Type; DataType Type;
unsigned long Time;
static const int StrLen = sizeof(DataHead); static const int StrLen = sizeof(DataHead);
}; };
@ -42,65 +36,30 @@ namespace DroneData {
float X, Y, Z; float X, Y, Z;
}; };
public value struct DataAcc public value struct DataIMU
{ {
public: public:
DataHead Head; DataHead Head;
XYZ Acc; XYZ Acc, Gyr, Mag;
unsigned long Time; static const int StrLen = sizeof(DataIMU);
static const int StrLen = sizeof(DataAcc);
}; };
public value struct DataGyr public value struct DataPos
{
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: public:
DataHead Head; DataHead Head;
XYZ Local;
float LiDAR; float LiDAR;
unsigned long Time; static const int StrLen = sizeof(DataPos);
static const int StrLen = sizeof(DataRange);
};
public value struct DataLocal
{
public:
DataHead Head;
XYZ Local;
unsigned long Time;
static const int StrLen = sizeof(DataLocal);
}; };
public value struct DataMotor4 public value struct DataMotor4
{ {
public: public:
DataHead Head; DataHead Head;
unsigned long long Count;
float UL, UR, DL, DR; float UL, UR, DL, DR;
static const int StrLen = sizeof(DataMotor4); static const int StrLen = sizeof(DataMotor4);
@ -110,6 +69,7 @@ namespace DroneData {
{ {
public: public:
DataHead Head; DataHead Head;
unsigned long long Count;
float UL, UR, LL, RR, DL, DR; float UL, UR, LL, RR, DL, DR;
static const int StrLen = sizeof(DataMotor6); 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); IPEndPoint^ ep = gcnew IPEndPoint(IPAddress::Parse(Addr), Port);
ServerSocket = gcnew Socket(AddressFamily::InterNetwork, SocketType::Stream, ProtocolType::Tcp); 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; } catch (...) { ServerSocket->Close(); return ClientState::Error; }
Connected = true; Connected = true;
@ -49,7 +49,7 @@ namespace DroneSimulator {
ReceiveData^ receiveData = gcnew ReceiveData(DataServer->buffer, ServerData::size, ServerSocket); 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 (...) {} catch (...) {}
return ClientState::Connected; return ClientState::Connected;
@ -58,9 +58,9 @@ namespace DroneSimulator {
// Реализация метода Close // Реализация метода Close
void NetClient::Close() void NetClient::Close()
{ {
try { if(ServerSocket) ServerSocket->Shutdown(SocketShutdown::Both); } try { ServerSocket->Shutdown(SocketShutdown::Both); }
catch (...) {} catch (...) {}
if(ServerSocket) ServerSocket->Close(); ServerSocket->Close();
Connected = false; Connected = false;
} }
@ -69,7 +69,7 @@ namespace DroneSimulator {
{ {
if (ServerSocket != nullptr && Connected) if (ServerSocket != nullptr && Connected)
{ {
try { if (ServerSocket) ServerSocket->Send(data); } try { ServerSocket->Send(data); }
catch (...) {} catch (...) {}
} }
} }
@ -86,7 +86,7 @@ namespace DroneSimulator {
if (bytes == 0) if (bytes == 0)
{ {
if (ServerSocket) ServerSocket->Close(); ServerSocket->Close();
Connected = false; Connected = false;
if (ServerSocket != nullptr) if (ServerSocket != nullptr)
@ -99,7 +99,7 @@ namespace DroneSimulator {
ReceiveCallback(gcnew ReceiveData(cd->Buffer, bytes, ServerSocket)); 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 (...) {} catch (...) {}
} }
} }

188
DroneSimulator/Drone.cs
View File

@ -2,9 +2,6 @@
using System.Numerics; using System.Numerics;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
using System.Security.Cryptography; 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 namespace DroneSimulator
{ {
@ -24,8 +21,6 @@ namespace DroneSimulator
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
public float LaserRange; // Имитация: Дальномер public float LaserRange; // Имитация: Дальномер
private uint DataTimer;
private const float Gravity = 9.8f; private const float Gravity = 9.8f;
private const float TO_GRAD = 180 / MathF.PI; private const float TO_GRAD = 180 / MathF.PI;
@ -34,13 +29,7 @@ namespace DroneSimulator
private Thread DroneThread; private Thread DroneThread;
private int Timer; private int Timer;
private Random MainRandom = new Random(); private static int CounterID = 0;
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();
public static byte[] getBytes(object data) public static byte[] getBytes(object data)
{ {
@ -83,6 +72,33 @@ namespace DroneSimulator
return mem; 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 struct DataVisual
{ {
public int ID; // Идентификатор public int ID; // Идентификатор
@ -248,14 +264,6 @@ namespace DroneSimulator
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt); if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
else LaserRange = float.MaxValue; 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) private float Range(float pow)
@ -284,94 +292,33 @@ namespace DroneSimulator
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR); 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)); imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
acc.Head.Mode = DroneData.DataMode.Response; imu.Head.Mode = DroneData.DataMode.Response;
acc.Head.Type = DroneData.DataType.DataAcc; imu.Head.Type = DroneData.DataType.DataIMU;
acc.Head.Time = (uint)Environment.TickCount;
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z; imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
acc.Time = RealAcc.timer; 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)); pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
gyr.Head.Mode = DroneData.DataMode.Response; pos.Head.Mode = DroneData.DataMode.Response;
gyr.Head.Type = DroneData.DataType.DataGyr; pos.Head.Type = DroneData.DataType.DataPos;
gyr.Head.Time = (uint)Environment.TickCount;
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z; pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
gyr.Time = RealGyr.timer; pos.LiDAR = LaserRange;
return getBytes(gyr); return getBytes(pos);
}
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);
} }
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body) private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
@ -380,12 +327,12 @@ namespace DroneSimulator
switch (head.Type) switch (head.Type)
{ {
case DroneData.DataType.DataAcc: case DroneData.DataType.DataIMU:
{ {
if (head.Mode == DroneData.DataMode.Request) if (head.Mode == DroneData.DataMode.Request)
{ {
// Запрос данных // Запрос данных
return SendDataAcc(); return SendDataIMU();
} }
else 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.DataMotor4:
{
case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero; if (head.Mode == DroneData.DataMode.Request)
{
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; //
return zero;
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero; }
else
{
// Пришли данные
RecvDataMotor4(body);
return zero;
}
}
} }
return zero; return zero;

67
DroneSimulator/DroneData.cs
View File

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

71
DroneSimulator/FormMain.cs
View File

@ -1,4 +1,4 @@
using System.Text; using System.Text;
using System.Numerics; using System.Numerics;
using System.Windows.Forms; using System.Windows.Forms;
using static System.Net.Mime.MediaTypeNames; using static System.Net.Mime.MediaTypeNames;
@ -22,13 +22,6 @@ namespace DroneSimulator
public Form_Main() public Form_Main()
{ {
InitializeComponent(); 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) private void ClientConnectionCallback(object o)
@ -82,7 +75,7 @@ namespace DroneSimulator
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size); List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
if (send == null) return; if (send == null) return;
try try
{ {
foreach (byte[]? b in send) foreach (byte[]? b in send)
{ {
@ -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;
}
}
}
}
}