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

update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-05-22 01:47:54 +03:00
parent e0f3e3db19
commit 90cec037eb
9 changed files with 553 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
DroneClient/Drone.cs
View File

@ -1,5 +1,7 @@
using System.Diagnostics.Metrics; using DroneData;
using System.Diagnostics.Metrics;
using System.Drawing; using System.Drawing;
using System.Numerics;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
namespace DroneClient namespace DroneClient
@ -15,6 +17,8 @@ namespace DroneClient
public float LaserRange; public float LaserRange;
public uint TimeRange; public uint TimeRange;
public Vector2 OF = Vector2.Zero;
public float MotorUL, MotorUR, MotorDL, MotorDR; public float MotorUL, MotorUR, MotorDL, MotorDR;
public static byte[] getBytes(object data) public static byte[] getBytes(object data)
@ -116,6 +120,16 @@ namespace DroneClient
return new byte[0]; return new byte[0];
} }
private byte[]? RecvDataOF(byte[] data)
{
DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF));
OF.X = of.X;
OF.Y = of.Y;
return new byte[0];
}
private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body) private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
{ {
byte[] zero = new byte[0]; byte[] zero = new byte[0];
@ -186,6 +200,22 @@ namespace DroneClient
} }
} }
case DroneData.DataType.DataOF:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataOF(body);
}
}
case DroneData.DataType.DataMotor4: case DroneData.DataType.DataMotor4:
{ {
if (head.Mode == DroneData.DataMode.Request) if (head.Mode == DroneData.DataMode.Request)
@ -271,12 +301,18 @@ namespace DroneClient
local.Mode = DroneData.DataMode.Request; local.Mode = DroneData.DataMode.Request;
local.Type = DroneData.DataType.DataLocal; local.Type = DroneData.DataType.DataLocal;
DroneData.DataHead of = new DroneData.DataHead();
of.Size = DroneData.DataHead.StrLen;
of.Mode = DroneData.DataMode.Request;
of.Type = DroneData.DataType.DataOF;
List<byte[]> list = new List<byte[]>(); List<byte[]> list = new List<byte[]>();
list.Add(getBytes(acc)); list.Add(getBytes(acc));
list.Add(getBytes(gyr)); list.Add(getBytes(gyr));
list.Add(getBytes(range)); list.Add(getBytes(range));
list.Add(getBytes(local)); list.Add(getBytes(local));
list.Add(getBytes(of));
list.Add(SendDataMotor4()); list.Add(SendDataMotor4());
int count = 0; int count = 0;

61
DroneClient/DroneData.cs
View File

@ -1,4 +1,4 @@
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
namespace DroneData namespace DroneData
{ {
@ -12,10 +12,13 @@ namespace DroneData
None = 0, Head = 1, None = 0, Head = 1,
// Output // Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Input // Input
DataMotor4 = 2001, DataMotor6 = 2002 DataMotor4 = 2001, DataMotor6 = 2002,
// State
DataQuat = 3001,
}; };
public struct DataHead public struct DataHead
@ -25,7 +28,7 @@ namespace DroneData
public DataMode Mode; public DataMode Mode;
public DataType Type; public DataType Type;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> public uint Time; // Общее время
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
} }
@ -37,7 +40,7 @@ namespace DroneData
public DataHead Head; public DataHead Head;
public XYZ Acc; public XYZ Acc;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
} }
@ -47,7 +50,7 @@ namespace DroneData
public DataHead Head; public DataHead Head;
public XYZ Gyr; public XYZ Gyr;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
} }
@ -57,31 +60,51 @@ namespace DroneData
public DataHead Head; public DataHead Head;
public XYZ Mag; public XYZ Mag;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
} }
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataRange public struct DataRange
{ {
public DataHead Head; public DataHead Head;
public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> public float LiDAR; // Датчик посадки
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange)); 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 DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
public struct DataMotor4 public struct DataMotor4
{ {
public DataHead Head; public DataHead Head;

131
DroneClient/FormMain.Designer.cs generated
View File

@ -38,12 +38,14 @@
label3 = new Label(); label3 = new Label();
label1 = new Label(); label1 = new Label();
groupBox2 = new GroupBox(); groupBox2 = new GroupBox();
label_time_acc = new Label();
label_Acc_Z = new Label(); label_Acc_Z = new Label();
label7 = new Label(); label7 = new Label();
label_Acc_Y = new Label(); label_Acc_Y = new Label();
label5 = new Label(); label5 = new Label();
label_Acc_X = new Label(); label_Acc_X = new Label();
groupBox3 = new GroupBox(); groupBox3 = new GroupBox();
label_time_gyr = new Label();
label_Gyr_Z = new Label(); label_Gyr_Z = new Label();
label9 = new Label(); label9 = new Label();
label_Gyr_Y = new Label(); label_Gyr_Y = new Label();
@ -51,6 +53,7 @@
label_Gyr_X = new Label(); label_Gyr_X = new Label();
label13 = new Label(); label13 = new Label();
groupBox4 = new GroupBox(); groupBox4 = new GroupBox();
label_time_range = new Label();
label_Pos_L = new Label(); label_Pos_L = new Label();
label6 = new Label(); label6 = new Label();
label_Pos_Y = new Label(); label_Pos_Y = new Label();
@ -65,15 +68,19 @@
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(); groupBox5 = new GroupBox();
label_time_range = new Label(); label_time_of = new Label();
label_time_gyr = new Label(); label_OF_Y = new Label();
label17 = new Label();
label_OF_X = new Label();
label19 = new Label();
groupBox1.SuspendLayout(); groupBox1.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
groupBox2.SuspendLayout(); groupBox2.SuspendLayout();
groupBox3.SuspendLayout(); groupBox3.SuspendLayout();
groupBox4.SuspendLayout(); groupBox4.SuspendLayout();
((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit(); ((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit();
groupBox5.SuspendLayout();
SuspendLayout(); SuspendLayout();
// //
// timer_Test // timer_Test
@ -92,7 +99,7 @@
groupBox1.Dock = DockStyle.Top; groupBox1.Dock = DockStyle.Top;
groupBox1.Location = new Point(0, 0); groupBox1.Location = new Point(0, 0);
groupBox1.Name = "groupBox1"; groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(275, 80); groupBox1.Size = new Size(357, 80);
groupBox1.TabIndex = 3; groupBox1.TabIndex = 3;
groupBox1.TabStop = false; groupBox1.TabStop = false;
groupBox1.Tag = ""; groupBox1.Tag = "";
@ -173,6 +180,15 @@
groupBox2.TabStop = false; groupBox2.TabStop = false;
groupBox2.Text = "Acc"; groupBox2.Text = "Acc";
// //
// 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_Acc_Z // label_Acc_Z
// //
label_Acc_Z.AutoSize = true; label_Acc_Z.AutoSize = true;
@ -227,13 +243,22 @@
groupBox3.Controls.Add(label11); groupBox3.Controls.Add(label11);
groupBox3.Controls.Add(label_Gyr_X); groupBox3.Controls.Add(label_Gyr_X);
groupBox3.Controls.Add(label13); groupBox3.Controls.Add(label13);
groupBox3.Location = new Point(95, 86); groupBox3.Location = new Point(94, 86);
groupBox3.Name = "groupBox3"; groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(78, 118); groupBox3.Size = new Size(78, 118);
groupBox3.TabIndex = 6; groupBox3.TabIndex = 6;
groupBox3.TabStop = false; groupBox3.TabStop = false;
groupBox3.Text = "Gyr"; groupBox3.Text = "Gyr";
// //
// 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";
//
// label_Gyr_Z // label_Gyr_Z
// //
label_Gyr_Z.AutoSize = true; label_Gyr_Z.AutoSize = true;
@ -297,13 +322,22 @@
groupBox4.Controls.Add(label10); groupBox4.Controls.Add(label10);
groupBox4.Controls.Add(label_Pos_X); groupBox4.Controls.Add(label_Pos_X);
groupBox4.Controls.Add(label14); groupBox4.Controls.Add(label14);
groupBox4.Location = new Point(188, 86); groupBox4.Location = new Point(182, 86);
groupBox4.Name = "groupBox4"; groupBox4.Name = "groupBox4";
groupBox4.Size = new Size(78, 118); groupBox4.Size = new Size(78, 118);
groupBox4.TabIndex = 7; groupBox4.TabIndex = 7;
groupBox4.TabStop = false; groupBox4.TabStop = false;
groupBox4.Text = "Pos"; groupBox4.Text = "Pos";
// //
// 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_Pos_L // label_Pos_L
// //
label_Pos_L.AutoSize = true; label_Pos_L.AutoSize = true;
@ -443,38 +477,71 @@
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 // groupBox5
// //
label_time_acc.AutoSize = true; groupBox5.Controls.Add(label_time_of);
label_time_acc.Location = new Point(6, 100); groupBox5.Controls.Add(label_OF_Y);
label_time_acc.Name = "label_time_acc"; groupBox5.Controls.Add(label17);
label_time_acc.Size = new Size(13, 15); groupBox5.Controls.Add(label_OF_X);
label_time_acc.TabIndex = 25; groupBox5.Controls.Add(label19);
label_time_acc.Text = "0"; groupBox5.Location = new Point(270, 86);
groupBox5.Name = "groupBox5";
groupBox5.Size = new Size(78, 118);
groupBox5.TabIndex = 24;
groupBox5.TabStop = false;
groupBox5.Text = "OF";
// //
// label_time_range // label_time_of
// //
label_time_range.AutoSize = true; label_time_of.AutoSize = true;
label_time_range.Location = new Point(6, 100); label_time_of.Location = new Point(6, 100);
label_time_range.Name = "label_time_range"; label_time_of.Name = "label_time_of";
label_time_range.Size = new Size(13, 15); label_time_of.Size = new Size(13, 15);
label_time_range.TabIndex = 27; label_time_of.TabIndex = 27;
label_time_range.Text = "0"; label_time_of.Text = "0";
// //
// label_time_gyr // label_OF_Y
// //
label_time_gyr.AutoSize = true; label_OF_Y.AutoSize = true;
label_time_gyr.Location = new Point(3, 100); label_OF_Y.Location = new Point(19, 45);
label_time_gyr.Name = "label_time_gyr"; label_OF_Y.Name = "label_OF_Y";
label_time_gyr.Size = new Size(13, 15); label_OF_Y.Size = new Size(13, 15);
label_time_gyr.TabIndex = 26; label_OF_Y.TabIndex = 7;
label_time_gyr.Text = "0"; label_OF_Y.Text = "0";
//
// label17
//
label17.AutoSize = true;
label17.Location = new Point(6, 45);
label17.Name = "label17";
label17.Size = new Size(17, 15);
label17.TabIndex = 6;
label17.Text = "Y:";
//
// label_OF_X
//
label_OF_X.AutoSize = true;
label_OF_X.Location = new Point(19, 19);
label_OF_X.Name = "label_OF_X";
label_OF_X.Size = new Size(13, 15);
label_OF_X.TabIndex = 5;
label_OF_X.Text = "0";
//
// label19
//
label19.AutoSize = true;
label19.Location = new Point(6, 19);
label19.Name = "label19";
label19.Size = new Size(17, 15);
label19.TabIndex = 4;
label19.Text = "X:";
// //
// Form_Main // Form_Main
// //
AutoScaleDimensions = new SizeF(7F, 15F); AutoScaleDimensions = new SizeF(7F, 15F);
AutoScaleMode = AutoScaleMode.Font; AutoScaleMode = AutoScaleMode.Font;
ClientSize = new Size(275, 447); ClientSize = new Size(357, 447);
Controls.Add(groupBox5);
Controls.Add(button_MR); Controls.Add(button_MR);
Controls.Add(button_ML); Controls.Add(button_ML);
Controls.Add(label_Pow); Controls.Add(label_Pow);
@ -501,6 +568,8 @@
groupBox4.ResumeLayout(false); groupBox4.ResumeLayout(false);
groupBox4.PerformLayout(); groupBox4.PerformLayout();
((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit(); ((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit();
groupBox5.ResumeLayout(false);
groupBox5.PerformLayout();
ResumeLayout(false); ResumeLayout(false);
PerformLayout(); PerformLayout();
} }
@ -546,5 +615,11 @@
private Label label_time_acc; private Label label_time_acc;
private Label label_time_range; private Label label_time_range;
private Label label_time_gyr; private Label label_time_gyr;
private GroupBox groupBox5;
private Label label_time_of;
private Label label_OF_Y;
private Label label17;
private Label label_OF_X;
private Label label19;
} }
} }

7
DroneClient/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 DroneSimulator.NetClient; using static DroneSimulator.NetClient;
@ -109,6 +109,11 @@ namespace DroneSimulator
label_time_range.Text = dataDrone.TimeRange.ToString(); label_time_range.Text = dataDrone.TimeRange.ToString();
label_OF_X.Text = dataDrone.OF.X.ToString();
label_OF_Y.Text = dataDrone.OF.Y.ToString();
label_time_of.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest()); netClient.SendData(dataDrone.SendReqest());
} }

40
DroneSimulator/Drone.cs
View File

@ -26,6 +26,8 @@ namespace DroneSimulator
public Vector4 Orientation; public Vector4 Orientation;
public static bool[] Freeze = new bool[3];
private uint DataTimer; private uint DataTimer;
private const float Gravity = 9.8f; private const float Gravity = 9.8f;
@ -36,13 +38,14 @@ namespace DroneSimulator
private Thread DroneThread; private Thread DroneThread;
private int Timer; private int Timer;
private Random MainRandom = new Random(); private Vector2 MoveOF = Vector2.Zero;
RealMode.Accelerometer RealAcc = new RealMode.Accelerometer(); RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
RealMode.Gyroscope RealGyr = new RealMode.Gyroscope(); RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
RealMode.Position RealPos = new RealMode.Position(); RealMode.Position RealPos = new RealMode.Position();
RealMode.Barometer RealBar = new RealMode.Barometer(); RealMode.Barometer RealBar = new RealMode.Barometer();
RealMode.Range RealRange = new RealMode.Range(); RealMode.Range RealRange = new RealMode.Range();
RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
public static byte[] getBytes(object data) public static byte[] getBytes(object data)
{ {
@ -169,7 +172,7 @@ namespace DroneSimulator
{ {
Quaternion grav = new Quaternion(0, 0, 1, 0); Quaternion grav = new Quaternion(0, 0, 1, 0);
grav = (Quat * grav) * Quaternion.Inverse(Quat); grav = Quat * grav * Quaternion.Inverse(Quat);
float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD; float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
if (yaw < 0.0f) yaw = 360.0f + yaw; if (yaw < 0.0f) yaw = 360.0f + yaw;
@ -201,6 +204,10 @@ namespace DroneSimulator
AccXYZ /= Gravity; // Вернуть измерения в G AccXYZ /= Gravity; // Вернуть измерения в G
if (Freeze[0]) { SpdXYZ.X = 0; PosXYZ.X = 0; }
if (Freeze[1]) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
if (Freeze[2]) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
if (PosXYZ.Z < 0) if (PosXYZ.Z < 0)
{ {
SpdPRY = Vector3.Zero; SpdPRY = Vector3.Zero;
@ -241,23 +248,26 @@ namespace DroneSimulator
//Active = false; // Перевернулся вверх ногами //Active = false; // Перевернулся вверх ногами
} }
Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0); Quaternion grav = Quat * new Quaternion(0, 0, flow/Mass, 0) * Quaternion.Inverse(Quat);
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
Acc = new Vector3(grav.X, grav.Y, grav.Z); Acc = new Vector3(grav.X, grav.Y, grav.Z);
Gyr = SpdPRY; Gyr = SpdPRY;
float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI; float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
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;
} }
MoveOF.X += SpdXYZ.X - Gyr.Y;
MoveOF.Y += SpdXYZ.Y + Gyr.X;
RealAcc.Update(Acc, (uint)tick); RealAcc.Update(Acc, (uint)tick);
RealGyr.Update(Gyr, (uint)tick); RealGyr.Update(Gyr, (uint)tick);
RealRange.Update(LaserRange, (uint)tick); RealRange.Update(LaserRange, (uint)tick);
RealBar.Update(PosXYZ.Z * 11, (uint)tick); RealBar.Update(PosXYZ.Z * 11, (uint)tick);
RealPos.Update(PosXYZ, (uint)tick); RealPos.Update(PosXYZ, (uint)tick);
RealOF.Update(MoveOF, LaserRange, (uint)tick);
DataTimer = (uint)tick; DataTimer = (uint)tick;
} }
@ -378,6 +388,24 @@ namespace DroneSimulator
return getBytes(bar); return getBytes(bar);
} }
private byte[] SendDataOF()
{
DroneData.DataOF of = new DroneData.DataOF();
of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
of.Head.Mode = DroneData.DataMode.Response;
of.Head.Type = DroneData.DataType.DataOF;
of.Head.Time = (uint)Environment.TickCount;
of.X = RealOF.result.X;
of.Y = RealOF.result.Y;
of.Time = RealBar.timer;
MoveOF = Vector2.Zero;
return getBytes(of);
}
private byte[] SendDataQuaternion() private byte[] SendDataQuaternion()
{ {
DroneData.DataQuat quat = new DroneData.DataQuat(); DroneData.DataQuat quat = new DroneData.DataQuat();
@ -424,6 +452,8 @@ namespace DroneSimulator
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero; case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
case DroneData.DataType.DataOF: if (head.Mode == DroneData.DataMode.Request) return SendDataOF(); else return zero;
case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero; case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); 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.Response) RecvDataMotor4(body); return zero;

12
DroneSimulator/DroneData.cs
View File

@ -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, DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Input // Input
DataMotor4 = 2001, DataMotor6 = 2002, DataMotor4 = 2001, DataMotor6 = 2002,
@ -95,6 +95,16 @@ namespace DroneData
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
} }
public struct DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
public struct DataMotor4 public struct DataMotor4
{ {
public DataHead Head; public DataHead Head;

338
DroneSimulator/FormMain.Designer.cs generated
View File

@ -70,17 +70,17 @@
label46 = new Label(); label46 = new Label();
label47 = new Label(); label47 = new Label();
groupBox_OF = new GroupBox(); groupBox_OF = new GroupBox();
label53 = new Label();
numericUpDown17 = new NumericUpDown();
label54 = new Label();
numericUpDown8 = new NumericUpDown();
numericUpDown7 = new NumericUpDown();
label41 = new Label();
label40 = new Label(); label40 = new Label();
numericUpDown_OF_Lens = new NumericUpDown();
label39 = new Label(); label39 = new Label();
numericUpDown_OF_Wait = new NumericUpDown();
label53 = new Label();
numericUpDown_OF_Laten = new NumericUpDown();
label54 = new Label();
numericUpDown_OF_Error = new NumericUpDown();
label38 = new Label(); label38 = new Label();
label37 = new Label(); label37 = new Label();
numericUpDown1 = new NumericUpDown(); numericUpDown_OF_Len = new NumericUpDown();
checkBox_OF_Enable = new CheckBox(); checkBox_OF_Enable = new CheckBox();
label17 = new Label(); label17 = new Label();
label16 = new Label(); label16 = new Label();
@ -151,15 +151,21 @@
numericUpDown_Acc_Freq = new NumericUpDown(); numericUpDown_Acc_Freq = new NumericUpDown();
label20 = new Label(); label20 = new Label();
groupBox2 = new GroupBox(); groupBox2 = new GroupBox();
checkBox_Mode_Real = new CheckBox(); radioButton1 = new RadioButton();
radioButton_Real = new RadioButton();
tabPage_Area = new TabPage(); tabPage_Area = new TabPage();
groupBox_Navi = new GroupBox(); groupBox_Navi = new GroupBox();
panel1 = new Panel(); panel1 = new Panel();
listBox_Drones = new ListBox();
button_Drone_Color = new Button(); button_Drone_Color = new Button();
comboBox_Drone_Rotor = new ComboBox(); comboBox_Drone_Rotor = new ComboBox();
comboBox_Drone = new ComboBox(); comboBox_Drone = new ComboBox();
timer_Test = new System.Windows.Forms.Timer(components); timer_Test = new System.Windows.Forms.Timer(components);
listBox_Drones = new ListBox(); groupBox3 = new GroupBox();
checkBox_Area_Freeze_X = new CheckBox();
label41 = new Label();
checkBox_Area_Freeze_Y = new CheckBox();
checkBox_Area_Freeze_Z = new CheckBox();
menuStrip_Menu.SuspendLayout(); menuStrip_Menu.SuspendLayout();
groupBox_Screen.SuspendLayout(); groupBox_Screen.SuspendLayout();
((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit(); ((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
@ -179,10 +185,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).BeginInit();
groupBox_OF.SuspendLayout(); groupBox_OF.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown17).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown8).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown7).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit();
groupBox_Barometer.SuspendLayout(); groupBox_Barometer.SuspendLayout();
@ -210,8 +217,10 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit();
groupBox2.SuspendLayout(); groupBox2.SuspendLayout();
tabPage_Area.SuspendLayout();
groupBox_Navi.SuspendLayout(); groupBox_Navi.SuspendLayout();
panel1.SuspendLayout(); panel1.SuspendLayout();
groupBox3.SuspendLayout();
SuspendLayout(); SuspendLayout();
// //
// menuStrip_Menu // menuStrip_Menu
@ -506,7 +515,7 @@
groupBox1.Controls.Add(label46); groupBox1.Controls.Add(label46);
groupBox1.Controls.Add(label47); groupBox1.Controls.Add(label47);
groupBox1.Dock = DockStyle.Top; groupBox1.Dock = DockStyle.Top;
groupBox1.Location = new Point(0, 749); groupBox1.Location = new Point(0, 732);
groupBox1.Name = "groupBox1"; groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(204, 133); groupBox1.Size = new Size(204, 133);
groupBox1.TabIndex = 8; groupBox1.TabIndex = 8;
@ -648,17 +657,17 @@
// //
// groupBox_OF // groupBox_OF
// //
groupBox_OF.Controls.Add(label53);
groupBox_OF.Controls.Add(numericUpDown17);
groupBox_OF.Controls.Add(label54);
groupBox_OF.Controls.Add(numericUpDown8);
groupBox_OF.Controls.Add(numericUpDown7);
groupBox_OF.Controls.Add(label41);
groupBox_OF.Controls.Add(label40); groupBox_OF.Controls.Add(label40);
groupBox_OF.Controls.Add(numericUpDown_OF_Lens);
groupBox_OF.Controls.Add(label39); groupBox_OF.Controls.Add(label39);
groupBox_OF.Controls.Add(numericUpDown_OF_Wait);
groupBox_OF.Controls.Add(label53);
groupBox_OF.Controls.Add(numericUpDown_OF_Laten);
groupBox_OF.Controls.Add(label54);
groupBox_OF.Controls.Add(numericUpDown_OF_Error);
groupBox_OF.Controls.Add(label38); groupBox_OF.Controls.Add(label38);
groupBox_OF.Controls.Add(label37); groupBox_OF.Controls.Add(label37);
groupBox_OF.Controls.Add(numericUpDown1); groupBox_OF.Controls.Add(numericUpDown_OF_Len);
groupBox_OF.Controls.Add(checkBox_OF_Enable); groupBox_OF.Controls.Add(checkBox_OF_Enable);
groupBox_OF.Controls.Add(label17); groupBox_OF.Controls.Add(label17);
groupBox_OF.Controls.Add(label16); groupBox_OF.Controls.Add(label16);
@ -669,95 +678,101 @@
groupBox_OF.Controls.Add(label14); groupBox_OF.Controls.Add(label14);
groupBox_OF.Controls.Add(label15); groupBox_OF.Controls.Add(label15);
groupBox_OF.Dock = DockStyle.Top; groupBox_OF.Dock = DockStyle.Top;
groupBox_OF.Enabled = false;
groupBox_OF.Location = new Point(0, 563); groupBox_OF.Location = new Point(0, 563);
groupBox_OF.Name = "groupBox_OF"; groupBox_OF.Name = "groupBox_OF";
groupBox_OF.Size = new Size(204, 186); groupBox_OF.Size = new Size(204, 169);
groupBox_OF.TabIndex = 4; groupBox_OF.TabIndex = 4;
groupBox_OF.TabStop = false; groupBox_OF.TabStop = false;
groupBox_OF.Text = "Optical flow"; groupBox_OF.Text = "Optical flow";
// //
// label40
//
label40.AutoSize = true;
label40.Location = new Point(158, 18);
label40.Name = "label40";
label40.Size = new Size(28, 15);
label40.TabIndex = 36;
label40.Text = "lens";
//
// numericUpDown_OF_Lens
//
numericUpDown_OF_Lens.DecimalPlaces = 1;
numericUpDown_OF_Lens.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Lens.Location = new Point(153, 36);
numericUpDown_OF_Lens.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
numericUpDown_OF_Lens.Minimum = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Lens.Name = "numericUpDown_OF_Lens";
numericUpDown_OF_Lens.Size = new Size(40, 23);
numericUpDown_OF_Lens.TabIndex = 35;
numericUpDown_OF_Lens.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Lens.ValueChanged += numericUpDown_OF_Update;
//
// label39
//
label39.AutoSize = true;
label39.Location = new Point(168, 111);
label39.Name = "label39";
label39.Size = new Size(24, 15);
label39.TabIndex = 34;
label39.Text = "sec";
//
// numericUpDown_OF_Wait
//
numericUpDown_OF_Wait.DecimalPlaces = 2;
numericUpDown_OF_Wait.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.Location = new Point(119, 109);
numericUpDown_OF_Wait.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Wait.Name = "numericUpDown_OF_Wait";
numericUpDown_OF_Wait.Size = new Size(47, 23);
numericUpDown_OF_Wait.TabIndex = 33;
numericUpDown_OF_Wait.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.ValueChanged += numericUpDown_OF_Update;
//
// label53 // label53
// //
label53.AutoSize = true; label53.AutoSize = true;
label53.Location = new Point(113, 155); label53.Location = new Point(113, 139);
label53.Name = "label53"; label53.Name = "label53";
label53.Size = new Size(24, 15); label53.Size = new Size(24, 15);
label53.TabIndex = 32; label53.TabIndex = 32;
label53.Text = "sec"; label53.Text = "sec";
// //
// numericUpDown17 // numericUpDown_OF_Laten
// //
numericUpDown17.DecimalPlaces = 2; numericUpDown_OF_Laten.DecimalPlaces = 2;
numericUpDown17.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown17.Location = new Point(66, 153); numericUpDown_OF_Laten.Location = new Point(66, 137);
numericUpDown17.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown17.Name = "numericUpDown17"; numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten";
numericUpDown17.Size = new Size(41, 23); numericUpDown_OF_Laten.Size = new Size(41, 23);
numericUpDown17.TabIndex = 31; numericUpDown_OF_Laten.TabIndex = 31;
numericUpDown17.Value = new decimal(new int[] { 1, 0, 0, 65536 }); numericUpDown_OF_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Laten.ValueChanged += numericUpDown_OF_Update;
// //
// label54 // label54
// //
label54.AutoSize = true; label54.AutoSize = true;
label54.Location = new Point(3, 155); label54.Location = new Point(3, 139);
label54.Name = "label54"; label54.Name = "label54";
label54.Size = new Size(55, 15); label54.Size = new Size(55, 15);
label54.TabIndex = 30; label54.TabIndex = 30;
label54.Text = "Lateness:"; label54.Text = "Lateness:";
// //
// numericUpDown8 // numericUpDown_OF_Error
// //
numericUpDown8.DecimalPlaces = 2; numericUpDown_OF_Error.DecimalPlaces = 1;
numericUpDown8.Increment = new decimal(new int[] { 1, 0, 0, 65536 }); numericUpDown_OF_Error.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown8.Location = new Point(126, 125); numericUpDown_OF_Error.Location = new Point(38, 109);
numericUpDown8.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); numericUpDown_OF_Error.Name = "numericUpDown_OF_Error";
numericUpDown8.Name = "numericUpDown8"; numericUpDown_OF_Error.Size = new Size(47, 23);
numericUpDown8.Size = new Size(46, 23); numericUpDown_OF_Error.TabIndex = 24;
numericUpDown8.TabIndex = 26; numericUpDown_OF_Error.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown8.Value = new decimal(new int[] { 1, 0, 0, 65536 }); numericUpDown_OF_Error.ValueChanged += numericUpDown_OF_Update;
//
// numericUpDown7
//
numericUpDown7.DecimalPlaces = 1;
numericUpDown7.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown7.Location = new Point(45, 125);
numericUpDown7.Name = "numericUpDown7";
numericUpDown7.Size = new Size(40, 23);
numericUpDown7.TabIndex = 24;
numericUpDown7.Value = new decimal(new int[] { 3, 0, 0, 0 });
//
// label41
//
label41.AutoSize = true;
label41.Location = new Point(171, 127);
label41.Name = "label41";
label41.Size = new Size(24, 15);
label41.TabIndex = 29;
label41.Text = "sec";
//
// label40
//
label40.AutoSize = true;
label40.Location = new Point(37, 107);
label40.Name = "label40";
label40.Size = new Size(64, 15);
label40.TabIndex = 28;
label40.Text = "probability";
//
// label39
//
label39.AutoSize = true;
label39.Location = new Point(135, 107);
label39.Name = "label39";
label39.Size = new Size(31, 15);
label39.TabIndex = 27;
label39.Text = "time";
// //
// label38 // label38
// //
label38.AutoSize = true; label38.AutoSize = true;
label38.Location = new Point(84, 127); label38.Location = new Point(83, 111);
label38.Name = "label38"; label38.Name = "label38";
label38.Size = new Size(17, 15); label38.Size = new Size(17, 15);
label38.TabIndex = 25; label38.TabIndex = 25;
@ -766,33 +781,35 @@
// label37 // label37
// //
label37.AutoSize = true; label37.AutoSize = true;
label37.Location = new Point(1, 127); label37.Location = new Point(1, 111);
label37.Name = "label37"; label37.Name = "label37";
label37.Size = new Size(35, 15); label37.Size = new Size(35, 15);
label37.TabIndex = 23; label37.TabIndex = 23;
label37.Text = "Error:"; label37.Text = "Error:";
// //
// numericUpDown1 // numericUpDown_OF_Len
// //
numericUpDown1.Location = new Point(68, 80); numericUpDown_OF_Len.Location = new Point(68, 80);
numericUpDown1.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_OF_Len.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown1.Name = "numericUpDown1"; numericUpDown_OF_Len.Name = "numericUpDown_OF_Len";
numericUpDown1.Size = new Size(40, 23); numericUpDown_OF_Len.Size = new Size(40, 23);
numericUpDown1.TabIndex = 20; numericUpDown_OF_Len.TabIndex = 20;
numericUpDown1.Value = new decimal(new int[] { 8, 0, 0, 0 }); numericUpDown_OF_Len.Value = new decimal(new int[] { 8, 0, 0, 0 });
numericUpDown_OF_Len.ValueChanged += numericUpDown_OF_Update;
// //
// checkBox_OF_Enable // checkBox_OF_Enable
// //
checkBox_OF_Enable.AutoSize = true; checkBox_OF_Enable.AutoSize = true;
checkBox_OF_Enable.Checked = true; checkBox_OF_Enable.Checked = true;
checkBox_OF_Enable.CheckState = CheckState.Checked; checkBox_OF_Enable.CheckState = CheckState.Checked;
checkBox_OF_Enable.Location = new Point(159, 157); checkBox_OF_Enable.Location = new Point(159, 141);
checkBox_OF_Enable.Name = "checkBox_OF_Enable"; checkBox_OF_Enable.Name = "checkBox_OF_Enable";
checkBox_OF_Enable.Size = new Size(39, 19); checkBox_OF_Enable.Size = new Size(39, 19);
checkBox_OF_Enable.TabIndex = 22; checkBox_OF_Enable.TabIndex = 22;
checkBox_OF_Enable.Tag = "#en"; checkBox_OF_Enable.Tag = "#en";
checkBox_OF_Enable.Text = "En"; checkBox_OF_Enable.Text = "En";
checkBox_OF_Enable.UseVisualStyleBackColor = true; checkBox_OF_Enable.UseVisualStyleBackColor = true;
checkBox_OF_Enable.CheckedChanged += numericUpDown_OF_Update;
// //
// label17 // label17
// //
@ -811,7 +828,7 @@
label16.Size = new Size(70, 15); label16.Size = new Size(70, 15);
label16.TabIndex = 19; label16.TabIndex = 19;
label16.Tag = "#max_height"; label16.Tag = "#max_height";
label16.Text = "Max height:"; label16.Text = "Max length:";
// //
// label12 // label12
// //
@ -831,6 +848,7 @@
numericUpDown_OF_Noise.Size = new Size(40, 23); numericUpDown_OF_Noise.Size = new Size(40, 23);
numericUpDown_OF_Noise.TabIndex = 17; numericUpDown_OF_Noise.TabIndex = 17;
numericUpDown_OF_Noise.Value = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_OF_Noise.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Noise.ValueChanged += numericUpDown_OF_Update;
// //
// label13 // label13
// //
@ -851,6 +869,7 @@
numericUpDown_OF_Freq.Size = new Size(40, 23); numericUpDown_OF_Freq.Size = new Size(40, 23);
numericUpDown_OF_Freq.TabIndex = 13; numericUpDown_OF_Freq.TabIndex = 13;
numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 }); numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 });
numericUpDown_OF_Freq.ValueChanged += numericUpDown_OF_Update;
// //
// label14 // label14
// //
@ -1541,7 +1560,8 @@
// //
// groupBox2 // groupBox2
// //
groupBox2.Controls.Add(checkBox_Mode_Real); groupBox2.Controls.Add(radioButton1);
groupBox2.Controls.Add(radioButton_Real);
groupBox2.Dock = DockStyle.Top; groupBox2.Dock = DockStyle.Top;
groupBox2.Location = new Point(0, 0); groupBox2.Location = new Point(0, 0);
groupBox2.Name = "groupBox2"; groupBox2.Name = "groupBox2";
@ -1550,19 +1570,33 @@
groupBox2.TabStop = false; groupBox2.TabStop = false;
groupBox2.Text = "Mode"; groupBox2.Text = "Mode";
// //
// checkBox_Mode_Real // radioButton1
// //
checkBox_Mode_Real.AutoSize = true; radioButton1.AutoSize = true;
checkBox_Mode_Real.Location = new Point(10, 22); radioButton1.Checked = true;
checkBox_Mode_Real.Name = "checkBox_Mode_Real"; radioButton1.Location = new Point(9, 19);
checkBox_Mode_Real.Size = new Size(107, 19); radioButton1.Name = "radioButton1";
checkBox_Mode_Real.TabIndex = 0; radioButton1.Size = new Size(50, 19);
checkBox_Mode_Real.Text = "Real simulation"; radioButton1.TabIndex = 2;
checkBox_Mode_Real.UseVisualStyleBackColor = true; radioButton1.TabStop = true;
checkBox_Mode_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged; radioButton1.Text = "Ideal";
radioButton1.UseVisualStyleBackColor = true;
radioButton1.CheckedChanged += checkBox_Mode_Real_CheckedChanged;
//
// radioButton_Real
//
radioButton_Real.AutoSize = true;
radioButton_Real.Location = new Point(150, 19);
radioButton_Real.Name = "radioButton_Real";
radioButton_Real.Size = new Size(47, 19);
radioButton_Real.TabIndex = 1;
radioButton_Real.Text = "Real";
radioButton_Real.UseVisualStyleBackColor = true;
radioButton_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged;
// //
// tabPage_Area // tabPage_Area
// //
tabPage_Area.Controls.Add(groupBox3);
tabPage_Area.Location = new Point(4, 24); tabPage_Area.Location = new Point(4, 24);
tabPage_Area.Name = "tabPage_Area"; tabPage_Area.Name = "tabPage_Area";
tabPage_Area.Size = new Size(210, 889); tabPage_Area.Size = new Size(210, 889);
@ -1595,6 +1629,14 @@
panel1.Size = new Size(194, 872); panel1.Size = new Size(194, 872);
panel1.TabIndex = 3; panel1.TabIndex = 3;
// //
// listBox_Drones
//
listBox_Drones.FormattingEnabled = true;
listBox_Drones.Location = new Point(3, 45);
listBox_Drones.Name = "listBox_Drones";
listBox_Drones.Size = new Size(188, 214);
listBox_Drones.TabIndex = 3;
//
// button_Drone_Color // button_Drone_Color
// //
button_Drone_Color.BackColor = Color.Tomato; button_Drone_Color.BackColor = Color.Tomato;
@ -1629,13 +1671,61 @@
timer_Test.Interval = 10; timer_Test.Interval = 10;
timer_Test.Tick += timer_Test_Tick; timer_Test.Tick += timer_Test_Tick;
// //
// listBox_Drones // groupBox3
// //
listBox_Drones.FormattingEnabled = true; groupBox3.Controls.Add(checkBox_Area_Freeze_Z);
listBox_Drones.Location = new Point(3, 45); groupBox3.Controls.Add(checkBox_Area_Freeze_Y);
listBox_Drones.Name = "listBox_Drones"; groupBox3.Controls.Add(label41);
listBox_Drones.Size = new Size(188, 214); groupBox3.Controls.Add(checkBox_Area_Freeze_X);
listBox_Drones.TabIndex = 3; groupBox3.Dock = DockStyle.Top;
groupBox3.Location = new Point(0, 0);
groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(210, 59);
groupBox3.TabIndex = 0;
groupBox3.TabStop = false;
groupBox3.Text = "Position";
//
// checkBox_Area_Freeze_X
//
checkBox_Area_Freeze_X.AutoSize = true;
checkBox_Area_Freeze_X.Location = new Point(55, 23);
checkBox_Area_Freeze_X.Name = "checkBox_Area_Freeze_X";
checkBox_Area_Freeze_X.Size = new Size(33, 19);
checkBox_Area_Freeze_X.TabIndex = 0;
checkBox_Area_Freeze_X.Text = "X";
checkBox_Area_Freeze_X.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_X.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
//
// label41
//
label41.AutoSize = true;
label41.Location = new Point(6, 24);
label41.Name = "label41";
label41.Size = new Size(43, 15);
label41.TabIndex = 1;
label41.Text = "Freeze:";
//
// checkBox_Area_Freeze_Y
//
checkBox_Area_Freeze_Y.AutoSize = true;
checkBox_Area_Freeze_Y.Location = new Point(103, 23);
checkBox_Area_Freeze_Y.Name = "checkBox_Area_Freeze_Y";
checkBox_Area_Freeze_Y.Size = new Size(33, 19);
checkBox_Area_Freeze_Y.TabIndex = 2;
checkBox_Area_Freeze_Y.Text = "Y";
checkBox_Area_Freeze_Y.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_Y.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
//
// checkBox_Area_Freeze_Z
//
checkBox_Area_Freeze_Z.AutoSize = true;
checkBox_Area_Freeze_Z.Location = new Point(151, 23);
checkBox_Area_Freeze_Z.Name = "checkBox_Area_Freeze_Z";
checkBox_Area_Freeze_Z.Size = new Size(33, 19);
checkBox_Area_Freeze_Z.TabIndex = 3;
checkBox_Area_Freeze_Z.Text = "Z";
checkBox_Area_Freeze_Z.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_Z.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
// //
// Form_Main // Form_Main
// //
@ -1676,10 +1766,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).EndInit();
groupBox_OF.ResumeLayout(false); groupBox_OF.ResumeLayout(false);
groupBox_OF.PerformLayout(); groupBox_OF.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown17).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown8).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown7).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit();
groupBox_Barometer.ResumeLayout(false); groupBox_Barometer.ResumeLayout(false);
@ -1713,8 +1804,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit();
groupBox2.ResumeLayout(false); groupBox2.ResumeLayout(false);
groupBox2.PerformLayout(); groupBox2.PerformLayout();
tabPage_Area.ResumeLayout(false);
groupBox_Navi.ResumeLayout(false); groupBox_Navi.ResumeLayout(false);
panel1.ResumeLayout(false); panel1.ResumeLayout(false);
groupBox3.ResumeLayout(false);
groupBox3.PerformLayout();
ResumeLayout(false); ResumeLayout(false);
PerformLayout(); PerformLayout();
} }
@ -1769,7 +1863,7 @@
private Label label15; private Label label15;
private CheckBox checkBox_Pos_Enable; private CheckBox checkBox_Pos_Enable;
private Label label17; private Label label17;
private NumericUpDown numericUpDown1; private NumericUpDown numericUpDown_OF_Len;
private Label label16; private Label label16;
private CheckBox checkBox_OF_Enable; private CheckBox checkBox_OF_Enable;
private CheckBox checkBox_Bar_Enable; private CheckBox checkBox_Bar_Enable;
@ -1809,12 +1903,8 @@
private Label label34; private Label label34;
private Label label36; private Label label36;
private Label label35; private Label label35;
private NumericUpDown numericUpDown7; private NumericUpDown numericUpDown_OF_Error;
private Label label37; private Label label37;
private Label label41;
private Label label40;
private Label label39;
private NumericUpDown numericUpDown8;
private Label label38; private Label label38;
private GroupBox groupBox1; private GroupBox groupBox1;
private CheckBox checkBox_Range_Enable; private CheckBox checkBox_Range_Enable;
@ -1843,13 +1933,23 @@
private NumericUpDown numericUpDown_Acc_Laten; private NumericUpDown numericUpDown_Acc_Laten;
private Label label50; private Label label50;
private Label label53; private Label label53;
private NumericUpDown numericUpDown17; private NumericUpDown numericUpDown_OF_Laten;
private Label label54; private Label label54;
private Label label55; private Label label55;
private NumericUpDown numericUpDown_Range_Laten; private NumericUpDown numericUpDown_Range_Laten;
private Label label56; private Label label56;
private GroupBox groupBox2; private GroupBox groupBox2;
private CheckBox checkBox_Mode_Real;
private ListBox listBox_Drones; private ListBox listBox_Drones;
private Label label39;
private NumericUpDown numericUpDown_OF_Wait;
private Label label40;
private NumericUpDown numericUpDown_OF_Lens;
private RadioButton radioButton1;
private RadioButton radioButton_Real;
private GroupBox groupBox3;
private CheckBox checkBox_Area_Freeze_X;
private CheckBox checkBox_Area_Freeze_Z;
private CheckBox checkBox_Area_Freeze_Y;
private Label label41;
} }
} }

25
DroneSimulator/FormMain.cs
View File

@ -23,7 +23,7 @@ namespace DroneSimulator
{ {
InitializeComponent(); InitializeComponent();
RealMode.RealSimulation = checkBox_Mode_Real.Checked; RealMode.RealSimulation = radioButton_Real.Checked;
numericUpDown_Acc_Update(null, null); numericUpDown_Acc_Update(null, null);
numericUpDown_Gyr_Update(null, null); numericUpDown_Gyr_Update(null, null);
numericUpDown_Pos_Update(null, null); numericUpDown_Pos_Update(null, null);
@ -236,7 +236,7 @@ namespace DroneSimulator
private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e) private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e)
{ {
RealMode.RealSimulation = checkBox_Mode_Real.Checked; RealMode.RealSimulation = radioButton_Real.Checked;
} }
private void numericUpDown_Acc_Update(object sender, EventArgs e) private void numericUpDown_Acc_Update(object sender, EventArgs e)
@ -277,5 +277,26 @@ namespace DroneSimulator
RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value; RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
} }
private void numericUpDown_OF_Update(object sender, EventArgs e)
{
RealMode.OpticalFlow.Freq = (uint)numericUpDown_OF_Freq.Value;
RealMode.OpticalFlow.Noise = (float)numericUpDown_OF_Noise.Value;
RealMode.OpticalFlow.Lateness = (float)numericUpDown_OF_Laten.Value;
RealMode.OpticalFlow.Enable = checkBox_OF_Enable.Checked;
RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10;
RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
RealMode.OpticalFlow.Error = (float)numericUpDown_OF_Error.Value * 10;
RealMode.OpticalFlow.Wait = (uint)numericUpDown_OF_Wait.Value * 1000;
}
private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
{
Drone.Freeze[0] = checkBox_Area_Freeze_X.Checked;
Drone.Freeze[1] = checkBox_Area_Freeze_Y.Checked;
Drone.Freeze[2] = checkBox_Area_Freeze_Z.Checked;
}
} }
} }

77
DroneSimulator/RealMode.cs
View File

@ -4,6 +4,7 @@ using System.Linq;
using System.Text; using System.Text;
using System.Numerics; using System.Numerics;
using System.Reflection; using System.Reflection;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
namespace DroneSimulator namespace DroneSimulator
{ {
@ -263,7 +264,83 @@ namespace DroneSimulator
internal class OpticalFlow internal class OpticalFlow
{ {
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static float Error;
public static uint Wait;
public static float Lens;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector2[] laten = new Vector2[count];
private uint index = 0;
public uint delay = 0;
public uint timer = 0;
public Vector2 result;
public void Update(Vector2 value, float Range, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
if (!Enable)
{
result = Vector2.NaN;
return;
}
value *= Lens;
if (rand.Next(0, 1000) < (Error * 10))
{
value = Vector2.Zero;
delay = time + Wait;
}
else if (delay > time)
{
value = Vector2.Zero;
}
if (Range > MaxHeight) value = Vector2.Zero;
else
{
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((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 Range internal class Range