dmarkova/Simulator
1
0
Fork 0
forked from CPL/Simulator
Code Pull Requests Activity

Update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-04-11 21:59:06 +03:00
parent 64c0637e6a
commit bac52d315b
12 changed files with 933 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
DroneClientCpp/Drone.cpp
View File

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

9
DroneClientCpp/Drone.h
View File

@ -18,8 +18,11 @@ namespace DroneClient {
public:
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
unsigned int TimeAcc, TimeGyr;
float PosX, PosY;
float LaserRange;
unsigned int TimeRange;
float MotorUL, MotorUR, MotorDL, MotorDR;
@ -28,8 +31,10 @@ namespace DroneClient {
private:
array<Byte>^ SendDataMotor4();
array<Byte>^ RecvDataIMU(array<Byte>^ data);
array<Byte>^ RecvDataPos(array<Byte>^ data);
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;

64
DroneClientCpp/DroneData.h
View File

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

BIN
DroneClientCpp/FormMain.h
View File

Binary file not shown.

14
DroneClientCpp/NetClient.cpp
View File

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