Merge branch 'sergey' of https://git.skbkit.ru/dmarkova/Simulator

DroneSimulator/Drone.cs

@@ -2,6 +2,8 @@
 using System.Numerics;
 using System.Runtime.InteropServices;
 using System.Security.Cryptography;
+using static DroneSimulator.Drone;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
 
 namespace DroneSimulator
 {
@@ -31,7 +33,17 @@ namespace DroneSimulator
     private Thread DroneThread;
     private int Timer;
 
-    private static int CounterID = 0;
+    private Random MainRandom = new Random();
+
+    public struct DataBarometer
+    {
+      public int Value, Pressure; // Значение давления в Паскальях
+      public float Accuracy;
+      public int Frequency;
+      public uint Time;
+    }
+
+    public DataBarometer dataBarometer;
 
     public static byte[] getBytes(object data)
     {
@@ -240,6 +252,14 @@ namespace DroneSimulator
         else LaserRange = float.MaxValue;
       }
 
+      if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer)
+      {
+        float bar = dataBarometer.Pressure - PosXYZ.Z * 11;
+        int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10;
+        dataBarometer.Value = (int)(bar + rnd);
+        dataBarometer.Time = DataTimer;
+      }
+
       DataTimer = (uint)tick;
     }
 
@@ -344,6 +364,21 @@ namespace DroneSimulator
       return getBytes(local);
     }
 
+    private byte[] SendDataBarometer()
+    {
+      DroneData.DataBar bar = new DroneData.DataBar();
+
+      bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
+      bar.Head.Mode = DroneData.DataMode.Response;
+      bar.Head.Type = DroneData.DataType.DataBar;
+      bar.Head.Time = (uint)Environment.TickCount;
+
+      bar.Pressure = dataBarometer.Value;
+      bar.Time = dataBarometer.Time;
+
+      return getBytes(bar);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
       byte[] zero = new byte[0];
@@ -366,86 +401,17 @@ namespace DroneSimulator
             }
           }
 
-        case DroneData.DataType.DataGyr:
+        case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataGyr();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
 
-        case DroneData.DataType.DataMag:
+        case DroneData.DataType.DataMag: if (head.Mode == DroneData.DataMode.Request) return SendDataMag(); else return zero;
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataMag();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
 
-        case DroneData.DataType.DataRange:
+        case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero;
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataRange();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
 
-        case DroneData.DataType.DataLocal:
+        case DroneData.DataType.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else return zero;
-          {
-            if (head.Mode == DroneData.DataMode.Request)
-            {
-              // Запрос данных
-              return SendDataLocal();
-            }
-            else
-            {
-              // Пришли данные
-              // ... //
-              //
-              return zero;
-            }
-          }
 
-        case DroneData.DataType.DataMotor4:
+        case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
-          {
+
-            if (head.Mode == DroneData.DataMode.Request)
+        case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
-            {
-              // Запрос данных
-              // ... //
-              //
-              return zero;
-            }
-            else
-            {
-              // Пришли данные
-              RecvDataMotor4(body);
-              return zero;
-            }
-          }
       }
 
       return zero;

DroneSimulator/DroneData.cs

@@ -12,7 +12,7 @@ namespace DroneData
     None = 0, Head = 1,
 
     // Output
-    DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, 
+    DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
 
     // Input
     DataMotor4 = 2001, DataMotor6 = 2002
@@ -62,6 +62,16 @@ namespace DroneData
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
   }
 
+  public struct DataRange
+  {
+    public DataHead Head;
+    public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+
+    public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
+  }
+
   public struct DataLocal
   {
     public DataHead Head;
@@ -72,14 +82,14 @@ namespace DroneData
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
   }
 
-  public struct DataRange
+  public struct DataBar
   {
     public DataHead Head;
-    public float LiDAR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    public int Pressure; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
     public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
+    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
   }
 
   public struct DataMotor4

DroneSimulator/FormMain.cs

@@ -22,11 +22,33 @@ namespace DroneSimulator
         NetServerClients netServerClient = new NetServerClients();
         NetServerVisual netServerVisual = new NetServerVisual();
 
-        List<Drone> AllDrones = new List<Drone>();
+      Invoke((MethodInvoker)delegate
-        private ClientWebSocket _webSocket;
+      {
-        private CancellationTokenSource _cts;
+        label_Clients_Num.Text = data.Count.ToString();
-        private bool _isRunning;
+      });
-        public Form_Main()
+
+      if (data.Connect)
+      {
+        Drone drone = new Drone(data.ID);
+        drone.Create(1.0f, 0.5f, 1.0f);
+
+        //
+        drone.dataBarometer.Pressure = 102258;
+        drone.dataBarometer.Accuracy = (float)numericUpDown_Bar_Accur.Value;
+        drone.dataBarometer.Frequency = (int)numericUpDown_Bar_Freq.Value;
+        try { drone.dataBarometer.Pressure = int.Parse(textBox_Bar_Pressure.Text); }
+        catch { MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error); }
+
+        //
+
+        screen2D.CreateDrone(Color.Red, data.ID);
+
+        AllDrones.Add(drone);
+      }
+      else
+      {
+        foreach (Drone drone in AllDrones)
+
         {
             InitializeComponent();
         }
@@ -214,4 +236,128 @@ namespace DroneSimulator
 
      
     }
+
+    private void button_Client_Start_Click(object sender, EventArgs e)
+    {
+      var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ClientConnectionCallback, ClientReceiveCallback);
+      switch (done)
+      {
+        case NetServerClients.ServerState.Error:
+          {
+            MessageBox.Show("Error to start clients server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
+            break;
+          }
+        case NetServerClients.ServerState.Start:
+          {
+            button_Client_Start.Text = "Stop";
+            button_Client_Start.BackColor = Color.LimeGreen;
+            panel_Menu_Model.Enabled = false;
+            break;
+          }
+        case NetServerClients.ServerState.Stop:
+          {
+            label_Clients_Num.Text = "0";
+            button_Client_Start.Text = "Start";
+            button_Client_Start.BackColor = Color.Transparent;
+            panel_Menu_Model.Enabled = true;
+            break;
+          }
+      }
+
+      if (done != NetServerClients.ServerState.Start) return;
+
+      pictureBox_2D.Image = null;
+
+      screen2D = new Screen2D(DrawCallback);
+    }
+
+    private void exitToolStripMenuItem_Click(object sender, EventArgs e)
+    {
+      Close();
+    }
+
+    private void DrawCallback(Bitmap bmp)
+    {
+      Invoke((MethodInvoker)delegate
+      {
+        if (pictureBox_2D.Image == null) pictureBox_2D.Image = bmp;
+        pictureBox_2D.Refresh();
+      });
+    }
+
+    private void timer_Test_Tick(object sender, EventArgs e)
+    {
+      if (screen2D == null) return;
+
+      foreach (Drone d in AllDrones)
+      {
+        screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
+      }
+
+      screen2D.DrawScene();
+    }
+    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+    {
+      foreach (Drone d in AllDrones) d.Close();
+    }
+
+    private void VisualConnectionCallback(object o)
+    {
+      NetServerVisual.ConnectData data = (NetServerVisual.ConnectData)o;
+
+      Invoke((MethodInvoker)delegate
+      {
+        label_Clients_Num.Text = data.Count.ToString();
+      });
+
+      if (data.Connect)
+      {
+        //---
+      }
+      else
+      {
+        //---
+      }
+    }
+
+    private void VisualReceiveCallback(object o)
+    {
+      NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;
+
+      foreach (Drone d in AllDrones)
+      {
+        Drone.DataVisual v = d.GetVisual();
+
+        try { data.Client.Send(Drone.getBytes(v)); }
+        catch { }
+      }
+    }
+
+    private void button_Visual_Start_Click(object sender, EventArgs e)
+    {
+      var done = netServerVisual.StartServer((int)numericUpDown_Visual_Port.Value, (int)numericUpDown_Visual_Limit.Value, VisualConnectionCallback, VisualReceiveCallback);
+      switch (done)
+      {
+        case NetServerVisual.ServerState.Error:
+          {
+            MessageBox.Show("Error to start visual server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
+            break;
+          }
+        case NetServerVisual.ServerState.Start:
+          {
+            button_Visual_Start.Text = "Stop";
+            button_Visual_Start.BackColor = Color.LimeGreen;
+            break;
+          }
+        case NetServerVisual.ServerState.Stop:
+          {
+            label_Visual_Num.Text = "0";
+            button_Visual_Start.Text = "Start";
+            button_Visual_Start.BackColor = Color.Transparent;
+            break;
+          }
+      }
+    }
+  }
+
 }