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;
}
}