Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| b7b07096d4 | |||
| 27a120fc7f | |||
| 1f97891439 |
@ -1,7 +1,5 @@
|
||||
using DroneData;
|
||||
using System.Diagnostics.Metrics;
|
||||
using System.Diagnostics.Metrics;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace DroneClient
|
||||
@ -17,8 +15,6 @@ namespace DroneClient
|
||||
public float LaserRange;
|
||||
public uint TimeRange;
|
||||
|
||||
public Vector2 OF = Vector2.Zero;
|
||||
|
||||
public float MotorUL, MotorUR, MotorDL, MotorDR;
|
||||
|
||||
public static byte[] getBytes(object data)
|
||||
@ -120,16 +116,6 @@ namespace DroneClient
|
||||
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)
|
||||
{
|
||||
byte[] zero = new byte[0];
|
||||
@ -200,22 +186,6 @@ namespace DroneClient
|
||||
}
|
||||
}
|
||||
|
||||
case DroneData.DataType.DataOF:
|
||||
{
|
||||
if (head.Mode == DroneData.DataMode.Request)
|
||||
{
|
||||
// Запрос данных
|
||||
// ... //
|
||||
//
|
||||
return zero;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Пришли данные
|
||||
return RecvDataOF(body);
|
||||
}
|
||||
}
|
||||
|
||||
case DroneData.DataType.DataMotor4:
|
||||
{
|
||||
if (head.Mode == DroneData.DataMode.Request)
|
||||
@ -301,18 +271,12 @@ namespace DroneClient
|
||||
local.Mode = DroneData.DataMode.Request;
|
||||
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.Add(getBytes(acc));
|
||||
list.Add(getBytes(gyr));
|
||||
list.Add(getBytes(range));
|
||||
list.Add(getBytes(local));
|
||||
list.Add(getBytes(of));
|
||||
list.Add(SendDataMotor4());
|
||||
|
||||
int count = 0;
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace DroneData
|
||||
{
|
||||
@ -12,13 +12,10 @@ namespace DroneData
|
||||
None = 0, Head = 1,
|
||||
|
||||
// Output
|
||||
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
|
||||
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
|
||||
|
||||
// Input
|
||||
DataMotor4 = 2001, DataMotor6 = 2002,
|
||||
|
||||
// State
|
||||
DataQuat = 3001,
|
||||
DataMotor4 = 2001, DataMotor6 = 2002
|
||||
};
|
||||
|
||||
public struct DataHead
|
||||
@ -28,7 +25,7 @@ namespace DroneData
|
||||
public DataMode Mode;
|
||||
public DataType Type;
|
||||
|
||||
public uint Time; // Общее время
|
||||
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
|
||||
}
|
||||
@ -40,7 +37,7 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Acc;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
|
||||
}
|
||||
@ -50,7 +47,7 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Gyr;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
|
||||
}
|
||||
@ -60,49 +57,29 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Mag;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
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 XYZ Local; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
|
||||
}
|
||||
|
||||
public struct DataBar
|
||||
public struct DataRange
|
||||
{
|
||||
public DataHead Head;
|
||||
public float Pressure; // Давление
|
||||
public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
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));
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
|
||||
}
|
||||
|
||||
public struct DataMotor4
|
||||
|
||||
164
DroneClient/FormMain.Designer.cs
generated
164
DroneClient/FormMain.Designer.cs
generated
@ -38,14 +38,12 @@
|
||||
label3 = new Label();
|
||||
label1 = new Label();
|
||||
groupBox2 = new GroupBox();
|
||||
label_time_acc = new Label();
|
||||
label_Acc_Z = new Label();
|
||||
label7 = new Label();
|
||||
label_Acc_Y = new Label();
|
||||
label5 = new Label();
|
||||
label_Acc_X = new Label();
|
||||
groupBox3 = new GroupBox();
|
||||
label_time_gyr = new Label();
|
||||
label_Gyr_Z = new Label();
|
||||
label9 = new Label();
|
||||
label_Gyr_Y = new Label();
|
||||
@ -53,7 +51,6 @@
|
||||
label_Gyr_X = new Label();
|
||||
label13 = new Label();
|
||||
groupBox4 = new GroupBox();
|
||||
label_time_range = new Label();
|
||||
label_Pos_L = new Label();
|
||||
label6 = new Label();
|
||||
label_Pos_Y = new Label();
|
||||
@ -68,22 +65,15 @@
|
||||
label_Pow = new Label();
|
||||
button_ML = new Button();
|
||||
button_MR = new Button();
|
||||
groupBox5 = new GroupBox();
|
||||
label_time_of = new Label();
|
||||
label_OF_Y = new Label();
|
||||
label17 = new Label();
|
||||
label_OF_X = new Label();
|
||||
label19 = new Label();
|
||||
trackBar_Value = new TrackBar();
|
||||
label4 = new Label();
|
||||
label_time_acc = new Label();
|
||||
label_time_range = new Label();
|
||||
label_time_gyr = new Label();
|
||||
groupBox1.SuspendLayout();
|
||||
((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
|
||||
groupBox2.SuspendLayout();
|
||||
groupBox3.SuspendLayout();
|
||||
groupBox4.SuspendLayout();
|
||||
((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit();
|
||||
groupBox5.SuspendLayout();
|
||||
((System.ComponentModel.ISupportInitialize)trackBar_Value).BeginInit();
|
||||
SuspendLayout();
|
||||
//
|
||||
// timer_Test
|
||||
@ -102,7 +92,7 @@
|
||||
groupBox1.Dock = DockStyle.Top;
|
||||
groupBox1.Location = new Point(0, 0);
|
||||
groupBox1.Name = "groupBox1";
|
||||
groupBox1.Size = new Size(446, 80);
|
||||
groupBox1.Size = new Size(275, 80);
|
||||
groupBox1.TabIndex = 3;
|
||||
groupBox1.TabStop = false;
|
||||
groupBox1.Tag = "";
|
||||
@ -178,20 +168,11 @@
|
||||
groupBox2.Controls.Add(label1);
|
||||
groupBox2.Location = new Point(6, 86);
|
||||
groupBox2.Name = "groupBox2";
|
||||
groupBox2.Size = new Size(100, 118);
|
||||
groupBox2.Size = new Size(78, 118);
|
||||
groupBox2.TabIndex = 5;
|
||||
groupBox2.TabStop = false;
|
||||
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.AutoSize = true;
|
||||
@ -246,22 +227,13 @@
|
||||
groupBox3.Controls.Add(label11);
|
||||
groupBox3.Controls.Add(label_Gyr_X);
|
||||
groupBox3.Controls.Add(label13);
|
||||
groupBox3.Location = new Point(112, 86);
|
||||
groupBox3.Location = new Point(95, 86);
|
||||
groupBox3.Name = "groupBox3";
|
||||
groupBox3.Size = new Size(103, 118);
|
||||
groupBox3.Size = new Size(78, 118);
|
||||
groupBox3.TabIndex = 6;
|
||||
groupBox3.TabStop = false;
|
||||
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.AutoSize = true;
|
||||
@ -325,22 +297,13 @@
|
||||
groupBox4.Controls.Add(label10);
|
||||
groupBox4.Controls.Add(label_Pos_X);
|
||||
groupBox4.Controls.Add(label14);
|
||||
groupBox4.Location = new Point(221, 86);
|
||||
groupBox4.Location = new Point(188, 86);
|
||||
groupBox4.Name = "groupBox4";
|
||||
groupBox4.Size = new Size(103, 118);
|
||||
groupBox4.Size = new Size(78, 118);
|
||||
groupBox4.TabIndex = 7;
|
||||
groupBox4.TabStop = false;
|
||||
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.AutoSize = true;
|
||||
@ -480,93 +443,38 @@
|
||||
button_MR.MouseDown += button_UU_MouseDown;
|
||||
button_MR.MouseUp += button_UU_MouseUp;
|
||||
//
|
||||
// groupBox5
|
||||
// label_time_acc
|
||||
//
|
||||
groupBox5.Controls.Add(label_time_of);
|
||||
groupBox5.Controls.Add(label_OF_Y);
|
||||
groupBox5.Controls.Add(label17);
|
||||
groupBox5.Controls.Add(label_OF_X);
|
||||
groupBox5.Controls.Add(label19);
|
||||
groupBox5.Location = new Point(330, 86);
|
||||
groupBox5.Name = "groupBox5";
|
||||
groupBox5.Size = new Size(104, 118);
|
||||
groupBox5.TabIndex = 24;
|
||||
groupBox5.TabStop = false;
|
||||
groupBox5.Text = "OF";
|
||||
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_of
|
||||
// label_time_range
|
||||
//
|
||||
label_time_of.AutoSize = true;
|
||||
label_time_of.Location = new Point(6, 100);
|
||||
label_time_of.Name = "label_time_of";
|
||||
label_time_of.Size = new Size(13, 15);
|
||||
label_time_of.TabIndex = 27;
|
||||
label_time_of.Text = "0";
|
||||
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_OF_Y
|
||||
// label_time_gyr
|
||||
//
|
||||
label_OF_Y.AutoSize = true;
|
||||
label_OF_Y.Location = new Point(19, 45);
|
||||
label_OF_Y.Name = "label_OF_Y";
|
||||
label_OF_Y.Size = new Size(13, 15);
|
||||
label_OF_Y.TabIndex = 7;
|
||||
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:";
|
||||
//
|
||||
// trackBar_Value
|
||||
//
|
||||
trackBar_Value.Location = new Point(349, 240);
|
||||
trackBar_Value.Maximum = 100;
|
||||
trackBar_Value.Minimum = 1;
|
||||
trackBar_Value.Name = "trackBar_Value";
|
||||
trackBar_Value.Orientation = Orientation.Vertical;
|
||||
trackBar_Value.Size = new Size(45, 195);
|
||||
trackBar_Value.TabIndex = 25;
|
||||
trackBar_Value.Value = 1;
|
||||
//
|
||||
// label4
|
||||
//
|
||||
label4.AutoSize = true;
|
||||
label4.Location = new Point(347, 219);
|
||||
label4.Name = "label4";
|
||||
label4.Size = new Size(47, 15);
|
||||
label4.TabIndex = 26;
|
||||
label4.Text = "POWER";
|
||||
label_time_gyr.AutoSize = true;
|
||||
label_time_gyr.Location = new Point(3, 100);
|
||||
label_time_gyr.Name = "label_time_gyr";
|
||||
label_time_gyr.Size = new Size(13, 15);
|
||||
label_time_gyr.TabIndex = 26;
|
||||
label_time_gyr.Text = "0";
|
||||
//
|
||||
// Form_Main
|
||||
//
|
||||
AutoScaleDimensions = new SizeF(7F, 15F);
|
||||
AutoScaleMode = AutoScaleMode.Font;
|
||||
ClientSize = new Size(446, 447);
|
||||
Controls.Add(label4);
|
||||
Controls.Add(trackBar_Value);
|
||||
Controls.Add(groupBox5);
|
||||
ClientSize = new Size(275, 447);
|
||||
Controls.Add(button_MR);
|
||||
Controls.Add(button_ML);
|
||||
Controls.Add(label_Pow);
|
||||
@ -593,9 +501,6 @@
|
||||
groupBox4.ResumeLayout(false);
|
||||
groupBox4.PerformLayout();
|
||||
((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit();
|
||||
groupBox5.ResumeLayout(false);
|
||||
groupBox5.PerformLayout();
|
||||
((System.ComponentModel.ISupportInitialize)trackBar_Value).EndInit();
|
||||
ResumeLayout(false);
|
||||
PerformLayout();
|
||||
}
|
||||
@ -641,12 +546,5 @@
|
||||
private Label label_time_acc;
|
||||
private Label label_time_range;
|
||||
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;
|
||||
private TrackBar trackBar_Value;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
using System.Text;
|
||||
using System.Text;
|
||||
using System.Numerics;
|
||||
using System.Windows.Forms;
|
||||
using static DroneSimulator.NetClient;
|
||||
@ -109,11 +109,6 @@ namespace DroneSimulator
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
@ -128,7 +123,7 @@ namespace DroneSimulator
|
||||
|
||||
private void button_UU_MouseDown(object sender, MouseEventArgs e)
|
||||
{
|
||||
float pow = ((float)trackBar_Value.Value) / 10.0f;
|
||||
const float pow = 0.1f;
|
||||
|
||||
if (sender == button_UU)
|
||||
{
|
||||
|
||||
@ -5,7 +5,7 @@ namespace DroneClient {
|
||||
// Реализация статического метода GetBytes
|
||||
array<Byte>^ Drone::GetBytes(Object^ data)
|
||||
{
|
||||
int size = Marshal::SizeOf(data);
|
||||
int size = Marshal::SizeOf(data);
|
||||
array<Byte>^ arr = gcnew array<Byte>(size);
|
||||
|
||||
IntPtr ptr = IntPtr::Zero;
|
||||
@ -45,12 +45,14 @@ namespace DroneClient {
|
||||
// Реализация приватного метода SendDataMotor4
|
||||
array<Byte>^ Drone::SendDataMotor4()
|
||||
{
|
||||
|
||||
DroneData::DataMotor4 mot4;
|
||||
|
||||
mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
|
||||
mot4.Head.Mode = DroneData::DataMode::Response;
|
||||
mot4.Head.Type = DroneData::DataType::DataMotor4;
|
||||
|
||||
updateData();
|
||||
setMotors();
|
||||
mot4.UL = MotorUL;
|
||||
mot4.UR = MotorUR;
|
||||
mot4.DL = MotorDL;
|
||||
@ -158,6 +160,92 @@ namespace DroneClient {
|
||||
return zero;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//void Drone::setMotors()
|
||||
//{
|
||||
// array<UInt16>^ ch = joy->Channels; // ссылка на тот же массив
|
||||
// UInt16 pow = (ch[2] - 1000) / 10;
|
||||
// float fpow = float(pow)/ 100.0f;
|
||||
|
||||
// const float maxAngle = 20.0f;
|
||||
//
|
||||
|
||||
// desPitch = (ch[1] - 1500) * maxAngle / 500;
|
||||
// desRoll = (ch[0] - 1499) * maxAngle / 500;
|
||||
|
||||
|
||||
// float errorPitch, errorRoll, forceRoll, forcePitch;
|
||||
// errorPitch = desPitch -pitch;
|
||||
// errorRoll = desRoll - roll;
|
||||
|
||||
// static float PRegulator = 0.001f;
|
||||
|
||||
// forcePitch = PRegulator * errorPitch;
|
||||
// forceRoll = PRegulator * errorRoll;
|
||||
|
||||
|
||||
// MotorUL = fpow-forcePitch + forceRoll;
|
||||
// MotorUR = fpow - forcePitch - forceRoll;
|
||||
// MotorDL = fpow + forcePitch + forceRoll;
|
||||
// MotorDR = fpow + forcePitch - forceRoll;
|
||||
//}
|
||||
|
||||
|
||||
/* ───────────── вспомогательная функция ───────────── */
|
||||
static float Clamp01(float v)
|
||||
{
|
||||
if (v < 0.0f) return 0.0f;
|
||||
if (v > 1.0f) return 1.0f;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ───────────── PD-регулятор и микширование ───────── */
|
||||
void Drone::setMotors()
|
||||
{
|
||||
/* ---------- входные каналы --------------------- */
|
||||
array<UInt16>^ ch = joy->Channels;
|
||||
const float fpow = (ch[2] - 1000) * 0.001f; // 0…1
|
||||
|
||||
/* ---------- желаемые углы ---------------------- */
|
||||
const float maxAngle = 20.0f;
|
||||
desPitch = (ch[1] - 1500) * maxAngle / 500.0f;
|
||||
desRoll = (ch[0] - 1499) * maxAngle / 500.0f;
|
||||
|
||||
|
||||
/* ---------- PD-регулятор ----------------------- */
|
||||
static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
|
||||
const float errPitch = desPitch - pitch;
|
||||
const float errRoll = desRoll - roll;
|
||||
|
||||
const float dt = 0.01f; // период 10 мс (100 Гц)
|
||||
const float dPitch = (errPitch - prevErrPitch) / dt;
|
||||
const float dRoll = (errRoll - prevErrRoll) / dt;
|
||||
|
||||
const float Kp = 0.115f;
|
||||
const float Kd = 0.0f;
|
||||
|
||||
const float forcePitch = Kp * errPitch + Kd * dPitch;
|
||||
const float forceRoll = Kp * errRoll + Kd * dRoll;
|
||||
|
||||
prevErrPitch = errPitch;
|
||||
prevErrRoll = errRoll;
|
||||
|
||||
/* ---------- распределение на моторы ------------ */
|
||||
MotorUL = fpow - forcePitch + forceRoll;
|
||||
MotorUR = fpow - forcePitch - forceRoll;
|
||||
MotorDL = fpow + forcePitch + forceRoll;
|
||||
MotorDR = fpow + forcePitch - forceRoll;
|
||||
|
||||
//MotorUL = Clamp01(MotorUL);
|
||||
//MotorUR = Clamp01(MotorUR);
|
||||
//MotorDL = Clamp01(MotorDL);
|
||||
//MotorDR = Clamp01(MotorDR);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// Реализация конструктора
|
||||
Drone::Drone()
|
||||
{
|
||||
@ -166,6 +254,11 @@ namespace DroneClient {
|
||||
DroneStreamHead.Mode = DroneData::DataMode::None;
|
||||
DroneStreamHead.Size = 0;
|
||||
DroneStreamHead.Type = DroneData::DataType::None;
|
||||
|
||||
this->joy = gcnew Joypad();
|
||||
|
||||
this->joy->Start("COM7", 115200);
|
||||
|
||||
}
|
||||
|
||||
// Реализация метода DataStream
|
||||
@ -252,4 +345,16 @@ namespace DroneClient {
|
||||
|
||||
return send;
|
||||
}
|
||||
|
||||
void Drone::updateData(){
|
||||
Vec3 acc{ this->AccX,this->AccY, this->AccZ };
|
||||
Vec3 gyr{ this->GyrX,this->GyrY, this->GyrZ };
|
||||
Vec3 mag{ 1,0, 0};
|
||||
ORI result = WorkAccGyroMag(acc, gyr, mag, 0, 0.01);
|
||||
this->pitch = result.Pitch;
|
||||
this->roll = -result.Roll;
|
||||
this->yaw = result.Yaw;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@ -3,10 +3,14 @@
|
||||
#include "DroneData.h"
|
||||
#include <Windows.h>
|
||||
#include <vcclr.h>
|
||||
#include "Orientation.h"
|
||||
#include "joypad.h"
|
||||
|
||||
|
||||
#using <System.dll>
|
||||
#using <mscorlib.dll>
|
||||
|
||||
|
||||
using namespace System;
|
||||
using namespace System::Collections::Generic;
|
||||
using namespace System::Runtime::InteropServices;
|
||||
@ -20,12 +24,18 @@ namespace DroneClient {
|
||||
float GyrX, GyrY, GyrZ;
|
||||
unsigned int TimeAcc, TimeGyr;
|
||||
|
||||
|
||||
float PosX, PosY;
|
||||
float LaserRange;
|
||||
unsigned int TimeRange;
|
||||
|
||||
float MotorUL, MotorUR, MotorDL, MotorDR;
|
||||
|
||||
float pitch, roll, yaw;
|
||||
|
||||
float desPitch, desRoll, desYaw;
|
||||
|
||||
Joypad^ joy;
|
||||
static array<Byte>^ GetBytes(Object^ data);
|
||||
static Object^ FromBytes(array<Byte>^ arr, Type^ type);
|
||||
|
||||
@ -37,6 +47,9 @@ namespace DroneClient {
|
||||
array<Byte>^ RecvDataLocal(array<Byte>^ data);
|
||||
array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
|
||||
|
||||
void setMotors();
|
||||
|
||||
|
||||
literal int DroneStreamCount = 512;
|
||||
array<Byte>^ DroneStreamData;
|
||||
int DroneStreamIndex;
|
||||
@ -47,5 +60,8 @@ namespace DroneClient {
|
||||
|
||||
System::Collections::Generic::List<array<Byte>^>^ DataStream(array<Byte>^ data, int size);
|
||||
array<Byte>^ SendRequest();
|
||||
void updateData();
|
||||
|
||||
|
||||
};
|
||||
}
|
||||
@ -121,6 +121,7 @@
|
||||
<ClCompile Include="Drone.cpp" />
|
||||
<ClCompile Include="FormMain.cpp" />
|
||||
<ClCompile Include="NetClient.cpp" />
|
||||
<ClCompile Include="Orientation.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="Drone.h" />
|
||||
@ -128,7 +129,10 @@
|
||||
<ClInclude Include="FormMain.h">
|
||||
<FileType>CppForm</FileType>
|
||||
</ClInclude>
|
||||
<ClInclude Include="joypad.h" />
|
||||
<ClInclude Include="NetClient.h" />
|
||||
<ClInclude Include="Orientation.h" />
|
||||
<ClInclude Include="WsServer.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<EmbeddedResource Include="FormMain.resx">
|
||||
|
||||
@ -24,6 +24,9 @@
|
||||
<ClCompile Include="Drone.cpp">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="Orientation.cpp">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="FormMain.h">
|
||||
@ -38,5 +41,14 @@
|
||||
<ClInclude Include="Drone.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="WsServer.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="Orientation.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="joypad.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@ -110,7 +110,7 @@ namespace DroneData {
|
||||
{
|
||||
public:
|
||||
DataHead Head;
|
||||
float UL, UR, LL, RR, DL, DR;
|
||||
float UL, UR, LL, RR, DL, DR;
|
||||
|
||||
static const int StrLen = sizeof(DataMotor6);
|
||||
};
|
||||
|
||||
@ -9,4 +9,5 @@ int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) {
|
||||
Application::SetCompatibleTextRenderingDefault(false);
|
||||
Application::Run(gcnew FormMain);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
@ -123,4 +123,7 @@
|
||||
<metadata name="timer1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
|
||||
<value>310, 17</value>
|
||||
</metadata>
|
||||
<metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
|
||||
<value>25</value>
|
||||
</metadata>
|
||||
</root>
|
||||
254
DroneClientCpp/Orientation.cpp
Normal file
254
DroneClientCpp/Orientation.cpp
Normal file
@ -0,0 +1,254 @@
|
||||
#include "orientation.h"
|
||||
#include <math.h>
|
||||
|
||||
static const float PI = 3.14159265359f;
|
||||
static const float TO_DEG = 180.0f / PI;
|
||||
static const float TO_RAD = PI / 180.0f;
|
||||
static const float R = 8.314f;
|
||||
static const float M = 0.029f;
|
||||
static const float g = 9.80665f;
|
||||
static const float ro = 1.189f;
|
||||
|
||||
struct Quaternion
|
||||
{
|
||||
float w, x, y, z;
|
||||
};
|
||||
|
||||
static Quaternion qCurrent = { 1, 0, 0, 0 };
|
||||
|
||||
static const float period = 0.005f;
|
||||
static bool isFirst = true;
|
||||
|
||||
static void vecNormalize(Vec3& v)
|
||||
{
|
||||
float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
|
||||
if (n > 1e-9f)
|
||||
{
|
||||
v.x /= n;
|
||||
v.y /= n;
|
||||
v.z /= n;
|
||||
}
|
||||
}
|
||||
|
||||
static Vec3 vecCross(const Vec3& a, const Vec3& b)
|
||||
{
|
||||
return
|
||||
{
|
||||
a.y * b.z - a.z * b.y,
|
||||
a.z * b.x - a.x * b.z,
|
||||
a.x * b.y - a.y * b.x
|
||||
};
|
||||
}
|
||||
|
||||
static void normalizeQuaternion(Quaternion& q)
|
||||
{
|
||||
float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
|
||||
if (norm > 1e-12f)
|
||||
{
|
||||
q.w /= norm;
|
||||
q.x /= norm;
|
||||
q.y /= norm;
|
||||
q.z /= norm;
|
||||
}
|
||||
}
|
||||
|
||||
static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
|
||||
{
|
||||
Quaternion r;
|
||||
r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
|
||||
r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
|
||||
r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
|
||||
r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
|
||||
{
|
||||
Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
|
||||
{
|
||||
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, Quaternion{ 0, In.x, In.y, In.z }), q);
|
||||
|
||||
return { r.x, r.y, r.z };
|
||||
}
|
||||
|
||||
static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
|
||||
{
|
||||
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, In), q);
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
static Quaternion createYawQuaternion(float angle)
|
||||
{
|
||||
Quaternion q;
|
||||
|
||||
q.w = cosf(angle);
|
||||
q.x = 0.0f;
|
||||
q.y = 0.0f;
|
||||
q.z = sinf(angle);
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity)
|
||||
{
|
||||
float acc = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
|
||||
if (acc > (1.0f + gravity) || acc < (1.0f - gravity)) return current;
|
||||
|
||||
vecNormalize(a);
|
||||
|
||||
Vec3 g{ 0, 0, 1 };
|
||||
Vec3 axis = vecCross(g, a);
|
||||
float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
|
||||
Quaternion q = { w, axis.x, axis.y, axis.z };
|
||||
normalizeQuaternion(q);
|
||||
|
||||
Quaternion qYaw{ current.w, 0, 0, current.z };
|
||||
|
||||
return quaternionMultiply(q, qYaw); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Z
|
||||
}
|
||||
|
||||
static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
|
||||
{
|
||||
Quaternion Mapp = current;
|
||||
Quaternion Spd{ 0, wx, wy, wz };
|
||||
Quaternion aq = quaternionMultiply(Spd, Mapp);
|
||||
|
||||
Mapp.w -= 0.5f * aq.w;
|
||||
Mapp.x -= 0.5f * aq.x;
|
||||
Mapp.y -= 0.5f * aq.y;
|
||||
Mapp.z -= 0.5f * aq.z;
|
||||
|
||||
normalizeQuaternion(Mapp);
|
||||
return Mapp;
|
||||
}
|
||||
|
||||
static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
|
||||
{
|
||||
|
||||
float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
|
||||
|
||||
Quaternion q2_ = q2;
|
||||
if (dot < 0)
|
||||
{
|
||||
q2_.w = -q2.w;
|
||||
q2_.x = -q2.x;
|
||||
q2_.y = -q2.y;
|
||||
q2_.z = -q2.z;
|
||||
}
|
||||
|
||||
Quaternion r;
|
||||
r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
|
||||
r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
|
||||
r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
|
||||
r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;
|
||||
|
||||
normalizeQuaternion(r);
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
inline float GetAngle(float a1, float a2, float az)
|
||||
{
|
||||
if (a2 == 0.0f && az == 0.0f) return a1 > 0.0f ? 90.0f : -90.0f;
|
||||
return atanf(a1 / sqrtf(a2 * a2 + az * az)) * TO_DEG;
|
||||
}
|
||||
|
||||
static Vec3 quaternionToPitchRollYaw(const Quaternion& q, float& Upside)
|
||||
{
|
||||
Quaternion pry = rotateVectorByQuaternion(q, { 0, 0, 1 });
|
||||
|
||||
Upside = (pry.z > 0.0f) ? 1.0f : -1.0f;
|
||||
|
||||
float yaw = 2 * atan2f(q.z, q.w) * TO_DEG;
|
||||
if (yaw < 0.0f) yaw = 360.0f + yaw;
|
||||
|
||||
return // Sovereign orientation
|
||||
{
|
||||
GetAngle(pry.y, pry.x, pry.z), // Pitch
|
||||
GetAngle(-pry.x, pry.y, pry.z), // Roll
|
||||
yaw // Yaw
|
||||
};
|
||||
}
|
||||
|
||||
static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
|
||||
{
|
||||
static Quaternion yq = createYawQuaternion(shift * TO_RAD);
|
||||
|
||||
vecNormalize(mag);
|
||||
|
||||
Quaternion mQ = { 0, mag.x, mag.y, mag.z };
|
||||
|
||||
Quaternion mW = backRotateVectorByQuaternion2(q, mQ);
|
||||
|
||||
mW = quaternionMultiply(mW, yq); // Shifting the axes to the true north
|
||||
|
||||
float gamma = mW.x * mW.x + mW.y * mW.y;
|
||||
float beta = sqrtf(gamma + mW.x * sqrtf(gamma));
|
||||
|
||||
Quaternion mD
|
||||
{
|
||||
beta / (sqrtf(2.0f * gamma)),
|
||||
0.0f,
|
||||
0.0f,
|
||||
mW.y / (sqrtf(2.0f) * beta),
|
||||
};
|
||||
|
||||
mD.w = (1.0f - alpha) + alpha * mD.w;
|
||||
mD.z = alpha * mD.z;
|
||||
|
||||
if (mD.w != mD.w || mD.x != mD.x || mD.y != mD.y || mD.z != mD.z) return;
|
||||
|
||||
q = quaternionMultiply(q, mD);
|
||||
normalizeQuaternion(q);
|
||||
}
|
||||
|
||||
// WorkAccGyro({DataIMU.Acc.X, DataIMU.Acc.Y, DataIMU.Acc.Z}, {DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z}, {-DataIMU.Mag.X, DataIMU.Mag.Y, DataIMU.Mag.Z}, 0.01);
|
||||
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
|
||||
{
|
||||
float wx = gyr.x * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||||
float wy = gyr.y * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||||
float wz = gyr.z * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||||
|
||||
Vec3 aB = acc;
|
||||
|
||||
Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f); // Tolerance for gravity deviation 5%
|
||||
|
||||
qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
|
||||
|
||||
Quaternion qFused = nlerp(qCurrent, qAcc, alpha);
|
||||
|
||||
if (isFirst)
|
||||
{
|
||||
qFused = qAcc;
|
||||
isFirst = false;
|
||||
}
|
||||
|
||||
qCurrent = qFused;
|
||||
|
||||
addMagneto(qCurrent, mag, alpha, mag_shift);
|
||||
|
||||
float up;
|
||||
Vec3 pry = quaternionToPitchRollYaw(qCurrent, up);
|
||||
|
||||
Vec3 ine = backRotateVectorByQuaternion(qCurrent, aB);
|
||||
return
|
||||
{
|
||||
sqrtf(pry.x * pry.x + pry.y * pry.y) * up,
|
||||
pry.x, pry.y, pry.z,
|
||||
ine.x, ine.y, ine.z
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
float calculateHeight(float bar, float temp)
|
||||
{
|
||||
static double firstBar = bar;
|
||||
return (R * (temp + 273) / (M * g)) * log(firstBar / bar);
|
||||
|
||||
}
|
||||
15
DroneClientCpp/Orientation.h
Normal file
15
DroneClientCpp/Orientation.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
struct Vec3 {
|
||||
float x, y, z;
|
||||
};
|
||||
|
||||
struct ORI
|
||||
{
|
||||
float Tilt; // Earth's plane tilt
|
||||
float Pitch, Roll, Yaw; // Sovereign orientation (not Euler)
|
||||
float IneX, IneY, IneZ; // Inertial accelerations
|
||||
};
|
||||
|
||||
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha);
|
||||
|
||||
float calculateHeight(float bar, float temp);
|
||||
156
DroneClientCpp/WsServer.h
Normal file
156
DroneClientCpp/WsServer.h
Normal file
@ -0,0 +1,156 @@
|
||||
#pragma once
|
||||
#include <Windows.h>
|
||||
#using <System.dll>
|
||||
#using <System.Net.Http.dll>
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct angles_native { float pitch, roll, yaw; };
|
||||
#pragma pack(pop)
|
||||
|
||||
using namespace System;
|
||||
using namespace System::Collections::Generic;
|
||||
using namespace System::Net;
|
||||
using namespace System::Net::WebSockets;
|
||||
using namespace System::Threading;
|
||||
using namespace System::Text;
|
||||
using namespace DroneClient; // ← поправьте, если у вас другое
|
||||
|
||||
|
||||
|
||||
public ref class WsServer // имя можно оставить тем же
|
||||
{
|
||||
public:
|
||||
WsServer() : _connected(false) {}
|
||||
|
||||
/* ---------- подключение --------------------------------------- */
|
||||
bool Connect(String^ uri)
|
||||
{
|
||||
if (_connected) return true; // уже подключены
|
||||
|
||||
_ws = gcnew ClientWebSocket();
|
||||
_cts = gcnew CancellationTokenSource();
|
||||
|
||||
try
|
||||
{
|
||||
// 1) запускаем асинхронное соединение
|
||||
System::Threading::Tasks::Task^ t =
|
||||
_ws->ConnectAsync(gcnew Uri(uri), _cts->Token);
|
||||
|
||||
// 2) ждём завершения
|
||||
t->Wait(); // здесь AggregateException «обёртывает»
|
||||
// все реальные ошибки
|
||||
_connected = true;
|
||||
}
|
||||
catch (AggregateException^ ag)
|
||||
{
|
||||
// сообщений может быть несколько – выводим главное
|
||||
Exception^ ex = ag->InnerExceptions->Count
|
||||
? ag->InnerExceptions[0] : ag;
|
||||
|
||||
System::String^ msg = ex->Message;
|
||||
|
||||
// типовые причины:
|
||||
// • WebSocketException (ошибка DNS / connection refused / timeout)
|
||||
// • NotSupportedException (Windows 7: платформа без WebSocket‑клиента)
|
||||
// • InvalidOperationException (неверный URI)
|
||||
// → выводим в Debug и просто возвращаем false
|
||||
System::Diagnostics::Debug::WriteLine("Connect error: " + msg);
|
||||
_connected = false;
|
||||
}
|
||||
catch (Exception^ ex) // на всякий случай – «прочие»
|
||||
{
|
||||
System::Diagnostics::Debug::WriteLine("Connect error: " + ex->Message);
|
||||
_connected = false;
|
||||
}
|
||||
return _connected;
|
||||
}
|
||||
|
||||
/* ---------- отключение ---------------------------------------- */
|
||||
void Disconnect()
|
||||
{
|
||||
if (!_connected) return;
|
||||
|
||||
try
|
||||
{
|
||||
_ws->CloseAsync(WebSocketCloseStatus::NormalClosure,
|
||||
"bye", CancellationToken::None)->Wait();
|
||||
}
|
||||
catch (Exception^) { /* игнор */ }
|
||||
|
||||
_ws = nullptr;
|
||||
_cts = nullptr;
|
||||
_connected = false;
|
||||
}
|
||||
|
||||
/* ---------- быстрая отправка строки --------------------------- */
|
||||
bool SendString(String^ msg)
|
||||
{
|
||||
if (!_connected) return false;
|
||||
|
||||
array<Byte>^ bytes = Encoding::UTF8->GetBytes(msg);
|
||||
try
|
||||
{
|
||||
_ws->SendAsync(ArraySegment<Byte>(bytes),
|
||||
WebSocketMessageType::Text,
|
||||
true, CancellationToken::None)->Wait();
|
||||
return true;
|
||||
}
|
||||
catch (Exception^) { return false; }
|
||||
}
|
||||
|
||||
/* ---------- состояние ---------------------------------------- */
|
||||
property bool IsConnected
|
||||
{
|
||||
bool get() { return _connected; }
|
||||
}
|
||||
|
||||
void SendAnglesBinary(float p, float r, float y)
|
||||
{
|
||||
angles_native a = { p, r, y };
|
||||
|
||||
// безопасно копируем struct → byte[]
|
||||
array<System::Byte>^ buf = gcnew array<System::Byte>(sizeof(a));
|
||||
System::Runtime::InteropServices::GCHandle h =
|
||||
System::Runtime::InteropServices::GCHandle::Alloc(buf,
|
||||
System::Runtime::InteropServices::GCHandleType::Pinned);
|
||||
|
||||
memcpy(h.AddrOfPinnedObject().ToPointer(), &a, sizeof(a));
|
||||
h.Free();
|
||||
|
||||
// шлём как Binary
|
||||
_ws->SendAsync(System::ArraySegment<Byte>(buf),
|
||||
System::Net::WebSockets::WebSocketMessageType::Binary,
|
||||
true, System::Threading::CancellationToken::None)->Wait();
|
||||
}
|
||||
|
||||
void TxLoop(System::Object^ param)
|
||||
{
|
||||
Drone^ d = safe_cast<Drone^>(param);
|
||||
// 20 мс = 50 Гц
|
||||
const int PERIOD_MS = 20;
|
||||
|
||||
// примерные значения, чтобы «шевелились»
|
||||
float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
|
||||
|
||||
while (_runTx && _connected)
|
||||
{
|
||||
float p = System::Threading::Volatile::Read(d->pitch);
|
||||
float r = System::Threading::Volatile::Read(d->roll);
|
||||
float y = System::Threading::Volatile::Read(d->yaw);
|
||||
|
||||
SendAnglesBinary(p,r, y);
|
||||
System::Threading::Thread::Sleep(PERIOD_MS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
ClientWebSocket^ _ws;
|
||||
CancellationTokenSource^ _cts;
|
||||
bool _connected;
|
||||
|
||||
public:
|
||||
// --- рядом с тем, где уже лежит wsClient ---
|
||||
System::Threading::Thread^ _txThread = nullptr; // поток передатчик
|
||||
bool _runTx = false; // признак «живого» цикла
|
||||
};
|
||||
137
DroneClientCpp/joypad.h
Normal file
137
DroneClientCpp/joypad.h
Normal file
@ -0,0 +1,137 @@
|
||||
// joypad.h ───────────────────────────────────────────────────────────
|
||||
#pragma once
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <Windows.h> // <- для AllocConsole
|
||||
#using <System.dll>
|
||||
|
||||
using namespace System;
|
||||
using namespace System::IO::Ports;
|
||||
using namespace System::Threading;
|
||||
|
||||
/*---------------------------------------------------------------------
|
||||
Joypad (i-Bus → COM-порт)
|
||||
---------------------------------------------------------------------*/
|
||||
public ref class Joypad
|
||||
{
|
||||
public:
|
||||
/* событие – _не подписывайтесь_ ➜ ничего в форме не вызовется */
|
||||
delegate void TickHandler(array<UInt16>^ ch);
|
||||
event TickHandler^ TickEvent;
|
||||
|
||||
Joypad()
|
||||
{
|
||||
// -------- открываем консоль один раз ----------------------
|
||||
static bool consoleReady = false;
|
||||
if (!consoleReady && ::AllocConsole())
|
||||
{
|
||||
FILE* fp;
|
||||
freopen_s(&fp, "CONOUT$", "w", stdout);
|
||||
std::cout << " CH1 CH2 CH3 CH4 CH5 CH6\n";
|
||||
consoleReady = true;
|
||||
}
|
||||
// ----------------------------------------------------------
|
||||
|
||||
_sp = gcnew SerialPort();
|
||||
_sp->ReadTimeout = 200;
|
||||
_sp->ReadBufferSize = 256;
|
||||
}
|
||||
|
||||
/* ---------- запуск ------------------------------------------- */
|
||||
bool Start(String^ port, int baud )
|
||||
{
|
||||
if (_run) return true;
|
||||
try
|
||||
{
|
||||
_sp->PortName = port;
|
||||
_sp->BaudRate = baud;
|
||||
_sp->Parity = Parity::None;
|
||||
_sp->DataBits = 8;
|
||||
_sp->StopBits = StopBits::One;
|
||||
_sp->Open();
|
||||
}
|
||||
catch (Exception^ ex)
|
||||
{
|
||||
System::Diagnostics::Debug::WriteLine("Serial error: " + ex->Message);
|
||||
return false;
|
||||
}
|
||||
|
||||
_run = true;
|
||||
_thr = gcnew Thread(gcnew ThreadStart(this, &Joypad::RxLoop));
|
||||
_thr->IsBackground = true;
|
||||
_thr->Start();
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---------- остановка ---------------------------------------- */
|
||||
void Stop()
|
||||
{
|
||||
_run = false;
|
||||
if (_thr && _thr->IsAlive) _thr->Join();
|
||||
if (_sp->IsOpen) _sp->Close();
|
||||
}
|
||||
|
||||
property array<UInt16>^ Channels { array<UInt16>^ get() { return _ch; } }
|
||||
|
||||
private:
|
||||
// ----------- поток приёма i-Bus ---------------------------------
|
||||
void RxLoop()
|
||||
{
|
||||
array<Byte>^ buf = gcnew array<Byte>(64);
|
||||
array<Byte>^ pkt = gcnew array<Byte>(32);
|
||||
|
||||
while (_run)
|
||||
{
|
||||
int read = 0;
|
||||
try { read = _sp->Read(buf, 0, buf->Length); }
|
||||
catch (TimeoutException^) { continue; }
|
||||
|
||||
for (int i = 0; i < read; ++i)
|
||||
{
|
||||
_fifo[_head++] = buf[i];
|
||||
if (_head >= _fifo->Length) _head = 0;
|
||||
|
||||
if (_fifo[_head] == 0x20 && _fifo[(_head + 1) & 0x7F] == 0x40)
|
||||
{
|
||||
for (int j = 0; j < 32; ++j)
|
||||
pkt[j] = _fifo[(_head + j) & 0x7F];
|
||||
|
||||
if (!CheckPkt(pkt)) continue;
|
||||
ParseChannels(pkt);
|
||||
|
||||
// -------- 1) печать в консоль ----------------
|
||||
std::cout << '\r';
|
||||
for (int k = 0; k < 6; ++k)
|
||||
std::cout << std::setw(6) << _ch[k] << ' ';
|
||||
std::cout << std::flush;
|
||||
// ---------------------------------------------
|
||||
|
||||
// -------- 2) необязательный вызов события ----
|
||||
TickEvent(_ch); // raise
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CheckPkt(array<Byte>^ p)
|
||||
{
|
||||
UInt16 sum = 0; for (int i = 0; i < 30; ++i) sum += p[i];
|
||||
sum = 0xFFFF - sum;
|
||||
return sum == (p[30] | p[31] << 8);
|
||||
}
|
||||
|
||||
void ParseChannels(array<Byte>^ p)
|
||||
{
|
||||
for (int i = 0; i < 14; ++i)
|
||||
_ch[i] = (UInt16)(p[2 + i * 2] | p[3 + i * 2] << 8);
|
||||
}
|
||||
|
||||
/* ---------- поля --------------------------------------------- */
|
||||
SerialPort^ _sp;
|
||||
Thread^ _thr;
|
||||
bool _run = false;
|
||||
|
||||
array<UInt16>^ _ch = gcnew array<UInt16>(14);
|
||||
array<Byte>^ _fifo = gcnew array<Byte>(128);
|
||||
int _head = 0;
|
||||
};
|
||||
@ -1,27 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
internal class Area
|
||||
{
|
||||
public struct Poisition
|
||||
{
|
||||
public struct Freeze{ public static bool X, Y, Z; }
|
||||
}
|
||||
|
||||
public struct Wind
|
||||
{
|
||||
public static bool Enable;
|
||||
public struct Speed { public static float From, To; }
|
||||
public static float Direction;
|
||||
public static float Density;
|
||||
public static float PosResist;
|
||||
public static float RotResist;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,26 +1,28 @@
|
||||
using System.Numerics;
|
||||
using System.CodeDom;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
using static System.Net.Mime.MediaTypeNames;
|
||||
using System.Security.Cryptography;
|
||||
using static DroneSimulator.Drone;
|
||||
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
internal class Drone
|
||||
{
|
||||
public int ID;
|
||||
|
||||
public float Mass; // Масса
|
||||
public bool Active; // Живой?
|
||||
public float Length; // Длинна лучей
|
||||
public const float Dynamic = 10; // Динамика вращения
|
||||
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
|
||||
public Quaternion Quat; // Основной кватернион
|
||||
public float Power = 0; // Тяга всех двигателей (0-1)
|
||||
|
||||
public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
|
||||
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
|
||||
|
||||
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
|
||||
public float LaserRange; // Имитация: Дальномер
|
||||
|
||||
public Vector4 Orientation;
|
||||
|
||||
private uint DataTimer;
|
||||
|
||||
private const float Gravity = 9.8f;
|
||||
@ -31,21 +33,17 @@ namespace DroneSimulator
|
||||
private Thread DroneThread;
|
||||
private int Timer;
|
||||
|
||||
private Vector2 MoveOF = Vector2.Zero;
|
||||
private Random MainRandom = new Random();
|
||||
|
||||
public struct Physics
|
||||
public struct DataBarometer
|
||||
{
|
||||
static public float Mass; // Масса
|
||||
static public float Length; // Длинна лучей
|
||||
static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
|
||||
public int Value, Pressure; // Значение давления в Паскальях
|
||||
public float Accuracy;
|
||||
public int Frequency;
|
||||
public uint Time;
|
||||
}
|
||||
|
||||
private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
|
||||
private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
|
||||
private RealMode.Position RealPos = new RealMode.Position();
|
||||
private RealMode.Barometer RealBar = new RealMode.Barometer();
|
||||
private RealMode.Range RealRange = new RealMode.Range();
|
||||
private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
|
||||
public DataBarometer dataBarometer;
|
||||
|
||||
public static byte[] getBytes(object data)
|
||||
{
|
||||
@ -88,28 +86,16 @@ namespace DroneSimulator
|
||||
return mem;
|
||||
}
|
||||
|
||||
public VisualData.VisualDrone GetVisual(int count, int index)
|
||||
public struct DataVisual
|
||||
{
|
||||
VisualData.VisualDrone drone = new VisualData.VisualDrone();
|
||||
public int ID; // Идентификатор
|
||||
public float W, X, Y, Z; // Кватернион вращения
|
||||
public float PosX, PosY, PosZ; // Координаты в пространстве
|
||||
}
|
||||
|
||||
drone.Head.Size = Marshal.SizeOf(typeof(VisualData.VisualDrone));
|
||||
drone.Head.Type = VisualData.VisualHead.VisualType.Drone;
|
||||
|
||||
drone.Count = count;
|
||||
drone.Index = index;
|
||||
|
||||
drone.ID = ID;
|
||||
drone.Color.A = 255; drone.Color.R = 255; drone.Color.G = 0; drone.Color.B = 0;
|
||||
|
||||
drone.Rotate.X = Quat.X; drone.Rotate.Y = Quat.Y; drone.Rotate.Z = Quat.Z; drone.Rotate.W = Quat.W;
|
||||
drone.Position.X = PosXYZ.X; drone.Position.Y = PosXYZ.Y; drone.Position.Z = PosXYZ.Z;
|
||||
drone.Scale = 1.0f;
|
||||
|
||||
drone.State = VisualData.VisualDrone.DroneState.Active;
|
||||
|
||||
drone.Power = Power;
|
||||
|
||||
return drone;
|
||||
public DataVisual GetVisual()
|
||||
{
|
||||
return new DataVisual() { ID = this.ID, W = this.Quat.W, X = this.Quat.X, Y = this.Quat.Y, Z = this.Quat.Z, PosX = this.PosXYZ.X, PosY = this.PosXYZ.Y, PosZ = this.PosXYZ.Z };
|
||||
}
|
||||
|
||||
private void ThreadFunction()
|
||||
@ -135,8 +121,12 @@ namespace DroneSimulator
|
||||
DroneThread.Start();
|
||||
}
|
||||
|
||||
public int Create()
|
||||
public int Create(float power, float mass, float len)
|
||||
{
|
||||
Mass = mass;
|
||||
Length = len;
|
||||
MaxPower = power;
|
||||
|
||||
Active = true;
|
||||
|
||||
return ID;
|
||||
@ -180,7 +170,7 @@ namespace DroneSimulator
|
||||
{
|
||||
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;
|
||||
if (yaw < 0.0f) yaw = 360.0f + yaw;
|
||||
@ -202,45 +192,16 @@ namespace DroneSimulator
|
||||
flow += flow * 0.1f; // Воздушная подушка
|
||||
}
|
||||
|
||||
|
||||
|
||||
float wind_x = 0, wind_y = 0, wind_z = 0;
|
||||
float wind_p = 0, wind_r = 0, wind_w = 0;
|
||||
|
||||
if (Area.Wind.Enable)
|
||||
{
|
||||
Quaternion dir = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), Area.Wind.Direction * TO_RADI * 2);
|
||||
|
||||
Quaternion spd = new Quaternion(0, Area.Wind.Speed.From, 0, 0) * dir;
|
||||
|
||||
float spd_x = spd.X - SpdXYZ.X;
|
||||
float spd_y = spd.Y - SpdXYZ.Y;
|
||||
float spd_z = spd.Z - SpdXYZ.Z;
|
||||
|
||||
wind_x = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_x * MathF.Abs(spd_x));
|
||||
wind_y = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_y * MathF.Abs(spd_y));
|
||||
wind_z = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_z * MathF.Abs(spd_z));
|
||||
|
||||
wind_p = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.X * MathF.Abs(SpdPRY.X));
|
||||
wind_r = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Y * MathF.Abs(SpdPRY.Y));
|
||||
wind_w = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Z * MathF.Abs(SpdPRY.Z));
|
||||
|
||||
AccPRY.X -= wind_p; AccPRY.Y -= wind_r; AccPRY.Z -= wind_w;
|
||||
}
|
||||
|
||||
SpdPRY += AccPRY * (Dynamic * time / (Physics.Mass * Physics.Length));
|
||||
SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
|
||||
|
||||
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
|
||||
AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Physics.Mass);
|
||||
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
|
||||
|
||||
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
|
||||
PosXYZ += SpdXYZ * time;
|
||||
|
||||
AccXYZ /= Gravity; // Вернуть измерения в G
|
||||
|
||||
if (Area.Poisition.Freeze.X) { SpdXYZ.X = 0; PosXYZ.X = 0; }
|
||||
if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
|
||||
if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
|
||||
|
||||
if (PosXYZ.Z < 0)
|
||||
{
|
||||
SpdPRY = Vector3.Zero;
|
||||
@ -252,8 +213,6 @@ namespace DroneSimulator
|
||||
|
||||
Vector4 ori = GetOrientation();
|
||||
|
||||
Orientation = ori;
|
||||
|
||||
if (PosXYZ.Z < 0)
|
||||
{
|
||||
PosXYZ.Z = 0;
|
||||
@ -281,26 +240,25 @@ namespace DroneSimulator
|
||||
//Active = false; // Перевернулся вверх ногами
|
||||
}
|
||||
|
||||
Quaternion grav = Quat * new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0) * Quaternion.Inverse(Quat);
|
||||
Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
|
||||
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
|
||||
Acc = new Vector3(grav.X, grav.Y, grav.Z);
|
||||
|
||||
Gyr = SpdPRY;
|
||||
|
||||
float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
|
||||
float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;
|
||||
|
||||
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
|
||||
else LaserRange = float.MaxValue;
|
||||
}
|
||||
|
||||
MoveOF.X += SpdXYZ.X - Gyr.Y;
|
||||
MoveOF.Y += SpdXYZ.Y + Gyr.X;
|
||||
|
||||
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);
|
||||
RealOF.Update(MoveOF, LaserRange, (uint)tick);
|
||||
if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer)
|
||||
{
|
||||
float bar = dataBarometer.Pressure - PosXYZ.Z * 11;
|
||||
int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10;
|
||||
dataBarometer.Value = (int)(bar + rnd);
|
||||
dataBarometer.Time = DataTimer;
|
||||
}
|
||||
|
||||
DataTimer = (uint)tick;
|
||||
}
|
||||
@ -310,7 +268,7 @@ namespace DroneSimulator
|
||||
if (pow > 1) pow = 1;
|
||||
if (pow < 0) pow = 0;
|
||||
|
||||
return pow * Physics.MaxPower;
|
||||
return pow * MaxPower;
|
||||
}
|
||||
|
||||
public void SetQadroPow(float ul, float ur, float dl, float dr)
|
||||
@ -340,8 +298,8 @@ namespace DroneSimulator
|
||||
acc.Head.Type = DroneData.DataType.DataAcc;
|
||||
acc.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
|
||||
acc.Time = RealAcc.timer;
|
||||
acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
|
||||
acc.Time = DataTimer;
|
||||
|
||||
return getBytes(acc);
|
||||
}
|
||||
@ -355,8 +313,8 @@ namespace DroneSimulator
|
||||
gyr.Head.Type = DroneData.DataType.DataGyr;
|
||||
gyr.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
|
||||
gyr.Time = RealGyr.timer;
|
||||
gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
|
||||
gyr.Time = DataTimer;
|
||||
|
||||
return getBytes(gyr);
|
||||
}
|
||||
@ -385,8 +343,8 @@ namespace DroneSimulator
|
||||
range.Head.Type = DroneData.DataType.DataRange;
|
||||
range.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
range.LiDAR = RealRange.result;
|
||||
range.Time = RealRange.timer;
|
||||
range.LiDAR = LaserRange;
|
||||
range.Time = DataTimer;
|
||||
|
||||
return getBytes(range);
|
||||
}
|
||||
@ -400,8 +358,8 @@ namespace DroneSimulator
|
||||
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;
|
||||
local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
|
||||
local.Time = DataTimer;
|
||||
|
||||
return getBytes(local);
|
||||
}
|
||||
@ -415,83 +373,15 @@ namespace DroneSimulator
|
||||
bar.Head.Type = DroneData.DataType.DataBar;
|
||||
bar.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
bar.Pressure = RealBar.result;
|
||||
bar.Time = RealBar.timer;
|
||||
bar.Pressure = dataBarometer.Value;
|
||||
bar.Time = dataBarometer.Time;
|
||||
|
||||
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[] SendDataGPS()
|
||||
{
|
||||
DroneData.DataGPS gps = new DroneData.DataGPS();
|
||||
|
||||
gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
|
||||
gps.Head.Mode = DroneData.DataMode.Response;
|
||||
gps.Head.Type = DroneData.DataType.DataGPS;
|
||||
gps.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
GPS.Point p = new GPS.Point();
|
||||
p.x = PosXYZ.Y; p.y= PosXYZ.X;
|
||||
|
||||
GPS.GlobalCoords g = new GPS.GlobalCoords();
|
||||
g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
|
||||
|
||||
g=GPS.localToGlobal(p, g);
|
||||
|
||||
gps.Lat = g.latitude; gps.Lon = g.longitude;
|
||||
|
||||
gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
|
||||
gps.Alt = GPS.Home.Alt + PosXYZ.Z;
|
||||
|
||||
DateTime tim = DateTime.Now;
|
||||
gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
|
||||
|
||||
gps.Fix = GPS.State.Fix;
|
||||
gps.SatVisible = GPS.State.SatVisible;
|
||||
gps.SatUsed = GPS.State.SatUsed;
|
||||
gps.Noise = GPS.State.Noise;
|
||||
gps.Hdop = GPS.State.Hdop;
|
||||
gps.Vdop = GPS.State.Vdop;
|
||||
gps.Pdop = GPS.State.Pdop;
|
||||
|
||||
return getBytes(gps);
|
||||
}
|
||||
|
||||
private byte[] SendDataQuaternion()
|
||||
{
|
||||
DroneData.DataQuat quat = new DroneData.DataQuat();
|
||||
|
||||
quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
|
||||
quat.Head.Mode = DroneData.DataMode.Response;
|
||||
quat.Head.Type = DroneData.DataType.DataQuat;
|
||||
quat.Head.Time = (uint)Environment.TickCount;
|
||||
|
||||
quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
|
||||
|
||||
return getBytes(quat);
|
||||
}
|
||||
|
||||
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
|
||||
{
|
||||
byte[] zero = Array.Empty<byte>();
|
||||
byte[] zero = new byte[0];
|
||||
|
||||
switch (head.Type)
|
||||
{
|
||||
@ -521,12 +411,6 @@ namespace DroneSimulator
|
||||
|
||||
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.DataGPS: if (head.Mode == DroneData.DataMode.Request) return SendDataGPS(); 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;
|
||||
}
|
||||
|
||||
|
||||
@ -1,4 +1,3 @@
|
||||
using System.Net;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace DroneData
|
||||
@ -13,13 +12,10 @@ namespace DroneData
|
||||
None = 0, Head = 1,
|
||||
|
||||
// Output
|
||||
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, DataGPS = 1008,
|
||||
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
|
||||
|
||||
// Input
|
||||
DataMotor4 = 2001, DataMotor6 = 2002,
|
||||
|
||||
// State
|
||||
DataQuat = 3001,
|
||||
DataMotor4 = 2001, DataMotor6 = 2002
|
||||
};
|
||||
|
||||
public struct DataHead
|
||||
@ -29,7 +25,7 @@ namespace DroneData
|
||||
public DataMode Mode;
|
||||
public DataType Type;
|
||||
|
||||
public uint Time; // Общее время
|
||||
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
|
||||
}
|
||||
@ -41,7 +37,7 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Acc;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
|
||||
}
|
||||
@ -51,7 +47,7 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Gyr;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
|
||||
}
|
||||
@ -61,7 +57,7 @@ namespace DroneData
|
||||
public DataHead Head;
|
||||
public XYZ Mag;
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
|
||||
}
|
||||
@ -69,9 +65,9 @@ namespace DroneData
|
||||
public struct DataRange
|
||||
{
|
||||
public DataHead Head;
|
||||
public float LiDAR; // Датчик посадки
|
||||
public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
|
||||
}
|
||||
@ -79,9 +75,9 @@ namespace DroneData
|
||||
public struct DataLocal
|
||||
{
|
||||
public DataHead Head;
|
||||
public XYZ Local; // Локальные координаты
|
||||
public XYZ Local; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
|
||||
}
|
||||
@ -89,43 +85,13 @@ namespace DroneData
|
||||
public struct DataBar
|
||||
{
|
||||
public DataHead Head;
|
||||
public float Pressure; // Давление
|
||||
public int Pressure; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
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>
|
||||
|
||||
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 DataGPS
|
||||
{
|
||||
public DataHead Head;
|
||||
|
||||
public double Lat, Lon; // Координаты (градусы)
|
||||
public float Alt; // Высота (метры)
|
||||
public float Speed; // Скорость (м/с)
|
||||
public int UTC; // Время UTC hhmmss
|
||||
|
||||
public byte Fix; // Тип решения 0-8 (NMEA Fix type)
|
||||
public byte SatVisible; // Количество видимых спутников
|
||||
public byte SatUsed; // Количество используемых спутников
|
||||
public float Hdop, Vdop, Pdop; // Геометрический фактор
|
||||
public float Noise; // Шум (db)
|
||||
|
||||
public uint Time; // Последнее время изменения данных
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
|
||||
}
|
||||
|
||||
public struct DataMotor4
|
||||
{
|
||||
public DataHead Head;
|
||||
@ -141,12 +107,4 @@ namespace DroneData
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
|
||||
}
|
||||
|
||||
public struct DataQuat
|
||||
{
|
||||
public DataHead Head;
|
||||
public float X, Y, Z, W;
|
||||
|
||||
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat));
|
||||
}
|
||||
}
|
||||
}
|
||||
1874
DroneSimulator/FormMain.Designer.cs
generated
1874
DroneSimulator/FormMain.Designer.cs
generated
File diff suppressed because it is too large
Load Diff
@ -1,4 +1,4 @@
|
||||
using System.Text;
|
||||
using System.Text;
|
||||
using System.Numerics;
|
||||
using System.Windows.Forms;
|
||||
using static System.Net.Mime.MediaTypeNames;
|
||||
@ -8,36 +8,19 @@ using System.Runtime.InteropServices;
|
||||
using System.CodeDom;
|
||||
using System.Linq;
|
||||
|
||||
using System.Net.WebSockets;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows.Forms;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
public partial class Form_Main : Form
|
||||
{
|
||||
Screen2D screen2D = null;
|
||||
|
||||
NetServerClients netServerClient = new NetServerClients();
|
||||
NetServerVisual netServerVisual = new NetServerVisual();
|
||||
|
||||
List<Drone> AllDrones = new List<Drone>();
|
||||
|
||||
public Form_Main()
|
||||
public partial class Form_Main : Form
|
||||
{
|
||||
InitializeComponent();
|
||||
Screen2D screen2D = null;
|
||||
|
||||
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);
|
||||
numericUpDown_OF_Update(null, null);
|
||||
checkBox_Area_Freeze_CheckedChanged(null, null);
|
||||
numericUpDown_Area_Wind_Update(null, null);
|
||||
numericUpDown_GPS_ValueChanged(null, null);
|
||||
numericUpDown_Physics_ValueChanged(null, null);
|
||||
}
|
||||
|
||||
private void ClientConnectionCallback(object o)
|
||||
{
|
||||
NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
|
||||
NetServerClients netServerClient = new NetServerClients();
|
||||
NetServerVisual netServerVisual = new NetServerVisual();
|
||||
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
@ -47,7 +30,16 @@ namespace DroneSimulator
|
||||
if (data.Connect)
|
||||
{
|
||||
Drone drone = new Drone(data.ID);
|
||||
drone.Create();
|
||||
drone.Create(1.0f, 0.5f, 1.0f);
|
||||
|
||||
//
|
||||
drone.dataBarometer.Pressure = 102258;
|
||||
drone.dataBarometer.Accuracy = (float)numericUpDown_Bar_Accur.Value;
|
||||
drone.dataBarometer.Frequency = (int)numericUpDown_Bar_Freq.Value;
|
||||
try { drone.dataBarometer.Pressure = int.Parse(textBox_Bar_Pressure.Text); }
|
||||
catch { MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error); }
|
||||
|
||||
//
|
||||
|
||||
screen2D.CreateDrone(Color.Red, data.ID);
|
||||
|
||||
@ -56,44 +48,193 @@ namespace DroneSimulator
|
||||
else
|
||||
{
|
||||
foreach (Drone drone in AllDrones)
|
||||
|
||||
{
|
||||
if (drone.ID != data.ID) continue;
|
||||
drone.Close();
|
||||
|
||||
screen2D.RemoveDrone(data.ID);
|
||||
|
||||
AllDrones.Remove(drone);
|
||||
break;
|
||||
InitializeComponent();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void ClientReceiveCallback(object o)
|
||||
{
|
||||
NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
|
||||
|
||||
Drone? drone = null;
|
||||
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
if (d.ID != data.ID) continue;
|
||||
drone = d;
|
||||
break;
|
||||
}
|
||||
|
||||
if (drone == null) return;
|
||||
|
||||
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
|
||||
|
||||
if (send == null) return;
|
||||
try
|
||||
{
|
||||
foreach (byte[]? b in send)
|
||||
private void ClientConnectionCallback(object o)
|
||||
{
|
||||
if (b != null) data.Client?.Send(b);
|
||||
NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
|
||||
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
label_Clients_Num.Text = data.Count.ToString();
|
||||
});
|
||||
|
||||
if (data.Connect)
|
||||
{
|
||||
Drone drone = new Drone(data.ID);
|
||||
drone.Create(1.0f, 0.5f, 1.0f);
|
||||
|
||||
screen2D.CreateDrone(Color.Red, data.ID);
|
||||
|
||||
AllDrones.Add(drone);
|
||||
}
|
||||
else
|
||||
{
|
||||
foreach (Drone drone in AllDrones)
|
||||
{
|
||||
if (drone.ID != data.ID) continue;
|
||||
drone.Close();
|
||||
|
||||
screen2D.RemoveDrone(data.ID);
|
||||
|
||||
AllDrones.Remove(drone);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
|
||||
private void ClientReceiveCallback(object o)
|
||||
{
|
||||
NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
|
||||
|
||||
Drone? drone = null;
|
||||
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
if (d.ID != data.ID) continue;
|
||||
drone = d;
|
||||
break;
|
||||
}
|
||||
|
||||
if (drone == null) return;
|
||||
|
||||
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
|
||||
|
||||
if (send == null) return;
|
||||
try
|
||||
{
|
||||
foreach (byte[]? b in send)
|
||||
{
|
||||
if (b != null) data.Client?.Send(b);
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
|
||||
private void button_Client_Start_Click(object sender, EventArgs e)
|
||||
{
|
||||
var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ClientConnectionCallback, ClientReceiveCallback);
|
||||
switch (done)
|
||||
{
|
||||
case NetServerClients.ServerState.Error:
|
||||
{
|
||||
MessageBox.Show("Error to start clients server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||||
break;
|
||||
}
|
||||
case NetServerClients.ServerState.Start:
|
||||
{
|
||||
button_Client_Start.Text = "Stop";
|
||||
button_Client_Start.BackColor = Color.LimeGreen;
|
||||
break;
|
||||
}
|
||||
case NetServerClients.ServerState.Stop:
|
||||
{
|
||||
label_Clients_Num.Text = "0";
|
||||
button_Client_Start.Text = "Start";
|
||||
button_Client_Start.BackColor = Color.Transparent;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (done != NetServerClients.ServerState.Start) return;
|
||||
|
||||
pictureBox_2D.Image = null;
|
||||
|
||||
screen2D = new Screen2D(DrawCallback);
|
||||
}
|
||||
|
||||
private void exitToolStripMenuItem_Click(object sender, EventArgs e)
|
||||
{
|
||||
Close();
|
||||
}
|
||||
|
||||
private void DrawCallback(Bitmap bmp)
|
||||
{
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
if (pictureBox_2D.Image == null) pictureBox_2D.Image = bmp;
|
||||
pictureBox_2D.Refresh();
|
||||
});
|
||||
}
|
||||
|
||||
private void timer_Test_Tick(object sender, EventArgs e)
|
||||
{
|
||||
if (screen2D == null) return;
|
||||
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
|
||||
}
|
||||
|
||||
screen2D.DrawScene();
|
||||
}
|
||||
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
|
||||
{
|
||||
foreach (Drone d in AllDrones) d.Close();
|
||||
}
|
||||
|
||||
private void VisualConnectionCallback(object o)
|
||||
{
|
||||
NetServerVisual.ConnectData data = (NetServerVisual.ConnectData)o;
|
||||
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
label_Clients_Num.Text = data.Count.ToString();
|
||||
});
|
||||
|
||||
if (data.Connect)
|
||||
{
|
||||
//---
|
||||
}
|
||||
else
|
||||
{
|
||||
//---
|
||||
}
|
||||
}
|
||||
|
||||
private void VisualReceiveCallback(object o)
|
||||
{
|
||||
NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
|
||||
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
Drone.DataVisual v = d.GetVisual();
|
||||
|
||||
try { data.Client.Send(Drone.getBytes(v)); }
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
|
||||
private void button_Visual_Start_Click(object sender, EventArgs e)
|
||||
{
|
||||
var done = netServerVisual.StartServer((int)numericUpDown_Visual_Port.Value, (int)numericUpDown_Visual_Limit.Value, VisualConnectionCallback, VisualReceiveCallback);
|
||||
switch (done)
|
||||
{
|
||||
case NetServerVisual.ServerState.Error:
|
||||
{
|
||||
MessageBox.Show("Error to start visual server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||||
break;
|
||||
}
|
||||
case NetServerVisual.ServerState.Start:
|
||||
{
|
||||
button_Visual_Start.Text = "Stop";
|
||||
button_Visual_Start.BackColor = Color.LimeGreen;
|
||||
break;
|
||||
}
|
||||
case NetServerVisual.ServerState.Stop:
|
||||
{
|
||||
label_Visual_Num.Text = "0";
|
||||
button_Visual_Start.Text = "Start";
|
||||
button_Visual_Start.BackColor = Color.Transparent;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
private void button_Client_Start_Click(object sender, EventArgs e)
|
||||
@ -110,6 +251,7 @@ namespace DroneSimulator
|
||||
{
|
||||
button_Client_Start.Text = "Stop";
|
||||
button_Client_Start.BackColor = Color.LimeGreen;
|
||||
panel_Menu_Model.Enabled = false;
|
||||
break;
|
||||
}
|
||||
case NetServerClients.ServerState.Stop:
|
||||
@ -117,6 +259,7 @@ namespace DroneSimulator
|
||||
label_Clients_Num.Text = "0";
|
||||
button_Client_Start.Text = "Start";
|
||||
button_Client_Start.BackColor = Color.Transparent;
|
||||
panel_Menu_Model.Enabled = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -144,27 +287,12 @@ namespace DroneSimulator
|
||||
|
||||
private void timer_Test_Tick(object sender, EventArgs e)
|
||||
{
|
||||
DateTime test = DateTime.Now;
|
||||
|
||||
int tim = test.Second + test.Minute * 100 + test.Hour * 10000;
|
||||
|
||||
|
||||
if (screen2D == null) return;
|
||||
|
||||
listBox_Drones.Items.Clear();
|
||||
|
||||
try
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
|
||||
|
||||
string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
|
||||
|
||||
listBox_Drones.Items.Add(line);
|
||||
}
|
||||
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
|
||||
}
|
||||
catch { }
|
||||
|
||||
screen2D.DrawScene();
|
||||
}
|
||||
@ -179,7 +307,7 @@ namespace DroneSimulator
|
||||
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
label_Visual_Num.Text = data.Count.ToString();
|
||||
label_Clients_Num.Text = data.Count.ToString();
|
||||
});
|
||||
|
||||
if (data.Connect)
|
||||
@ -196,11 +324,9 @@ namespace DroneSimulator
|
||||
{
|
||||
NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
|
||||
|
||||
int index = 0;
|
||||
|
||||
foreach (Drone d in AllDrones)
|
||||
{
|
||||
VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
|
||||
Drone.DataVisual v = d.GetVisual();
|
||||
|
||||
try { data.Client.Send(Drone.getBytes(v)); }
|
||||
catch { }
|
||||
@ -232,125 +358,6 @@ namespace DroneSimulator
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void numericUpDown_Bar_Update(object sender, EventArgs e)
|
||||
{
|
||||
RealMode.Barometer.RealSimulation = checkBox_Model_Bar_Real.Checked;
|
||||
|
||||
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 numericUpDown_Acc_Update(object sender, EventArgs e)
|
||||
{
|
||||
RealMode.Accelerometer.RealSimulation = checkBox_Model_Acc_Real.Checked;
|
||||
|
||||
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.RealSimulation = checkBox_Model_Gyr_Real.Checked;
|
||||
|
||||
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.RealSimulation = checkBox_Model_Pos_Real.Checked;
|
||||
|
||||
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.RealSimulation = checkBox_Model_Range_Real.Checked;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
private void numericUpDown_OF_Update(object sender, EventArgs e)
|
||||
{
|
||||
RealMode.OpticalFlow.RealSimulation = checkBox_Model_OF_Real.Checked;
|
||||
|
||||
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)
|
||||
{
|
||||
Area.Poisition.Freeze.X = checkBox_Area_Freeze_X.Checked;
|
||||
Area.Poisition.Freeze.Y = checkBox_Area_Freeze_Y.Checked;
|
||||
Area.Poisition.Freeze.Z = checkBox_Area_Freeze_Z.Checked;
|
||||
}
|
||||
|
||||
private void numericUpDown_Area_Wind_Update(object sender, EventArgs e)
|
||||
{
|
||||
Area.Wind.Enable = checkBox_Area_Wind_Enable.Checked;
|
||||
Area.Wind.Speed.From = (float)numericUpDown_Area_Wind_Speed_From.Value;
|
||||
Area.Wind.Speed.To = (float)numericUpDown_Area_Wind_Speed_To.Value;
|
||||
Area.Wind.Direction = (float)numericUpDown_Area_Wind_Direction.Value;
|
||||
Area.Wind.Density = (float)numericUpDown_Area_Wind_Density.Value;
|
||||
Area.Wind.PosResist = ((float)numericUpDown_Area_Wind_PosResist.Value) / 1000.0f;
|
||||
Area.Wind.RotResist = ((float)numericUpDown_Area_Wind_RotResist.Value) / 1000.0f;
|
||||
}
|
||||
|
||||
private void numericUpDown_GPS_ValueChanged(object sender, EventArgs e)
|
||||
{
|
||||
GPS.Home.Lat = (double)numericUpDown_GPS_Lat.Value;
|
||||
GPS.Home.Lon = (double)numericUpDown_GPS_Lon.Value;
|
||||
GPS.Home.Alt = (float)numericUpDown_GPS_Alt.Value;
|
||||
|
||||
GPS.State.Fix = (byte)numericUpDown_GPS_Fix.Value;
|
||||
GPS.State.SatVisible = (byte)numericUpDown_GPS_Vis.Value;
|
||||
GPS.State.SatUsed = (byte)numericUpDown_GPS_Use.Value;
|
||||
GPS.State.Noise = (float)numericUpDown_GPS_Noise.Value;
|
||||
GPS.State.Hdop = (float)numericUpDown_GPS_HDOP.Value;
|
||||
GPS.State.Vdop = (float)numericUpDown_GPS_VDOP.Value;
|
||||
GPS.State.Pdop = (float)numericUpDown_GPS_PDOP.Value;
|
||||
}
|
||||
|
||||
private void numericUpDown_Physics_ValueChanged(object sender, EventArgs e)
|
||||
{
|
||||
Drone.Physics.Mass = (float)numericUpDown_Physics_Mass.Value;
|
||||
Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value;
|
||||
Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -117,11 +117,14 @@
|
||||
<resheader name="writer">
|
||||
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
|
||||
</resheader>
|
||||
<metadata name="menuStrip_Menu.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
|
||||
<value>17, 17</value>
|
||||
</metadata>
|
||||
<metadata name="timer_Test.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
|
||||
<value>162, 5</value>
|
||||
</metadata>
|
||||
<metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
|
||||
<value>78</value>
|
||||
<value>25</value>
|
||||
</metadata>
|
||||
<assembly alias="System.Drawing" name="System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" />
|
||||
<data name="$this.Icon" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
|
||||
|
||||
@ -1,74 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
internal class GPS
|
||||
{
|
||||
static double PI = 3.14159265358979323846;
|
||||
public struct Home
|
||||
{
|
||||
public static double Lat, Lon;
|
||||
public static float Alt;
|
||||
}
|
||||
|
||||
public struct State
|
||||
{
|
||||
public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
|
||||
public static byte SatVisible; // Количество видимых спутников
|
||||
public static byte SatUsed; // Количество используемых спутников
|
||||
public static float Hdop, Vdop, Pdop; // Геометрический фактор
|
||||
public static float Noise; // Шум (db)
|
||||
}
|
||||
|
||||
public struct GlobalCoords
|
||||
{
|
||||
public double latitude, longitude;
|
||||
}
|
||||
|
||||
public struct Point
|
||||
{
|
||||
public double x, y;
|
||||
}
|
||||
|
||||
// Конвертация градусов в радианы
|
||||
static double deg2rad(double deg)
|
||||
{
|
||||
return deg * PI / 180.0;
|
||||
}
|
||||
|
||||
// Конвертация радиан в градусы
|
||||
static double rad2deg(double rad)
|
||||
{
|
||||
return rad * 180.0 / PI;
|
||||
}
|
||||
|
||||
// Перевод локальных координат в глобальные
|
||||
public static GlobalCoords localToGlobal(Point local, GlobalCoords origin)
|
||||
{
|
||||
const double er = 6371000; // Radius of the earth in m
|
||||
|
||||
// Преобразование приращений координат
|
||||
double dLat = local.x / er; // В радианах
|
||||
double originLatRad = deg2rad(origin.latitude);
|
||||
|
||||
// Вычисление новой широты
|
||||
double newLatRad = originLatRad + dLat;
|
||||
double newLat = rad2deg(newLatRad);
|
||||
|
||||
// Вычисление новой долготы (с использованием средней широты для точности)
|
||||
double avgLatRad = (originLatRad + newLatRad) / 2.0;
|
||||
double dLon = local.y / (er * Math.Cos(avgLatRad)); // В радианах
|
||||
double newLon = origin.longitude + rad2deg(dLon);
|
||||
GlobalCoords coord = new GlobalCoords();
|
||||
coord.latitude = newLat;
|
||||
coord.longitude = newLon;
|
||||
return coord;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,416 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
internal class RealMode
|
||||
{
|
||||
|
||||
|
||||
internal class Accelerometer
|
||||
{
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float ScaleLeft;
|
||||
public static float ScaleRight;
|
||||
public static float Lateness;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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 (!Enable)
|
||||
{
|
||||
result = Vector3.NaN;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!RealSimulation)
|
||||
{
|
||||
result = value;
|
||||
timer = time;
|
||||
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;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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 (!Enable)
|
||||
{
|
||||
result = float.NaN;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!RealSimulation)
|
||||
{
|
||||
result = value;
|
||||
timer = time;
|
||||
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
|
||||
{
|
||||
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;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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 (!Enable)
|
||||
{
|
||||
result = Vector2.NaN;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!RealSimulation)
|
||||
{
|
||||
result = value;
|
||||
timer = time;
|
||||
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
|
||||
{
|
||||
public static bool Enable;
|
||||
public static float MaxHeight;
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float Lateness;
|
||||
public static bool RealSimulation;
|
||||
|
||||
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 (!Enable)
|
||||
{
|
||||
result = float.NaN;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!RealSimulation)
|
||||
{
|
||||
result = value;
|
||||
timer = time;
|
||||
return;
|
||||
}
|
||||
|
||||
if (value > MaxHeight) value = MaxHeight;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -106,55 +106,51 @@ namespace DroneSimulator
|
||||
|
||||
foreach (var d in DroneList)
|
||||
{
|
||||
try
|
||||
if (d.Azimuth >= 360) d.Azimuth -= 360;
|
||||
var bmp = RotateImage(d.Drone, d.Azimuth);
|
||||
|
||||
g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
|
||||
|
||||
g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
|
||||
|
||||
//g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
|
||||
|
||||
float x1 = 0, y1 = 0;
|
||||
float x2 = 130, y2 = 0;
|
||||
float x3 = 0, y3 = 130;
|
||||
|
||||
const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
|
||||
Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
|
||||
|
||||
tilt = tilt * rotate * rotate;
|
||||
|
||||
if (tilt.Y > 0)
|
||||
{
|
||||
if (d.Azimuth >= 360) d.Azimuth -= 360;
|
||||
var bmp = RotateImage(d.Drone, d.Azimuth);
|
||||
|
||||
g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
|
||||
|
||||
g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
|
||||
|
||||
//g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
|
||||
|
||||
float x1 = 0, y1 = 0;
|
||||
float x2 = 130, y2 = 0;
|
||||
float x3 = 0, y3 = 130;
|
||||
|
||||
const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
|
||||
Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
|
||||
|
||||
tilt = tilt * rotate * rotate;
|
||||
|
||||
if (tilt.Y > 0)
|
||||
{
|
||||
x1 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
x3 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
}
|
||||
else
|
||||
{
|
||||
x2 = (int)(Math.Cos(tilt.Y) * 130);
|
||||
}
|
||||
|
||||
if (tilt.X > 0)
|
||||
{
|
||||
y1 = (int)(Math.Sin(tilt.X) * 130);
|
||||
y2 = (int)(Math.Sin(tilt.X) * 130);
|
||||
}
|
||||
else
|
||||
{
|
||||
y3 = (int)(Math.Cos(tilt.X) * 130);
|
||||
}
|
||||
|
||||
PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
|
||||
PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
|
||||
PointF[] dest = { ul, ur, dl };
|
||||
|
||||
g.DrawImage(bmp, dest);
|
||||
x1 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
x3 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
}
|
||||
catch { }
|
||||
else
|
||||
{
|
||||
x2 = (int)(Math.Cos(tilt.Y) * 130);
|
||||
}
|
||||
|
||||
if (tilt.X > 0)
|
||||
{
|
||||
y1 = (int)(Math.Sin(tilt.X) * 130);
|
||||
y2 = (int)(Math.Sin(tilt.X) * 130);
|
||||
}
|
||||
else
|
||||
{
|
||||
y3 = (int)(Math.Cos(tilt.X) * 130);
|
||||
}
|
||||
|
||||
PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
|
||||
PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
|
||||
PointF[] dest = { ul, ur, dl };
|
||||
|
||||
g.DrawImage(bmp, dest);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1,39 +0,0 @@
|
||||
using System.Numerics;
|
||||
|
||||
namespace VisualData
|
||||
{
|
||||
public struct VisualHead
|
||||
{
|
||||
public enum VisualType : int { None = 0, Drone = 1 } // Тип объекта
|
||||
|
||||
public int Size; // Размер данных этой структуры в байтах (проверка для соответствия передачи структуры)
|
||||
|
||||
public VisualType Type; // Тип передоваемого объекта
|
||||
}
|
||||
|
||||
public struct VisualDrone
|
||||
{
|
||||
public VisualHead Head;
|
||||
public enum DroneState : int { Dead = 0, Disabled = 1, Waiting = 2, Active = 3 } // Переключения типа 3D модели
|
||||
|
||||
public int Count; // Всего дронов на полигоне
|
||||
public int Index; // Номер дрона
|
||||
|
||||
public int ID; // Идентификатор (для привязки камеры)
|
||||
|
||||
public struct ARGB { public byte A, R, G, B; }
|
||||
public struct Quat { public float X, Y, Z, W; }
|
||||
public struct Vect3 { public float X, Y, Z; }
|
||||
|
||||
public ARGB Color; // Цвет корпуса
|
||||
public Quat Rotate; // Кватернион вращения
|
||||
public Vect3 Position; // Координаты в пространстве
|
||||
|
||||
public float Scale; // Масштаб модельки (1=оригинальный)
|
||||
|
||||
public DroneState State; // Тип прорисовываемой модели
|
||||
|
||||
public float Power; // Скорость всех двигателей
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user