Add drone state window

DroneSimulator/Drone.cs

@@ -24,6 +24,8 @@ namespace DroneSimulator
     public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
     public float LaserRange; // Имитация: Дальномер
 
+    public Vector4 Orientation;
+
     private uint DataTimer;
 
     private const float Gravity = 9.8f;
@@ -210,6 +212,8 @@ namespace DroneSimulator
 
       Vector4 ori = GetOrientation();
 
+      Orientation = ori;
+
       if (PosXYZ.Z < 0)
       {
         PosXYZ.Z = 0;
@@ -374,9 +378,23 @@ namespace DroneSimulator
       return getBytes(bar);
     }
 
+    private byte[] SendDataQuaternion()
+    {
+      DroneData.DataQuat quat = new DroneData.DataQuat();
+
+      quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
+      quat.Head.Mode = DroneData.DataMode.Response;
+      quat.Head.Type = DroneData.DataType.DataQuat;
+      quat.Head.Time = (uint)Environment.TickCount;
+
+      quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
+
+      return getBytes(quat);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
-      byte[] zero = new byte[0];
+      byte[] zero = Array.Empty<byte>();
 
       switch (head.Type)
       {
@@ -406,6 +424,8 @@ namespace DroneSimulator
 
         case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
 
+        case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero;
+
         case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
       }
 

DroneSimulator/DroneData.cs

@@ -15,7 +15,10 @@ namespace DroneData
     DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
 
     // Input
-    DataMotor4 = 2001, DataMotor6 = 2002
+    DataMotor4 = 2001, DataMotor6 = 2002,
+
+    // State
+    DataQuat = 3001,
   };
 
   public struct DataHead
@@ -107,4 +110,12 @@ namespace DroneData
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
   }
+
+  public struct DataQuat
+  {
+    public DataHead Head;
+    public float X, Y, Z, W;
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataQuat));
+  }
 }

DroneSimulator/FormMain.Designer.cs

@@ -159,6 +159,7 @@
       comboBox_Drone_Rotor = new ComboBox();
       comboBox_Drone = new ComboBox();
       timer_Test = new System.Windows.Forms.Timer(components);
+      listBox_Drones = new ListBox();
       menuStrip_Menu.SuspendLayout();
       groupBox_Screen.SuspendLayout();
       ((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
@@ -1585,6 +1586,7 @@
       // 
       // panel1
       // 
+      panel1.Controls.Add(listBox_Drones);
       panel1.Controls.Add(button_Drone_Color);
       panel1.Controls.Add(comboBox_Drone_Rotor);
       panel1.Dock = DockStyle.Fill;
@@ -1627,6 +1629,14 @@
       timer_Test.Interval = 10;
       timer_Test.Tick += timer_Test_Tick;
       // 
+      // listBox_Drones
+      // 
+      listBox_Drones.FormattingEnabled = true;
+      listBox_Drones.Location = new Point(3, 45);
+      listBox_Drones.Name = "listBox_Drones";
+      listBox_Drones.Size = new Size(188, 214);
+      listBox_Drones.TabIndex = 3;
+      // 
       // Form_Main
       // 
       AutoScaleDimensions = new SizeF(7F, 15F);
@@ -1840,5 +1850,6 @@
     private Label label56;
     private GroupBox groupBox2;
     private CheckBox checkBox_Mode_Real;
+    private ListBox listBox_Drones;
   }
 }

DroneSimulator/FormMain.cs

@@ -142,9 +142,15 @@ namespace DroneSimulator
     {
       if (screen2D == null) return;
 
+      listBox_Drones.Items.Clear();
+
       foreach (Drone d in AllDrones)
       {
         screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+
+        string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString();
+
+        listBox_Drones.Items.Add(line);
       }
 
       screen2D.DrawScene();

DroneSimulator/FormMain.resx

@@ -124,7 +124,7 @@
     <value>162, 5</value>
   </metadata>
   <metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
-    <value>25</value>
+    <value>78</value>
   </metadata>
   <assembly alias="System.Drawing" name="System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" />
   <data name="$this.Icon" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">