cpp

DroneClientCpp/Drone.cpp

@@ -0,0 +1,197 @@
+#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);
+    }
+
+    // Реализация приватного метода RecvDataIMU
+    array<Byte>^ Drone::RecvDataIMU(array<Byte>^ data)
+    {
+        DroneData::DataIMU imu = (DroneData::DataIMU)FromBytes(data, DroneData::DataIMU::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;
+
+        return gcnew array<Byte>(0);
+    }
+
+    // Реализация приватного метода RecvDataPos
+    array<Byte>^ Drone::RecvDataPos(array<Byte>^ data)
+    {
+        DroneData::DataPos pos = (DroneData::DataPos)FromBytes(data, DroneData::DataPos::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:
+            if (head.Mode == DroneData::DataMode::Request)
+            {
+                return zero; // Запрос данных (не реализовано)
+            }
+            else
+            {
+                return RecvDataIMU(body); // Пришли данные
+            }
+
+        case DroneData::DataType::DataPos:
+            if (head.Mode == DroneData::DataMode::Request)
+            {
+                return zero; // Запрос данных (не реализовано)
+            }
+            else
+            {
+                return RecvDataPos(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
+    System::Collections::Generic::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 imu;
+        imu.Size = DroneData::DataHead::StrLen;
+        imu.Mode = DroneData::DataMode::Request;
+        imu.Type = DroneData::DataType::DataIMU;
+
+        DroneData::DataHead pos;
+        pos.Size = DroneData::DataHead::StrLen;
+        pos.Mode = DroneData::DataMode::Request;
+        pos.Type = DroneData::DataType::DataPos;
+
+        array<Byte>^ si = GetBytes(imu);
+        array<Byte>^ sp = GetBytes(pos);
+        array<Byte>^ sm = SendDataMotor4();
+
+        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);
+
+        return send;
+    }
+}