From e0f3e3db19401d42ebe06c9d9447ae767fc769c7 Mon Sep 17 00:00:00 2001
From: Sergey Sklyarov <sasklyarov@sfedu.ru>
Date: Fri, 16 May 2025 01:27:03 +0300
Subject: [PATCH] Add drone state window

---
 DroneSimulator/Drone.cs             | 22 +++++++++++++++++++++-
 DroneSimulator/DroneData.cs         | 13 ++++++++++++-
 DroneSimulator/FormMain.Designer.cs | 11 +++++++++++
 DroneSimulator/FormMain.cs          |  6 ++++++
 DroneSimulator/FormMain.resx        |  2 +-
 5 files changed, 51 insertions(+), 3 deletions(-)

diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs
index ca450cd..25fa9a7 100644
--- a/DroneSimulator/Drone.cs
+++ b/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;
       }
 
diff --git a/DroneSimulator/DroneData.cs b/DroneSimulator/DroneData.cs
index ac1310f..9e7961b 100644
--- a/DroneSimulator/DroneData.cs
+++ b/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));
+  }
 }
diff --git a/DroneSimulator/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs
index 3f80daa..a91a96b 100644
--- a/DroneSimulator/FormMain.Designer.cs
+++ b/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;
   }
 }
diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs
index 4eab543..00a0f35 100644
--- a/DroneSimulator/FormMain.cs
+++ b/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();
diff --git a/DroneSimulator/FormMain.resx b/DroneSimulator/FormMain.resx
index 8f19afc..1a738dd 100644
--- a/DroneSimulator/FormMain.resx
+++ b/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">