update

DroneSimulator/Drone.cs

@@ -1,5 +1,6 @@
 using System.Numerics;
 using System.Runtime.InteropServices;
+using System.Security.Cryptography;
 
 namespace DroneSimulator
 {
@@ -12,8 +13,8 @@ namespace DroneSimulator
     public const float Dynamic = 10; // Динамика вращения
     public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
     public Quaternion Quat; // Основной кватернион
-    public float Power = 0; // Тяга всех двигателей
-    public float MaxPower; // Тяга всех двигателей
+    public float Power = 0; // Тяга всех двигателей (0-1)
+    public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
 
     public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
@@ -70,7 +71,7 @@ namespace DroneSimulator
       return mem;
     }
 
-    public struct DataOut
+    /*public struct DataOut
     {
       public float AccX, AccY, AccZ;
       public float GyrX, GyrY, GyrZ;
@@ -95,7 +96,7 @@ namespace DroneSimulator
     public struct DataIn
     {
       public float MotorUL, MotorUR, MotorDL, MotorDR;
-    }
+    }*/
 
     public struct DataVisual
     {
@@ -113,9 +114,7 @@ namespace DroneSimulator
     {
       while (DroneThread != null)
       {
-        float time = Environment.TickCount - Timer;
-        Timer = Environment.TickCount;
-        Action(time / 1000);
+        Action(Environment.TickCount);
         Thread.Sleep(1);
       }
     }
@@ -191,8 +190,11 @@ namespace DroneSimulator
       return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
     }
 
-    public void Action(float time)
+    public void Action(int tick)
     {
+      float time = (tick - Timer) / 1000.0f;
+      Timer = tick;
+
       if (!Active) return;
 
       float flow = Power;
@@ -206,7 +208,7 @@ namespace DroneSimulator
 
       Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
       AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
-      
+
       SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
       PosXYZ += SpdXYZ * time;
 
@@ -281,5 +283,72 @@ namespace DroneSimulator
       AccPRY.X = ((ul + ur) - (dl + dr));
       AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
     }
+
+    private DroneData.DataHead StreamHead = new DroneData.DataHead() { Type = DroneData.StructType.None, Size = 0 };
+
+    public int RecvDataStream(byte[] data)
+    {
+      if (StreamHead.Type == DroneData.StructType.None) return Marshal.SizeOf(typeof(DroneData.DataHead));
+
+      if (StreamHead.Type == DroneData.StructType.Head)
+      {
+        StreamHead = (DroneData.DataHead)fromBytes(data, typeof(DroneData.DataHead));
+
+        return (int)StreamHead.Size;
+      }
+
+      switch(StreamHead.Type)
+      {
+        case DroneData.StructType.DataIMU:
+          {
+            DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));
+            /* обработка */
+            break;
+          }
+          case DroneData.StructType.DataPos:
+          {
+            DroneData.DataPos pos = (DroneData.DataPos)fromBytes(data, typeof(DroneData.DataPos));
+            /* обработка */
+            break;
+          }
+        case DroneData.StructType.DataMotor4:
+          {
+            DroneData.DataMotor4 pos = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
+            /* обработка */
+            SetQadroPow(pos.UL, pos.UR, pos.DL, pos.DR);
+            break;
+          }
+      }
+
+      return Marshal.SizeOf(typeof(DroneData.DataHead));
+    }
+
+    public byte[] SendDataStream(DroneData.StructType type)
+    {
+      switch(type)
+      {
+        case DroneData.StructType.DataIMU:
+          {
+            DroneData.DataIMU imu=new DroneData.DataIMU();
+
+            imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
+            imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
+            imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;
+
+            return getBytes(imu);
+          }
+          case DroneData.StructType.DataPos:
+          {
+            DroneData.DataPos pos = new DroneData.DataPos();
+
+            pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
+            pos.LiDAR = LaserRange;
+
+            return getBytes(pos);
+          }
+      }
+
+      return null;
+    }
   }
 }