update

DroneClient/Drone.cs

@@ -1,21 +1,17 @@
-using System.Runtime.InteropServices;
+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 float PosX, PosY;
+    public float LaserRange;
+
+    public float MotorUL, MotorUR, MotorDL, MotorDR;
 
     public static byte[] getBytes(object data)
     {
@@ -58,5 +54,165 @@ 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[]? RecvDataIMU(byte[] data)
+    {
+      DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
+
+      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 new byte[0];
+    }
+
+    private byte[]? RecvDataPos(byte[] data)
+    {
+      DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos));
+
+      PosX = pos.Local.X; PosY = pos.Local.Y;
+      LaserRange = pos.LiDAR;
+
+      return new byte[0];
+    }
+
+    private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
+    {
+      byte[] zero = new 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;
+    }
+
+    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 imu = new DroneData.DataHead();
+      imu.Size = DroneData.DataHead.StrLen;
+      imu.Mode = DroneData.DataMode.Request;
+      imu.Type = DroneData.DataType.DataIMU;
+
+      DroneData.DataHead pos = new DroneData.DataHead();
+      pos.Size = DroneData.DataHead.StrLen;
+      pos.Mode = DroneData.DataMode.Request;
+      pos.Type = DroneData.DataType.DataPos;
+
+      byte[] si = getBytes(imu);
+      byte[] sp = getBytes(pos);
+      byte[] sm = SendDataMotor4();
+
+      byte[] send = new byte[si.Length + sp.Length + sm.Length];
+      si.CopyTo(send, 0);
+      sp.CopyTo(send, si.Length);
+      sm.CopyTo(send, si.Length + sp.Length);
+
+      return send;
+    }
   }
 }