update

Common/DroneData.cs

@@ -0,0 +1,42 @@
+namespace DroneData
+{
+  enum StructType : ulong
+  { 
+    // Output
+    DataIMU = 1, DataPos = 2, 
+
+    // Input
+    DataMotor4 = 1001, DataMotor6 = 1002
+  };
+
+  public struct DataInfo
+  {
+    StructType Type;
+    ulong Size;
+  }
+
+  public struct DataXYZ { float X, Y, Z; }
+
+  public struct DataIMU
+  {
+    DataXYZ Acc, Gyr, Mag;
+  }
+
+  public struct DataPos
+  {
+    DataXYZ Local; // Ëîêàëüíûå êîîðäèíàòû
+    float LiDAR; // Äàò÷èê ïîñàäêè
+  }
+
+  public struct DataMotor4
+  {
+    ulong Count;
+    float M1, M2, M3, M4;
+  }
+
+  public struct DataMotor6
+  {
+    ulong Count;
+    float M1, M2, M3, M4, M5, M6;
+  }
+}

Common/DroneData.h

@@ -0,0 +1,42 @@
+namespace DroneData
+{
+  enum class StructType : unsigned long
+  { 
+    // Output
+    DataIMU = 1, DataPos = 2, 
+
+    // Input
+    DataMotor4 = 1001, DataMotor6 = 1002
+  };
+
+  public struct DataInfo
+  {
+    StructType Type;
+    unsigned long Size;
+  };
+
+  public struct DataXYZ { float X, Y, Z; };
+
+  public struct DataIMU
+  {
+    DataXYZ Acc, Gyr, Mag;
+  };
+
+  public struct DataPos
+  {
+    DataXYZ Local; // Ëîêàëüíûå êîîðäèíàòû
+    float LiDAR; // Äàò÷èê ïîñàäêè
+  };
+
+  public struct DataMotor4
+  {
+    ulong Count;
+    float M1, M2, M3, M4;
+  };
+
+  public struct DataMotor6
+  {
+    ulong Count;
+    float M1, M2, M3, M4, M5, M6;
+  };
+}

DroneClient/NetClient.cs

@@ -41,9 +41,7 @@ namespace DroneSimulator
     {
       if (Connected)
       {
-        try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
+        Close();
-        ServerSocket?.Close();
-        Connected = false;
         return ClientState.Stop;
       }
 

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 Power = 0; // Тяга всех двигателей (0-1)
-    public float MaxPower; // Тяга всех двигателей
+    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;
+        Action(Environment.TickCount);
-        Timer = Environment.TickCount;
-        Action(time / 1000);
         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;
@@ -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;
+    }
   }
 }

DroneSimulator/DroneData.cs

@@ -0,0 +1,44 @@
+namespace DroneData
+{
+  public enum StructType : ulong
+  { 
+    None = 0, Head = 1,
+
+    // Output
+    DataIMU = 1001, DataPos = 1002, 
+
+    // Input
+    DataMotor4 = 2001, DataMotor6 = 2002
+  };
+
+  public struct DataHead
+  {
+    public StructType Type;
+    public ulong Size;
+  }
+
+  public struct XYZ { public float X, Y, Z; }
+
+  public struct DataIMU
+  {
+    public XYZ Acc, Gyr, Mag;
+  }
+
+  public struct DataPos
+  {
+    public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû
+    public float LiDAR; // Äàò÷èê ïîñàäêè
+  }
+
+  public struct DataMotor4
+  {
+    public ulong Count;
+    public float UL, UR, DL, DR;
+  }
+
+  public struct DataMotor6
+  {
+    public ulong Count;
+    public float UL, UR, LL, RR, DL, DR;
+  }
+}

DroneSimulator/FormMain.cs

@@ -4,6 +4,8 @@ using System.Windows.Forms;
 using static System.Net.Mime.MediaTypeNames;
 using static System.Runtime.InteropServices.JavaScript.JSType;
 using System.Security.Policy;
+using System.Runtime.InteropServices;
+using System.CodeDom;
 
 namespace DroneSimulator
 {
@@ -21,7 +23,7 @@ namespace DroneSimulator
       InitializeComponent();
     }
 
-    private void ClientConnectionCallback(object o)
+    private int ClientConnectionCallback(object o)
     {
       NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
 
@@ -52,9 +54,11 @@ namespace DroneSimulator
           break;
         }
       }
+
+      return Marshal.SizeOf(typeof(DroneData.DataHead));
     }
 
-    private void ClientReceiveCallback(object o)
+    private int ClientReceiveCallback(object o)
     {
       NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
 
@@ -67,16 +71,18 @@ namespace DroneSimulator
         break;
       }
 
-      if (drone == null) return;
+      if (drone == null) return 0;
 
-      Drone.DataIn id = (Drone.DataIn)Drone.fromBytes(data.Buffer, typeof(Drone.DataIn));
+      int size=drone.RecvDataStream(data.Buffer);
 
-      drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
+      try 
-
+      { 
-      Drone.DataOut od = drone.GetDataOut();
+        data.Client.Send(drone.SendDataStream(DroneData.StructType.DataIMU));
-
+        data.Client.Send(drone.SendDataStream(DroneData.StructType.DataPos));
-      try { data.Client.Send(Drone.getBytes(od)); }
+      }
       catch { }
+
+      return size;
     }
 
     private void button_Client_Start_Click(object sender, EventArgs e)

DroneSimulator/NetServerClients.cs

@@ -1,5 +1,6 @@
 using System.Net.Sockets;
 using System.Net;
+using System.Drawing;
 
 namespace DroneSimulator
 {
@@ -27,8 +28,7 @@ namespace DroneSimulator
     {
       public int ID;
       public Socket? workSocket = null;
-      public const int BufferSize = 1024;
+      public byte[] buffer;
-      public byte[] buffer = new byte[BufferSize];
     }
 
     private int SocketID = 0;
@@ -36,7 +36,7 @@ namespace DroneSimulator
     private Socket? ServerSocket;
     private List<ClientData> ClientSockets = new List<ClientData>();
 
-    public delegate void ServerCallback(object o);
+    public delegate int ServerCallback(object o);
 
     private ServerCallback? ConnectionCallback;
     private ServerCallback? ReceiveCallback;
@@ -99,9 +99,11 @@ namespace DroneSimulator
 
         ClientSockets.Add(clientData);
 
-        ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
+        int size = ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
 
-        handler.BeginReceive(clientData.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), clientData);
+        clientData.buffer=new byte[size];
+
+        handler.BeginReceive(clientData.buffer, 0, size, 0, new AsyncCallback(ReadCallback), clientData);
       }
       else handler.Close();
 
@@ -128,11 +130,13 @@ namespace DroneSimulator
         return;
       }
 
-      ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
+      int size = ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
+
+      cd.buffer = new byte[size];
 
       try
       {
-        cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), cd);
+        cd.workSocket?.BeginReceive(cd.buffer, 0, size, 0, new AsyncCallback(ReadCallback), cd);
       }
       catch { }
     }