Update

DroneClient/Drone.cs

@@ -1,4 +1,5 @@
-using System.Drawing;
+using System.Diagnostics.Metrics;
+using System.Drawing;
 using System.Runtime.InteropServices;
 
 namespace DroneClient
@@ -8,8 +9,11 @@ namespace DroneClient
 
     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;
 
@@ -70,22 +74,44 @@ namespace DroneClient
       return getBytes(mot4);
     }
 
-    private byte[]? RecvDataIMU(byte[] data)
+    private byte[]? RecvDataAcc(byte[] data)
     {
-      DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
+      DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));
 
       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 new byte[0];
     }
 
-    private byte[]? RecvDataPos(byte[] data)
+    private byte[]? RecvDataGyr(byte[] data)
     {
-      DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos));
+      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;
-      LaserRange = pos.LiDAR;
 
       return new byte[0];
     }
@@ -96,7 +122,7 @@ namespace DroneClient
 
       switch (head.Type)
       {
-        case DroneData.DataType.DataIMU:
+        case DroneData.DataType.DataAcc:
           {
             if (head.Mode == DroneData.DataMode.Request)
             {
@@ -108,11 +134,11 @@ namespace DroneClient
             else
             {
               // Пришли данные
-              return RecvDataIMU(body);
+              return RecvDataAcc(body);
             }
           }
 
-        case DroneData.DataType.DataPos:
+        case DroneData.DataType.DataGyr:
           {
             if (head.Mode == DroneData.DataMode.Request)
             {
@@ -124,7 +150,39 @@ namespace DroneClient
             else
             {
               // Пришли данные
-              return RecvDataPos(body);
+              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);
             }
           }
 
@@ -193,24 +251,46 @@ namespace DroneClient
 
     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 acc = new DroneData.DataHead();
+      acc.Size = DroneData.DataHead.StrLen;
+      acc.Mode = DroneData.DataMode.Request;
+      acc.Type = DroneData.DataType.DataAcc;
 
-      DroneData.DataHead pos = new DroneData.DataHead();
-      pos.Size = DroneData.DataHead.StrLen;
-      pos.Mode = DroneData.DataMode.Request;
-      pos.Type = DroneData.DataType.DataPos;
+      DroneData.DataHead gyr = new DroneData.DataHead();
+      gyr.Size = DroneData.DataHead.StrLen;
+      gyr.Mode = DroneData.DataMode.Request;
+      gyr.Type = DroneData.DataType.DataGyr;
 
-      byte[] si = getBytes(imu);
-      byte[] sp = getBytes(pos);
-      byte[] sm = SendDataMotor4();
+      DroneData.DataHead range = new DroneData.DataHead();
+      range.Size = DroneData.DataHead.StrLen;
+      range.Mode = DroneData.DataMode.Request;
+      range.Type = DroneData.DataType.DataRange;
 
-      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);
+      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;
     }