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

Compare commits

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

8 Commits
e0af20b46d ... Dana

Author SHA1 Message Date
Dana Markova
06d621288c pulled from main 2025-04-14 10:52:29 +03:00
Sergey Sklyarov
bac52d315b Update 2025-04-11 21:59:06 +03:00
Sergey Sklyarov
64c0637e6a cpp 2025-04-11 14:56:04 +03:00
Sergey Sklyarov
6aeb99968e update 2025-04-06 20:58:15 +03:00
Sergey Sklyarov
21b60b7f4c update 2025-04-06 20:56:02 +03:00
Sergey Sklyarov
da7f5a8404 update 2025-04-06 17:17:15 +03:00
sasklyarov
fd2318cbf6 Удалить www.txt 2025-04-04 11:35:28 +00:00
Dana Markova
28b33ed671 test 2025-03-31 11:31:28 +03:00
32 changed files with 2543 additions and 104 deletions
Expand all files Collapse all files

3
.gitignore vendored
View File

@ -1,3 +1,4 @@
.vs/
obj/
bin/
bin/
x64/

260
DroneClient/Drone.cs
View File

@ -1,21 +1,21 @@
using System.Runtime.InteropServices;
using System.Diagnostics.Metrics;
using System.Drawing;
using System.Runtime.InteropServices;
namespace DroneClient
{
internal class Drone
{
public struct DataOut
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public float PosX, PosY;
public float LaserRange;
}
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
}
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public uint TimeAcc, TimeGyr;
public float PosX, PosY;
public float LaserRange;
public uint TimeRange;
public float MotorUL, MotorUR, MotorDL, MotorDR;
public static byte[] getBytes(object data)
{
@ -58,5 +58,241 @@ namespace DroneClient
return mem;
}
private byte[] SendDataMotor4()
{
DroneData.DataMotor4 mot4 = new DroneData.DataMotor4();
mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4));
mot4.Head.Mode = DroneData.DataMode.Response;
mot4.Head.Type = DroneData.DataType.DataMotor4;
mot4.UL = MotorUL;
mot4.UR = MotorUR;
mot4.DL = MotorDL;
mot4.DR = MotorDR;
return getBytes(mot4);
}
private byte[]? RecvDataAcc(byte[] data)
{
DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc= imu.Time;
return new byte[0];
}
private byte[]? RecvDataGyr(byte[] data)
{
DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
return new byte[0];
}
private byte[]? RecvDataRange(byte[] data)
{
DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
return new byte[0];
}
private byte[]? RecvDataLocal(byte[] data)
{
DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));
PosX = pos.Local.X; PosY = pos.Local.Y;
return new byte[0];
}
private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
switch (head.Type)
{
case DroneData.DataType.DataAcc:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataAcc(body);
}
}
case DroneData.DataType.DataGyr:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataGyr(body);
}
}
case DroneData.DataType.DataRange:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataRange(body);
}
}
case DroneData.DataType.DataLocal:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataLocal(body);
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataMotor4();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
}
return zero;
}
private const int DroneStreamCount = 512;
private byte[] DroneStreamData = new byte[DroneStreamCount];
private int DroneStreamIndex = 0;
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
public List<byte[]?>? DataStream(byte[]? data, int size)
{
List<byte[]?> ret = new List<byte[]?>();
if (data == null) return ret; // Последовательность не сформирована, ждать данных
if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец)
Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
DroneStreamIndex += size;
while (true)
{
if (DroneStreamHead.Size == 0) // Заголовок ещё не получен
{
if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать
DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
}
if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный
byte[] body = new byte[DroneStreamHead.Size];
Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);
int shift = DroneStreamHead.Size;
DroneStreamIndex -= shift;
Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные
DroneStreamHead.Size = 0; // Отменяем прошлый заголовок
ret.Add(ClientRequestResponse(DroneStreamHead, body));
}
return ret;
}
public byte[] SendReqest()
{
DroneData.DataHead acc = new DroneData.DataHead();
acc.Size = DroneData.DataHead.StrLen;
acc.Mode = DroneData.DataMode.Request;
acc.Type = DroneData.DataType.DataAcc;
DroneData.DataHead gyr = new DroneData.DataHead();
gyr.Size = DroneData.DataHead.StrLen;
gyr.Mode = DroneData.DataMode.Request;
gyr.Type = DroneData.DataType.DataGyr;
DroneData.DataHead range = new DroneData.DataHead();
range.Size = DroneData.DataHead.StrLen;
range.Mode = DroneData.DataMode.Request;
range.Type = DroneData.DataType.DataRange;
DroneData.DataHead local = new DroneData.DataHead();
local.Size = DroneData.DataHead.StrLen;
local.Mode = DroneData.DataMode.Request;
local.Type = DroneData.DataType.DataLocal;
List<byte[]> list = new List<byte[]>();
list.Add(getBytes(acc));
list.Add(getBytes(gyr));
list.Add(getBytes(range));
list.Add(getBytes(local));
list.Add(SendDataMotor4());
int count = 0;
foreach (byte[] d in list) count += d.Length;
byte[] send = new byte[count];
count = 0;
foreach (byte[] d in list)
{
d.CopyTo(send, count);
count += d.Length;
}
return send;
}
}
}

100
DroneClient/DroneData.cs Normal file
View File

@ -0,0 +1,100 @@
using System.Runtime.InteropServices;
namespace DroneData
{
public enum DataMode : ushort
{
None = 0, Response = 1, Request = 2
};
public enum DataType : ushort
{
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
};
public struct DataHead
{
public int Size;
public DataMode Mode;
public DataType Type;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct DataAcc
{
public DataHead Head;
public XYZ Acc;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
}
public struct DataGyr
{
public DataHead Head;
public XYZ Gyr;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
public struct DataMotor4
{
public DataHead Head;
public float UL, UR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
}
public struct DataMotor6
{
public DataHead Head;
public float UL, UR, LL, RR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
}
}

43
DroneClient/FormMain.Designer.cs generated
View File

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

87
DroneClient/FormMain.cs
View File

@ -21,34 +21,37 @@ namespace DroneSimulator
if (!data.Connect)
{
Invoke((MethodInvoker)delegate
try
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
MessageBox.Show("Connection closed");
});
Invoke((MethodInvoker)delegate
{
button_Connect.Text = "Connect";
button_Connect.BackColor = Color.Transparent;
MessageBox.Show("Connection closed");
});
}
catch { }
return;
}
byte[] send = Drone.getBytes(sendDrone);
data.Server.Send(send);
}
Drone.DataIn sendDrone;
Drone.DataOut recvDrone;
Drone dataDrone = new Drone();
private void ReceiveCallback(object o)
{
ReceiveData data = (ReceiveData)o;
recvDrone = (Drone.DataOut)Drone.fromBytes(data.Buffer, typeof(Drone.DataOut));
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
byte[] send = Drone.getBytes(sendDrone);
try { data.Server.Send(send); }
if (send == null) return;
try
{
foreach (byte[]? b in send)
{
if (b != null) data.Server?.Send(b);
}
}
catch { }
}
@ -88,17 +91,25 @@ namespace DroneSimulator
private void timer_Test_Tick(object sender, EventArgs e)
{
label_Acc_X.Text = recvDrone.AccX.ToString();
label_Acc_Y.Text = recvDrone.AccY.ToString();
label_Acc_Z.Text = recvDrone.AccZ.ToString();
label_Acc_X.Text = dataDrone.AccX.ToString();
label_Acc_Y.Text = dataDrone.AccY.ToString();
label_Acc_Z.Text = dataDrone.AccZ.ToString();
label_Gyr_X.Text = recvDrone.GyrX.ToString();
label_Gyr_Y.Text = recvDrone.GyrY.ToString();
label_Gyr_Z.Text = recvDrone.GyrZ.ToString();
label_time_acc.Text = dataDrone.TimeAcc.ToString();
label_Pos_X.Text = recvDrone.PosX.ToString();
label_Pos_Y.Text = recvDrone.PosY.ToString();
label_Pos_L.Text = recvDrone.LaserRange.ToString();
label_Gyr_X.Text = dataDrone.GyrX.ToString();
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
label_Pos_X.Text = dataDrone.PosX.ToString();
label_Pos_Y.Text = dataDrone.PosY.ToString();
label_Pos_L.Text = dataDrone.LaserRange.ToString();
label_time_range.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest());
}
private void trackBar_Power_Scroll(object sender, EventArgs e)
@ -107,7 +118,7 @@ namespace DroneSimulator
label_Pow.Text = pow.ToString();
sendDrone.MotorUL = sendDrone.MotorUR = sendDrone.MotorDL = sendDrone.MotorDR = pow;
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
}
private void button_UU_MouseDown(object sender, MouseEventArgs e)
@ -116,35 +127,35 @@ namespace DroneSimulator
if (sender == button_UU)
{
sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR += pow;
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
}
if (sender == button_DD)
{
sendDrone.MotorUL += pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow;
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
}
if (sender == button_LL)
{
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
if (sender == button_RR)
{
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_ML)
{
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
}
if (sender == button_MR)
{
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
}
}

9
DroneClient/NetClient.cs
View File

@ -41,9 +41,7 @@ namespace DroneSimulator
{
if (Connected)
{
try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
ServerSocket?.Close();
Connected = false;
Close();
return ClientState.Stop;
}
@ -99,5 +97,10 @@ namespace DroneSimulator
try { ServerSocket?.BeginReceive(cd.Buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), cd); }
catch { }
}
public void SendData(byte[] data)
{
try { ServerSocket?.Send(data); } catch { }
}
}
}

106
DroneClientCpp/Client.cpp Normal file
View File

@ -0,0 +1,106 @@
#pragma once
#include "Client.h"
Client::Client(const char* ip, int port) : running(true), Connection(INVALID_SOCKET)
{
if (!ConnectToServer(ip, port))
{
std::cerr << "Failed to connect to server!" << std::endl;
}
}
Client::~Client()
{
Stop();
CloseConnection();
}
bool Client::ConnectToServer(const char* ip, int port)
{
WSAData wsaData;
WORD DLLVersion = MAKEWORD(2, 2);
if (WSAStartup(DLLVersion, &wsaData) != 0)
{
std::cerr << "Error: WSAStartup failed" << std::endl;
return false;
}
SOCKADDR_IN addr;
inet_pton(AF_INET, ip, &addr.sin_addr);
addr.sin_port = htons(port);
addr.sin_family = AF_INET;
Connection = socket(AF_INET, SOCK_STREAM, 0);
if (Connection == INVALID_SOCKET)
{
std::cerr << "Error: Failed to create socket" << std::endl;
WSACleanup();
return false;
}
if (connect(Connection, (SOCKADDR*)&addr, sizeof(addr)) != 0)
{
std::cerr << "Error: Failed to connect to server" << std::endl;
closesocket(Connection);
WSACleanup();
return false;
}
return true;
}
void Client::CloseConnection()
{
if (Connection != INVALID_SOCKET)
{
closesocket(Connection);
WSACleanup();
Connection = INVALID_SOCKET;
}
}
void Client::ReceiveHandler()
{
while (running)
{
DataIn tempData;
int result = recv(Connection, (char*)&tempData, sizeof(tempData), 0);
if (result <= 0)
{
std::cerr << "Error: Connection lost (recv failed)" << std::endl;
running = false;
break;
}
std::lock_guard<std::mutex> lock(dataMutex);
dataIn = tempData;
}
}
void Client::SendHandler()
{
while (running)
{
{
std::lock_guard<std::mutex> lock(dataMutex);
send(Connection, (char*)&dataOut, sizeof(dataOut), 0);
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void Client::Start()
{
recvThread = std::thread(&Client::ReceiveHandler, this);
sendThread = std::thread(&Client::SendHandler, this);
}
void Client::Stop()
{
running = false;
if (recvThread.joinable()) recvThread.join();
if (sendThread.joinable()) sendThread.join();
}

44
DroneClientCpp/Client.h Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#include <iostream>
#include <WinSock2.h>
#include <WS2tcpip.h>
#include <thread>
#include <atomic>
#include <mutex>
#pragma comment(lib, "Ws2_32.lib")
class Client
{
public:
Client(const char* ip, int port);
~Client();
void Start(); // Запуск потоков
void Stop(); // Остановка потоков
struct DataIn
{
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
float PosX, PosY, LaserRange;
} dataIn;
struct DataOut
{
float MotorUL, MotorUR, MotorDL, MotorDR;
} dataOut;
bool ConnectToServer(const char* ip, int port);
void CloseConnection();
private:
std::atomic<bool> running;
std::mutex dataMutex;
SOCKET Connection;
std::thread recvThread;
std::thread sendThread;
void ReceiveHandler();
void SendHandler();
};

255
DroneClientCpp/Drone.cpp Normal file
View File

@ -0,0 +1,255 @@
#include "Drone.h"
namespace DroneClient {
// Реализация статического метода GetBytes
array<Byte>^ Drone::GetBytes(Object^ data)
{
int size = Marshal::SizeOf(data);
array<Byte>^ arr = gcnew array<Byte>(size);
IntPtr ptr = IntPtr::Zero;
try
{
ptr = Marshal::AllocHGlobal(size);
Marshal::StructureToPtr(data, ptr, true);
Marshal::Copy(ptr, arr, 0, size);
}
finally
{
Marshal::FreeHGlobal(ptr);
}
return arr;
}
// Реализация статического метода FromBytes
Object^ Drone::FromBytes(array<Byte>^ arr, Type^ type)
{
Object^ mem = gcnew Object();
int size = Marshal::SizeOf(type);
IntPtr ptr = IntPtr::Zero;
try
{
ptr = Marshal::AllocHGlobal(size);
Marshal::Copy(arr, 0, ptr, size);
mem = Marshal::PtrToStructure(ptr, type);
}
finally
{
Marshal::FreeHGlobal(ptr);
}
return mem;
}
// Реализация приватного метода 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;
mot4.UL = MotorUL;
mot4.UR = MotorUR;
mot4.DL = MotorDL;
mot4.DR = MotorDR;
return GetBytes(mot4);
}
array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
{
DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
TimeAcc = imu.Time;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
{
DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
TimeGyr = imu.Time;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
{
DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);
LaserRange = pos.LiDAR;
TimeRange = pos.Time;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
{
DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);
PosX = pos.Local.X; PosY = pos.Local.Y;
return gcnew array<Byte>(0);
}
array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
{
array<Byte>^ zero = gcnew array<Byte>(0);
switch (head.Type)
{
case DroneData::DataType::DataAcc:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataAcc(body); // Пришли данные
}
case DroneData::DataType::DataGyr:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataGyr(body); // Пришли данные
}
case DroneData::DataType::DataRange:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataRange(body); // Пришли данные
}
case DroneData::DataType::DataLocal:
if (head.Mode == DroneData::DataMode::Request)
{
return zero; // Запрос данных (не реализовано)
}
else
{
return RecvDataLocal(body); // Пришли данные
}
case DroneData::DataType::DataMotor4:
if (head.Mode == DroneData::DataMode::Request)
{
return SendDataMotor4(); // Запрос данных
}
else
{
return zero; // Пришли данные (не реализовано)
}
}
return zero;
}
// Реализация конструктора
Drone::Drone()
{
DroneStreamData = gcnew array<Byte>(DroneStreamCount); // Инициализация массива
DroneStreamIndex = 0;
DroneStreamHead.Mode = DroneData::DataMode::None;
DroneStreamHead.Size = 0;
DroneStreamHead.Type = DroneData::DataType::None;
}
// Реализация метода DataStream
List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
{
System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();
if (data == nullptr) return ret; // Последовательность не сформирована
if (size + DroneStreamIndex > DroneStreamCount) return nullptr; // Ошибка переполнения
Array::Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
DroneStreamIndex += size;
while (true)
{
if (DroneStreamHead.Size == 0) // Заголовок еще не получен
{
if (DroneStreamIndex < DroneData::DataHead::StrLen) return ret;
DroneStreamHead = (DroneData::DataHead)FromBytes(DroneStreamData, DroneData::DataHead::typeid);
}
if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет еще не полный
array<Byte>^ body = gcnew array<Byte>(DroneStreamHead.Size);
Array::Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);
int shift = DroneStreamHead.Size;
DroneStreamIndex -= shift;
Array::Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвиг массива
DroneStreamHead.Size = 0; // Сброс заголовка
ret->Add(ClientRequestResponse(DroneStreamHead, body));
}
return ret;
}
// Реализация метода SendRequest
array<Byte>^ Drone::SendRequest()
{
DroneData::DataHead^ acc = gcnew DroneData::DataHead();
acc->Size = DroneData::DataHead::StrLen;
acc->Mode = DroneData::DataMode::Request;
acc->Type = DroneData::DataType::DataAcc;
DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
gyr->Size = DroneData::DataHead::StrLen;
gyr->Mode = DroneData::DataMode::Request;
gyr->Type = DroneData::DataType::DataGyr;
DroneData::DataHead^ range = gcnew DroneData::DataHead();
range->Size = DroneData::DataHead::StrLen;
range->Mode = DroneData::DataMode::Request;
range->Type = DroneData::DataType::DataRange;
DroneData::DataHead^ local = gcnew DroneData::DataHead();
local->Size = DroneData::DataHead::StrLen;
local->Mode = DroneData::DataMode::Request;
local->Type = DroneData::DataType::DataLocal;
List<array<byte>^>^ list = gcnew List<array<byte>^>();
list->Add(GetBytes(acc));
list->Add(GetBytes(gyr));
list->Add(GetBytes(range));
list->Add(GetBytes(local));
list->Add(SendDataMotor4());
int count = 0;
for each(array<byte>^ d in list) count += d->Length;
array<byte>^ send = gcnew array<byte>(count);
count = 0;
for each (array<byte> ^ d in list)
{
d->CopyTo(send, count);
count += d->Length;
}
return send;
}
}

51
DroneClientCpp/Drone.h Normal file
View File

@ -0,0 +1,51 @@
#pragma once
#include "DroneData.h"
#include <Windows.h>
#include <vcclr.h>
#using <System.dll>
#using <mscorlib.dll>
using namespace System;
using namespace System::Collections::Generic;
using namespace System::Runtime::InteropServices;
namespace DroneClient {
public ref class Drone
{
public:
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
unsigned int TimeAcc, TimeGyr;
float PosX, PosY;
float LaserRange;
unsigned int TimeRange;
float MotorUL, MotorUR, MotorDL, MotorDR;
static array<Byte>^ GetBytes(Object^ data);
static Object^ FromBytes(array<Byte>^ arr, Type^ type);
private:
array<Byte>^ SendDataMotor4();
array<Byte>^ RecvDataAcc(array<Byte>^ data);
array<Byte>^ RecvDataGyr(array<Byte>^ data);
array<Byte>^ RecvDataRange(array<Byte>^ data);
array<Byte>^ RecvDataLocal(array<Byte>^ data);
array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
literal int DroneStreamCount = 512;
array<Byte>^ DroneStreamData;
int DroneStreamIndex;
DroneData::DataHead DroneStreamHead;
public:
Drone(); // Конструктор для инициализации
System::Collections::Generic::List<array<Byte>^>^ DataStream(array<Byte>^ data, int size);
array<Byte>^ SendRequest();
};
}

65
DroneClientCpp/DroneClient.h Normal file
View File

@ -0,0 +1,65 @@
#pragma once
#include <Windows.h>
#include <vcclr.h>
#using <System.dll>
#using <mscorlib.dll>
using namespace System;
using namespace System::Runtime::InteropServices;
namespace DroneClient {
public ref class Drone
{
public:
value struct DataOut
{
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
float PosX, PosY;
float LaserRange;
};
value struct DataIn
{
float MotorUL, MotorUR, MotorDL, MotorDR;
};
static array<Byte>^ GetBytes(Object^ data)
{
int size = Marshal::SizeOf(data);
array<Byte>^ arr = gcnew array<Byte>(size);
IntPtr ptr = IntPtr::Zero;
try
{
ptr = Marshal::AllocHGlobal(size);
Marshal::StructureToPtr(data, ptr, true);
Marshal::Copy(ptr, arr, 0, size);
}
finally
{
Marshal::FreeHGlobal(ptr);
}
return arr;
}
static Object^ FromBytes(array<Byte>^ arr, Type^ type)
{
int size = Marshal::SizeOf(type);
IntPtr ptr = IntPtr::Zero;
try
{
ptr = Marshal::AllocHGlobal(size);
Marshal::Copy(arr, 0, ptr, size);
return Marshal::PtrToStructure(ptr, type);
}
finally
{
Marshal::FreeHGlobal(ptr);
}
}
};
}

28
DroneClientCpp/DroneClientCpp.sln Normal file
View File

@ -0,0 +1,28 @@

Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.12.35527.113
MinimumVisualStudioVersion = 10.0.40219.1
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DroneClientCpp", "DroneClientCpp.vcxproj", "{690C304C-A70B-4B0F-BF61-8C51290BF444}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|x64 = Debug|x64
Debug|x86 = Debug|x86
Release|x64 = Release|x64
Release|x86 = Release|x86
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Debug|x64.ActiveCfg = Debug|x64
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Debug|x64.Build.0 = Debug|x64
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Debug|x86.ActiveCfg = Debug|Win32
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Debug|x86.Build.0 = Debug|Win32
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Release|x64.ActiveCfg = Release|x64
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Release|x64.Build.0 = Release|x64
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Release|x86.ActiveCfg = Release|Win32
{690C304C-A70B-4B0F-BF61-8C51290BF444}.Release|x86.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
EndGlobal

141
DroneClientCpp/DroneClientCpp.vcxproj Normal file
View File

@ -0,0 +1,141 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<ItemGroup Label="ProjectConfigurations">
<ProjectConfiguration Include="Debug|Win32">
<Configuration>Debug</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|Win32">
<Configuration>Release</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Debug|x64">
<Configuration>Debug</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|x64">
<Configuration>Release</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
</ItemGroup>
<PropertyGroup Label="Globals">
<VCProjectVersion>17.0</VCProjectVersion>
<ProjectGuid>{690C304C-A70B-4B0F-BF61-8C51290BF444}</ProjectGuid>
<TargetFrameworkVersion>v4.7.2</TargetFrameworkVersion>
<Keyword>ManagedCProj</Keyword>
<RootNamespace>DroneClientCpp</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CLRSupport>true</CLRSupport>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CLRSupport>true</CLRSupport>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CLRSupport>true</CLRSupport>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CLRSupport>true</CLRSupport>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings">
</ImportGroup>
<ImportGroup Label="Shared">
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<PropertyGroup Label="UserMacros" />
<PropertyGroup />
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PreprocessorDefinitions>_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions>
</ClCompile>
<Link>
<AdditionalDependencies />
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PreprocessorDefinitions>WIN32;_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions>
</ClCompile>
<Link>
<AdditionalDependencies />
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PreprocessorDefinitions>WIN32;NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions>
</ClCompile>
<Link>
<AdditionalDependencies />
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PreprocessorDefinitions>NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions>
</ClCompile>
<Link>
<AdditionalDependencies />
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<Reference Include="System" />
<Reference Include="System.Data" />
<Reference Include="System.Drawing" />
<Reference Include="System.Windows.Forms" />
<Reference Include="System.Xml" />
</ItemGroup>
<ItemGroup>
<ClCompile Include="Drone.cpp" />
<ClCompile Include="FormMain.cpp" />
<ClCompile Include="NetClient.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="Drone.h" />
<ClInclude Include="DroneData.h" />
<ClInclude Include="FormMain.h">
<FileType>CppForm</FileType>
</ClInclude>
<ClInclude Include="NetClient.h" />
</ItemGroup>
<ItemGroup>
<EmbeddedResource Include="FormMain.resx">
<DependentUpon>FormMain.h</DependentUpon>
</EmbeddedResource>
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

42
DroneClientCpp/DroneClientCpp.vcxproj.filters Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<ItemGroup>
<Filter Include="Исходные файлы">
<UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
<Extensions>cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
</Filter>
<Filter Include="Файлы заголовков">
<UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
<Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions>
</Filter>
<Filter Include="Файлы ресурсов">
<UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
<Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
</Filter>
</ItemGroup>
<ItemGroup>
<ClCompile Include="FormMain.cpp">
<Filter>Исходные файлы</Filter>
</ClCompile>
<ClCompile Include="NetClient.cpp">
<Filter>Исходные файлы</Filter>
</ClCompile>
<ClCompile Include="Drone.cpp">
<Filter>Исходные файлы</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="FormMain.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="NetClient.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="DroneData.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="Drone.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
</ItemGroup>
</Project>

4
DroneClientCpp/DroneClientCpp.vcxproj.user Normal file
View File

@ -0,0 +1,4 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup />
</Project>

117
DroneClientCpp/DroneData.h Normal file
View File

@ -0,0 +1,117 @@
#pragma once
#include <Windows.h>
#include <vcclr.h>
using namespace System;
using namespace System::Runtime::InteropServices;
namespace DroneData {
public enum class DataMode : unsigned short
{
None = 0, Response = 1, Request = 2
};
public enum class DataType : unsigned short
{
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
};
public value struct DataHead
{
public:
long Size;
DataMode Mode;
DataType Type;
unsigned long Time;
static const int StrLen = sizeof(DataHead);
};
public value struct XYZ
{
public:
float X, Y, Z;
};
public value struct DataAcc
{
public:
DataHead Head;
XYZ Acc;
unsigned long Time;
static const int StrLen = sizeof(DataAcc);
};
public value struct DataGyr
{
public:
DataHead Head;
XYZ Gyr;
unsigned long Time;
static const int StrLen = sizeof(DataGyr);
};
public value struct DataMag
{
public:
DataHead Head;
XYZ Mag;
unsigned long Time;
static const int StrLen = sizeof(DataMag);
};
public value struct DataRange
{
public:
DataHead Head;
float LiDAR;
unsigned long Time;
static const int StrLen = sizeof(DataRange);
};
public value struct DataLocal
{
public:
DataHead Head;
XYZ Local;
unsigned long Time;
static const int StrLen = sizeof(DataLocal);
};
public value struct DataMotor4
{
public:
DataHead Head;
float UL, UR, DL, DR;
static const int StrLen = sizeof(DataMotor4);
};
public value struct DataMotor6
{
public:
DataHead Head;
float UL, UR, LL, RR, DL, DR;
static const int StrLen = sizeof(DataMotor6);
};
}

12
DroneClientCpp/FormMain.cpp Normal file
View File

@ -0,0 +1,12 @@
#include "FormMain.h"
#include <Windows.h>
using namespace DroneClientCpp;
int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) {
Application::EnableVisualStyles();
Application::SetCompatibleTextRenderingDefault(false);
Application::Run(gcnew FormMain);
return 0;
}

BIN
DroneClientCpp/FormMain.h Normal file
View File

Binary file not shown.

126
DroneClientCpp/FormMain.resx Normal file
View File

@ -0,0 +1,126 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<metadata name="backgroundWorker1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
<value>17, 17</value>
</metadata>
<metadata name="timer1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
<value>310, 17</value>
</metadata>
</root>

12
DroneClientCpp/MyForm.cpp Normal file
View File

@ -0,0 +1,12 @@
#include "MyForm.h"
#include <Windows.h>
using namespace DroneClientCpp;
int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) {
Application::EnableVisualStyles();
Application::SetCompatibleTextRenderingDefault(false);
Application::Run(gcnew MyForm);
return 0;
}

BIN
DroneClientCpp/MyForm.h Normal file
View File

Binary file not shown.

120
DroneClientCpp/MyForm.resx Normal file
View File

@ -0,0 +1,120 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
</root>

105
DroneClientCpp/NetClient.cpp Normal file
View File

@ -0,0 +1,105 @@
#include "NetClient.h"
namespace DroneSimulator {
// Конструктор ConnectData
NetClient::ConnectData::ConnectData(bool connect, Socket^ server)
{
Connect = connect;
Server = server;
}
// Конструктор ReceiveData
NetClient::ReceiveData::ReceiveData(array<Byte>^ buffer, int size, Socket^ server)
{
Buffer = buffer;
Size = size;
Server = server;
}
// Конструктор NetClient
NetClient::NetClient()
{
Connected = false;
ServerSocket = nullptr;
DataServer = gcnew ServerData(); // Инициализация DataServer
}
// Реализация метода Connect
NetClient::ClientState NetClient::Connect(String^ Addr, int Port, ClientCallback^ Connection, ClientCallback^ Receive)
{
if (Connected)
{
Close();
return ClientState::Stop;
}
ConnectionCallback = Connection;
ReceiveCallback = Receive;
IPEndPoint^ ep = gcnew IPEndPoint(IPAddress::Parse(Addr), Port);
ServerSocket = gcnew Socket(AddressFamily::InterNetwork, SocketType::Stream, ProtocolType::Tcp);
try { if (ServerSocket) ServerSocket->Connect(ep); }
catch (...) { ServerSocket->Close(); return ClientState::Error; }
Connected = true;
ConnectionCallback(gcnew ConnectData(true, ServerSocket));
ReceiveData^ receiveData = gcnew ReceiveData(DataServer->buffer, ServerData::size, ServerSocket);
try { if (ServerSocket) ServerSocket->BeginReceive(DataServer->buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), receiveData); }
catch (...) {}
return ClientState::Connected;
}
// Реализация метода Close
void NetClient::Close()
{
try { if(ServerSocket) ServerSocket->Shutdown(SocketShutdown::Both); }
catch (...) {}
if(ServerSocket) ServerSocket->Close();
Connected = false;
}
// Реализация метода Send
void NetClient::Send(array<Byte>^ data)
{
if (ServerSocket != nullptr && Connected)
{
try { if (ServerSocket) ServerSocket->Send(data); }
catch (...) {}
}
}
// Реализация метода ReadCallback
void NetClient::ReadCallback(IAsyncResult^ ar)
{
ReceiveData^ cd = (ReceiveData^)ar->AsyncState;
if (cd == nullptr) return;
int bytes = 0;
try { bytes = ServerSocket->EndReceive(ar); }
catch (...) {}
if (bytes == 0)
{
if (ServerSocket) ServerSocket->Close();
Connected = false;
if (ServerSocket != nullptr)
{
ConnectionCallback(gcnew ConnectData(false, nullptr));
}
return;
}
ReceiveCallback(gcnew ReceiveData(cd->Buffer, bytes, ServerSocket));
try { if (ServerSocket) ServerSocket->BeginReceive(cd->Buffer, 0, ServerData::size, SocketFlags::None, gcnew AsyncCallback(this, &NetClient::ReadCallback), cd); }
catch (...) {}
}
}

68
DroneClientCpp/NetClient.h Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#include <Windows.h>
#include <vcclr.h>
#using <System.dll>
#using <System.Net.dll>
using namespace System;
using namespace System::Net;
using namespace System::Net::Sockets;
namespace DroneSimulator {
public ref class NetClient
{
public:
ref class ConnectData
{
public:
bool Connect;
Socket^ Server;
ConnectData(bool connect, Socket^ server);
};
ref class ReceiveData
{
public:
array<Byte>^ Buffer;
int Size;
Socket^ Server;
ReceiveData(array<Byte>^ buffer, int size, Socket^ server);
};
private:
ref class ServerData
{
public:
literal int size = 1024;
array<Byte>^ buffer = gcnew array<Byte>(size);
};
bool Connected;
Socket^ ServerSocket;
ServerData^ DataServer;
public:
delegate void ClientCallback(Object^ o);
private:
ClientCallback^ ConnectionCallback;
ClientCallback^ ReceiveCallback;
public:
NetClient(); // Добавлен конструктор
enum class ClientState { Error, Connected, Stop };
ClientState Connect(String^ Addr, int Port, ClientCallback^ Connection, ClientCallback^ Receive);
void Close();
void Send(array<Byte>^ data);
private:
void ReadCallback(IAsyncResult^ ar);
};
}

11
DroneClientCpp/Source.cpp Normal file
View File

@ -0,0 +1,11 @@
#include "MyForm.h"
using namespace MyWinFormsApp; // ???????????? ???? ?????? ???????
[STAThread]
int main(array<String^>^ args) {
Application::EnableVisualStyles(); // ???????? ??????????? ????? ????????? ??????????
Application::SetCompatibleTextRenderingDefault(false); // ????????? ?????????? ??????
Application::Run(gcnew MyForm()); // ?????? ?????
return 0;
}

282
DroneSimulator/Drone.cs
View File

@ -1,5 +1,7 @@
using System.Numerics;
using System.CodeDom;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
namespace DroneSimulator
{
@ -12,13 +14,15 @@ namespace DroneSimulator
public const float Dynamic = 10; // Динамика вращения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион
public float Power = 0; // Тяга всех двигателей
public float MaxPower; // Тяга всех двигателей
public float Power = 0; // Тяга всех двигателей (0-1)
public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
public float LaserRange; // Имитация: Дальномер
private uint DataTimer;
private const float Gravity = 9.8f;
private const float TO_GRAD = 180 / MathF.PI;
@ -70,33 +74,6 @@ namespace DroneSimulator
return mem;
}
public struct DataOut
{
public float AccX, AccY, AccZ;
public float GyrX, GyrY, GyrZ;
public float PosX, PosY;
public float LaserRange;
}
public DataOut GetDataOut()
{
DataOut data = new DataOut();
data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
data.LaserRange = LaserRange;
return data;
}
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
}
public struct DataVisual
{
public int ID; // Идентификатор
@ -113,9 +90,7 @@ namespace DroneSimulator
{
while (DroneThread != null)
{
float time = Environment.TickCount - Timer;
Timer = Environment.TickCount;
Action(time / 1000);
Action(Environment.TickCount);
Thread.Sleep(1);
}
}
@ -191,8 +166,11 @@ namespace DroneSimulator
return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
}
public void Action(float time)
public void Action(int tick)
{
float time = (tick - Timer) / 1000.0f;
Timer = tick;
if (!Active) return;
float flow = Power;
@ -206,7 +184,7 @@ namespace DroneSimulator
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
PosXYZ += SpdXYZ * time;
@ -261,6 +239,8 @@ namespace DroneSimulator
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
else LaserRange = float.MaxValue;
}
DataTimer = (uint)tick;
}
private float Range(float pow)
@ -281,5 +261,237 @@ namespace DroneSimulator
AccPRY.X = ((ul + ur) - (dl + dr));
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
}
private void RecvDataMotor4(byte[] data)
{
DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
/* обработка */
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
}
private byte[] SendDataAcc()
{
DroneData.DataAcc acc = new DroneData.DataAcc();
acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
acc.Head.Mode = DroneData.DataMode.Response;
acc.Head.Type = DroneData.DataType.DataAcc;
acc.Head.Time = (uint)Environment.TickCount;
acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
acc.Time = DataTimer;
return getBytes(acc);
}
private byte[] SendDataGyr()
{
DroneData.DataGyr gyr = new DroneData.DataGyr();
gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
gyr.Head.Mode = DroneData.DataMode.Response;
gyr.Head.Type = DroneData.DataType.DataGyr;
gyr.Head.Time = (uint)Environment.TickCount;
gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
gyr.Time = DataTimer;
return getBytes(gyr);
}
private byte[] SendDataMag()
{
DroneData.DataMag mag = new DroneData.DataMag();
mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
mag.Head.Mode = DroneData.DataMode.Response;
mag.Head.Type = DroneData.DataType.DataMag;
mag.Head.Time = (uint)Environment.TickCount;
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
mag.Time = DataTimer;
return getBytes(mag);
}
private byte[] SendDataRange()
{
DroneData.DataRange range = new DroneData.DataRange();
range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
range.Head.Mode = DroneData.DataMode.Response;
range.Head.Type = DroneData.DataType.DataRange;
range.Head.Time = (uint)Environment.TickCount;
range.LiDAR = LaserRange;
range.Time = DataTimer;
return getBytes(range);
}
private byte[] SendDataLocal()
{
DroneData.DataLocal local = new DroneData.DataLocal();
local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
local.Head.Mode = DroneData.DataMode.Response;
local.Head.Type = DroneData.DataType.DataLocal;
local.Head.Time = (uint)Environment.TickCount;
local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
local.Time = DataTimer;
return getBytes(local);
}
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
switch (head.Type)
{
case DroneData.DataType.DataAcc:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataAcc();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataGyr:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataGyr();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataMag:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataMag();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataRange:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataRange();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataLocal:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataLocal();
}
else
{
// Пришли данные
// ... //
//
return zero;
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
RecvDataMotor4(body);
return zero;
}
}
}
return zero;
}
private const int DroneStreamCount = 512;
private byte[] DroneStreamData = new byte[DroneStreamCount];
private int DroneStreamIndex = 0;
private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };
public List<byte[]?>? DataStream(byte[]? data, int size)
{
List<byte[]?> ret = new List<byte[]?>();
if (data == null) return ret; // Последовательность не сформирована, ждать данных
if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец)
Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
DroneStreamIndex += size;
while (true)
{
if (DroneStreamHead.Size == 0) // Заголовок ещё не получен
{
if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать
DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
}
if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный
byte[] body = new byte[DroneStreamHead.Size];
Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);
int shift = DroneStreamHead.Size;
DroneStreamIndex -= shift;
Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные
DroneStreamHead.Size = 0; // Отменяем прошлый заголовок
ret.Add(ServerRequestResponse(DroneStreamHead, body));
}
return ret;
}
}
}

100
DroneSimulator/DroneData.cs Normal file
View File

@ -0,0 +1,100 @@
using System.Runtime.InteropServices;
namespace DroneData
{
public enum DataMode : ushort
{
None = 0, Response = 1, Request = 2
};
public enum DataType : ushort
{
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
};
public struct DataHead
{
public int Size;
public DataMode Mode;
public DataType Type;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
public struct XYZ { public float X, Y, Z; }
public struct DataAcc
{
public DataHead Head;
public XYZ Acc;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
}
public struct DataGyr
{
public DataHead Head;
public XYZ Gyr;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
public struct DataMag
{
public DataHead Head;
public XYZ Mag;
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
public struct DataMotor4
{
public DataHead Head;
public float UL, UR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor4));
}
public struct DataMotor6
{
public DataHead Head;
public float UL, UR, LL, RR, DL, DR;
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
}
}

425
DroneSimulator/FormMain.Designer.cs generated
View File

@ -61,6 +61,37 @@
comboBox_Drone_Rotor = new ComboBox();
comboBox_Drone = new ComboBox();
timer_Test = new System.Windows.Forms.Timer(components);
textBox_GPS_Lat = new TextBox();
label2 = new Label();
groupBox_GPS = new GroupBox();
textBox_GPS_Lon = new TextBox();
label4 = new Label();
numericUpDown_GPS_Freq = new NumericUpDown();
label_GPS_Frequency = new Label();
label6 = new Label();
groupBox_Barometer = new GroupBox();
numericUpDown_Bar_Freq = new NumericUpDown();
label5 = new Label();
label7 = new Label();
label8 = new Label();
numericUpDown_Bar_Accur = new NumericUpDown();
label9 = new Label();
label10 = new Label();
textBox_GPS_Accur = new NumericUpDown();
label11 = new Label();
groupBox1 = new GroupBox();
label12 = new Label();
numericUpDown_OF_Accur = new NumericUpDown();
label13 = new Label();
numericUpDown_OF_Freq = new NumericUpDown();
label14 = new Label();
label15 = new Label();
checkBox_GPS_Enable = new CheckBox();
label16 = new Label();
numericUpDown1 = new NumericUpDown();
label17 = new Label();
checkBox_OF_Enable = new CheckBox();
checkBox_Bar_Enable = new CheckBox();
menuStrip_Menu.SuspendLayout();
groupBox_Screen.SuspendLayout();
((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
@ -72,8 +103,19 @@
groupBox_Clients.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Clients_Limit).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Clients_Port).BeginInit();
tabPage_Model.SuspendLayout();
groupBox_Navi.SuspendLayout();
panel1.SuspendLayout();
groupBox_GPS.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Freq).BeginInit();
groupBox_Barometer.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Freq).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Accur).BeginInit();
((System.ComponentModel.ISupportInitialize)textBox_GPS_Accur).BeginInit();
groupBox1.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Accur).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).BeginInit();
SuspendLayout();
//
// menuStrip_Menu
@ -326,6 +368,9 @@
//
// tabPage_Model
//
tabPage_Model.Controls.Add(groupBox1);
tabPage_Model.Controls.Add(groupBox_Barometer);
tabPage_Model.Controls.Add(groupBox_GPS);
tabPage_Model.Location = new Point(4, 24);
tabPage_Model.Name = "tabPage_Model";
tabPage_Model.Padding = new Padding(3);
@ -402,6 +447,341 @@
timer_Test.Interval = 10;
timer_Test.Tick += timer_Test_Tick;
//
// textBox_GPS_Lat
//
textBox_GPS_Lat.Location = new Point(37, 22);
textBox_GPS_Lat.Name = "textBox_GPS_Lat";
textBox_GPS_Lat.Size = new Size(92, 23);
textBox_GPS_Lat.TabIndex = 0;
textBox_GPS_Lat.Text = "47.2125649";
//
// label2
//
label2.AutoSize = true;
label2.Location = new Point(5, 25);
label2.Name = "label2";
label2.Size = new Size(26, 15);
label2.TabIndex = 1;
label2.Text = "Lat:";
//
// groupBox_GPS
//
groupBox_GPS.Controls.Add(checkBox_GPS_Enable);
groupBox_GPS.Controls.Add(label10);
groupBox_GPS.Controls.Add(textBox_GPS_Accur);
groupBox_GPS.Controls.Add(label11);
groupBox_GPS.Controls.Add(numericUpDown_GPS_Freq);
groupBox_GPS.Controls.Add(label6);
groupBox_GPS.Controls.Add(label_GPS_Frequency);
groupBox_GPS.Controls.Add(label4);
groupBox_GPS.Controls.Add(textBox_GPS_Lon);
groupBox_GPS.Controls.Add(label2);
groupBox_GPS.Controls.Add(textBox_GPS_Lat);
groupBox_GPS.Dock = DockStyle.Top;
groupBox_GPS.Enabled = false;
groupBox_GPS.Location = new Point(3, 3);
groupBox_GPS.Name = "groupBox_GPS";
groupBox_GPS.Size = new Size(204, 112);
groupBox_GPS.TabIndex = 2;
groupBox_GPS.TabStop = false;
groupBox_GPS.Text = "GPS";
//
// textBox_GPS_Lon
//
textBox_GPS_Lon.Location = new Point(37, 51);
textBox_GPS_Lon.Name = "textBox_GPS_Lon";
textBox_GPS_Lon.Size = new Size(92, 23);
textBox_GPS_Lon.TabIndex = 2;
textBox_GPS_Lon.Text = "38.9160740";
//
// label4
//
label4.AutoSize = true;
label4.Location = new Point(5, 54);
label4.Name = "label4";
label4.Size = new Size(30, 15);
label4.TabIndex = 3;
label4.Text = "Lon:";
//
// numericUpDown_GPS_Freq
//
numericUpDown_GPS_Freq.Location = new Point(135, 51);
numericUpDown_GPS_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_GPS_Freq.Name = "numericUpDown_GPS_Freq";
numericUpDown_GPS_Freq.Size = new Size(40, 23);
numericUpDown_GPS_Freq.TabIndex = 4;
numericUpDown_GPS_Freq.Value = new decimal(new int[] { 10, 0, 0, 0 });
//
// label_GPS_Frequency
//
label_GPS_Frequency.AutoSize = true;
label_GPS_Frequency.Location = new Point(135, 25);
label_GPS_Frequency.Name = "label_GPS_Frequency";
label_GPS_Frequency.Size = new Size(62, 15);
label_GPS_Frequency.TabIndex = 5;
label_GPS_Frequency.Tag = "#frequency";
label_GPS_Frequency.Text = "Frequency";
//
// label6
//
label6.AutoSize = true;
label6.Location = new Point(176, 54);
label6.Name = "label6";
label6.Size = new Size(21, 15);
label6.TabIndex = 6;
label6.Text = "Hz";
//
// groupBox_Barometer
//
groupBox_Barometer.Controls.Add(checkBox_Bar_Enable);
groupBox_Barometer.Controls.Add(label9);
groupBox_Barometer.Controls.Add(numericUpDown_Bar_Accur);
groupBox_Barometer.Controls.Add(label8);
groupBox_Barometer.Controls.Add(numericUpDown_Bar_Freq);
groupBox_Barometer.Controls.Add(label5);
groupBox_Barometer.Controls.Add(label7);
groupBox_Barometer.Dock = DockStyle.Top;
groupBox_Barometer.Location = new Point(3, 115);
groupBox_Barometer.Name = "groupBox_Barometer";
groupBox_Barometer.Size = new Size(204, 88);
groupBox_Barometer.TabIndex = 3;
groupBox_Barometer.TabStop = false;
groupBox_Barometer.Tag = "#barometer";
groupBox_Barometer.Text = "Barometer";
//
// numericUpDown_Bar_Freq
//
numericUpDown_Bar_Freq.Location = new Point(69, 22);
numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Bar_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Bar_Freq.Name = "numericUpDown_Bar_Freq";
numericUpDown_Bar_Freq.Size = new Size(40, 23);
numericUpDown_Bar_Freq.TabIndex = 7;
numericUpDown_Bar_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 });
//
// label5
//
label5.AutoSize = true;
label5.Location = new Point(111, 24);
label5.Name = "label5";
label5.Size = new Size(21, 15);
label5.TabIndex = 9;
label5.Text = "Hz";
//
// label7
//
label7.AutoSize = true;
label7.Location = new Point(6, 24);
label7.Name = "label7";
label7.Size = new Size(62, 15);
label7.TabIndex = 8;
label7.Tag = "#frequency";
label7.Text = "Frequency";
//
// label8
//
label8.AutoSize = true;
label8.Location = new Point(7, 53);
label8.Name = "label8";
label8.Size = new Size(56, 15);
label8.TabIndex = 10;
label8.Tag = "#accuracy";
label8.Text = "Accuracy";
//
// numericUpDown_Bar_Accur
//
numericUpDown_Bar_Accur.DecimalPlaces = 1;
numericUpDown_Bar_Accur.Location = new Point(69, 51);
numericUpDown_Bar_Accur.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
numericUpDown_Bar_Accur.Name = "numericUpDown_Bar_Accur";
numericUpDown_Bar_Accur.Size = new Size(40, 23);
numericUpDown_Bar_Accur.TabIndex = 11;
numericUpDown_Bar_Accur.Value = new decimal(new int[] { 1, 0, 0, 0 });
//
// label9
//
label9.AutoSize = true;
label9.Location = new Point(112, 53);
label9.Name = "label9";
label9.Size = new Size(20, 15);
label9.TabIndex = 12;
label9.Text = "Pa";
//
// label10
//
label10.AutoSize = true;
label10.Location = new Point(111, 82);
label10.Name = "label10";
label10.Size = new Size(18, 15);
label10.TabIndex = 15;
label10.Text = "m";
//
// textBox_GPS_Accur
//
textBox_GPS_Accur.DecimalPlaces = 1;
textBox_GPS_Accur.Location = new Point(68, 80);
textBox_GPS_Accur.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
textBox_GPS_Accur.Name = "textBox_GPS_Accur";
textBox_GPS_Accur.Size = new Size(40, 23);
textBox_GPS_Accur.TabIndex = 14;
textBox_GPS_Accur.Value = new decimal(new int[] { 1, 0, 0, 0 });
//
// label11
//
label11.AutoSize = true;
label11.Location = new Point(6, 82);
label11.Name = "label11";
label11.Size = new Size(56, 15);
label11.TabIndex = 13;
label11.Tag = "#accuracy";
label11.Text = "Accuracy";
//
// groupBox1
//
groupBox1.Controls.Add(checkBox_OF_Enable);
groupBox1.Controls.Add(label17);
groupBox1.Controls.Add(numericUpDown1);
groupBox1.Controls.Add(label16);
groupBox1.Controls.Add(label12);
groupBox1.Controls.Add(numericUpDown_OF_Accur);
groupBox1.Controls.Add(label13);
groupBox1.Controls.Add(numericUpDown_OF_Freq);
groupBox1.Controls.Add(label14);
groupBox1.Controls.Add(label15);
groupBox1.Dock = DockStyle.Top;
groupBox1.Enabled = false;
groupBox1.Location = new Point(3, 203);
groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(204, 114);
groupBox1.TabIndex = 4;
groupBox1.TabStop = false;
groupBox1.Text = "Optical flow";
//
// label12
//
label12.AutoSize = true;
label12.Location = new Point(111, 53);
label12.Name = "label12";
label12.Size = new Size(28, 15);
label12.TabIndex = 18;
label12.Text = "Deg";
//
// numericUpDown_OF_Accur
//
numericUpDown_OF_Accur.DecimalPlaces = 1;
numericUpDown_OF_Accur.Location = new Point(69, 51);
numericUpDown_OF_Accur.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
numericUpDown_OF_Accur.Name = "numericUpDown_OF_Accur";
numericUpDown_OF_Accur.Size = new Size(40, 23);
numericUpDown_OF_Accur.TabIndex = 17;
numericUpDown_OF_Accur.Value = new decimal(new int[] { 1, 0, 0, 0 });
//
// label13
//
label13.AutoSize = true;
label13.Location = new Point(7, 53);
label13.Name = "label13";
label13.Size = new Size(56, 15);
label13.TabIndex = 16;
label13.Tag = "#accuracy";
label13.Text = "Accuracy";
//
// numericUpDown_OF_Freq
//
numericUpDown_OF_Freq.Location = new Point(69, 22);
numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_OF_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Freq.Name = "numericUpDown_OF_Freq";
numericUpDown_OF_Freq.Size = new Size(40, 23);
numericUpDown_OF_Freq.TabIndex = 13;
numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 });
//
// label14
//
label14.AutoSize = true;
label14.Location = new Point(112, 24);
label14.Name = "label14";
label14.Size = new Size(21, 15);
label14.TabIndex = 15;
label14.Text = "Hz";
//
// label15
//
label15.AutoSize = true;
label15.Location = new Point(6, 24);
label15.Name = "label15";
label15.Size = new Size(62, 15);
label15.TabIndex = 14;
label15.Tag = "#frequency";
label15.Text = "Frequency";
//
// checkBox_GPS_Enable
//
checkBox_GPS_Enable.AutoSize = true;
checkBox_GPS_Enable.Checked = true;
checkBox_GPS_Enable.CheckState = CheckState.Checked;
checkBox_GPS_Enable.Location = new Point(158, 87);
checkBox_GPS_Enable.Name = "checkBox_GPS_Enable";
checkBox_GPS_Enable.Size = new Size(39, 19);
checkBox_GPS_Enable.TabIndex = 16;
checkBox_GPS_Enable.Tag = "#en";
checkBox_GPS_Enable.Text = "En";
checkBox_GPS_Enable.UseVisualStyleBackColor = true;
//
// label16
//
label16.AutoSize = true;
label16.Location = new Point(1, 82);
label16.Name = "label16";
label16.Size = new Size(67, 15);
label16.TabIndex = 19;
label16.Tag = "#max_height";
label16.Text = "Max height";
//
// numericUpDown1
//
numericUpDown1.Location = new Point(68, 80);
numericUpDown1.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown1.Name = "numericUpDown1";
numericUpDown1.Size = new Size(40, 23);
numericUpDown1.TabIndex = 20;
numericUpDown1.Value = new decimal(new int[] { 8, 0, 0, 0 });
//
// label17
//
label17.AutoSize = true;
label17.Location = new Point(111, 82);
label17.Name = "label17";
label17.Size = new Size(18, 15);
label17.TabIndex = 21;
label17.Text = "m";
//
// checkBox_OF_Enable
//
checkBox_OF_Enable.AutoSize = true;
checkBox_OF_Enable.Checked = true;
checkBox_OF_Enable.CheckState = CheckState.Checked;
checkBox_OF_Enable.Location = new Point(158, 89);
checkBox_OF_Enable.Name = "checkBox_OF_Enable";
checkBox_OF_Enable.Size = new Size(39, 19);
checkBox_OF_Enable.TabIndex = 22;
checkBox_OF_Enable.Tag = "#en";
checkBox_OF_Enable.Text = "En";
checkBox_OF_Enable.UseVisualStyleBackColor = true;
//
// checkBox_Bar_Enable
//
checkBox_Bar_Enable.AutoSize = true;
checkBox_Bar_Enable.Checked = true;
checkBox_Bar_Enable.CheckState = CheckState.Checked;
checkBox_Bar_Enable.Location = new Point(158, 63);
checkBox_Bar_Enable.Name = "checkBox_Bar_Enable";
checkBox_Bar_Enable.Size = new Size(39, 19);
checkBox_Bar_Enable.TabIndex = 17;
checkBox_Bar_Enable.Tag = "#en";
checkBox_Bar_Enable.Text = "En";
checkBox_Bar_Enable.UseVisualStyleBackColor = true;
//
// Form_Main
//
AutoScaleDimensions = new SizeF(7F, 15F);
@ -431,8 +811,22 @@
groupBox_Clients.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Clients_Limit).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Clients_Port).EndInit();
tabPage_Model.ResumeLayout(false);
groupBox_Navi.ResumeLayout(false);
panel1.ResumeLayout(false);
groupBox_GPS.ResumeLayout(false);
groupBox_GPS.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_GPS_Freq).EndInit();
groupBox_Barometer.ResumeLayout(false);
groupBox_Barometer.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Freq).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Bar_Accur).EndInit();
((System.ComponentModel.ISupportInitialize)textBox_GPS_Accur).EndInit();
groupBox1.ResumeLayout(false);
groupBox1.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Accur).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).EndInit();
ResumeLayout(false);
PerformLayout();
}
@ -470,5 +864,36 @@
private Label label1;
private Label label_Visual_Num;
private Label label3;
private GroupBox groupBox_GPS;
private TextBox textBox_GPS_Lon;
private Label label2;
private TextBox textBox_GPS_Lat;
private NumericUpDown numericUpDown_GPS_Freq;
private Label label6;
private Label label_GPS_Frequency;
private Label label4;
private GroupBox groupBox_Barometer;
private NumericUpDown numericUpDown_Bar_Freq;
private Label label5;
private Label label7;
private Label label9;
private NumericUpDown numericUpDown_Bar_Accur;
private Label label8;
private Label label10;
private NumericUpDown textBox_GPS_Accur;
private Label label11;
private GroupBox groupBox1;
private Label label12;
private NumericUpDown numericUpDown_OF_Accur;
private Label label13;
private NumericUpDown numericUpDown_OF_Freq;
private Label label14;
private Label label15;
private CheckBox checkBox_GPS_Enable;
private Label label17;
private NumericUpDown numericUpDown1;
private Label label16;
private CheckBox checkBox_OF_Enable;
private CheckBox checkBox_Bar_Enable;
}
}

18
DroneSimulator/FormMain.cs
View File

@ -4,6 +4,9 @@ using System.Windows.Forms;
using static System.Net.Mime.MediaTypeNames;
using static System.Runtime.InteropServices.JavaScript.JSType;
using System.Security.Policy;
using System.Runtime.InteropServices;
using System.CodeDom;
using System.Linq;
namespace DroneSimulator
{
@ -69,13 +72,16 @@ namespace DroneSimulator
if (drone == null) return;
Drone.DataIn id = (Drone.DataIn)Drone.fromBytes(data.Buffer, typeof(Drone.DataIn));
List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
Drone.DataOut od = drone.GetDataOut();
try { data.Client.Send(Drone.getBytes(od)); }
if (send == null) return;
try
{
foreach (byte[]? b in send)
{
if (b != null) data.Client?.Send(b);
}
}
catch { }
}

12
DroneSimulator/NetServerClients.cs
View File

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

0
test.txt Normal file
View File

1
www.txt
View File

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