Update Drone.cs

DroneSimulator/Drone.cs

@@ -1,4 +1,6 @@
-using System.Numerics;
+using System.Collections.Concurrent;
+using System.Diagnostics;
+using System.Numerics;
 using System.Runtime.InteropServices;
 using static System.Net.Mime.MediaTypeNames;
 
@@ -7,14 +9,13 @@ namespace DroneSimulator
   internal class Drone
   {
     public int ID;
-    public float Mass; // Масса
+
     public bool Active; // Живой?
-    public float Length; // Длинна лучей
     public const float Dynamic = 10; // Динамика вращения
     public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
     public Quaternion Quat; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей (0-1)
-    public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
+    
     public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
 
     public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
@@ -29,17 +30,31 @@ namespace DroneSimulator
     private const float TO_GRAD = 180 / MathF.PI;
     private const float TO_RADI = MathF.PI / 180;
 
-    private Thread DroneThread;
-    private int Timer;
+    public static List<Drone> AllDrones = new List<Drone>();
+    private static Thread? DroneThread = null;
+    public static long Timing = 0;
+    public static long Lag = 0;
+    public static long Freq = 1000;
+    public static bool Boost = false;
+
+    private uint Timer;
 
     private Vector2 MoveOF = Vector2.Zero;
+    private uint CountOF = 0;
 
-    RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
-    RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
-    RealMode.Position RealPos = new RealMode.Position();
-    RealMode.Barometer RealBar = new RealMode.Barometer();
-    RealMode.Range RealRange = new RealMode.Range();
-    RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
+    public struct Physics
+    {
+      static public float Mass; // Масса
+      static public float Length; // Длинна лучей
+      static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
+    }
+
+    private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
+    private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
+    private RealMode.Position RealPos = new RealMode.Position();
+    private RealMode.Barometer RealBar = new RealMode.Barometer();
+    private RealMode.Range RealRange = new RealMode.Range();
+    private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
 
     public static byte[] getBytes(object data)
     {
@@ -106,12 +121,49 @@ namespace DroneSimulator
       return drone;
     }
 
-    private void ThreadFunction()
+    public static void StartThread()
     {
+      if (DroneThread != null) return;
+
+      DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
+      DroneThread.Priority = ThreadPriority.Highest;
+      DroneThread.Start();
+    }
+
+    public static void StopThread()
+    {
+      if (DroneThread == null) return;
+
+      DroneThread = null;
+    }
+
+    private static void ThreadFunction()
+    {
+      long last = Stopwatch.GetTimestamp();
+      long prev = Stopwatch.GetTimestamp();
+
       while (DroneThread != null)
       {
-        Action(Environment.TickCount);
-        Thread.Sleep(1);
+        if(!Boost) Thread.Yield();
+
+        long tick = Stopwatch.GetTimestamp();
+
+        if (tick == prev) continue;
+
+        Timing = Stopwatch.Frequency / (tick - prev);
+        prev = tick;
+
+        long quant = Stopwatch.Frequency / Freq;
+
+        if (tick < last + quant) continue;
+
+        if (tick > (last + quant) + quant * 0.1) Lag++;
+
+        last = tick;
+
+        lock (AllDrones)
+          foreach (Drone drone in AllDrones)
+            drone.Action((uint)(tick / (Stopwatch.Frequency / 1000)));
       }
     }
 
@@ -123,28 +175,15 @@ namespace DroneSimulator
       SpdXYZ = Vector3.Zero;
       AccXYZ = Vector3.Zero;
       Quat = Quaternion.Identity;
-
-      DroneThread = new Thread(new ThreadStart(ThreadFunction));
-      Timer = Environment.TickCount;
-      DroneThread.Start();
     }
 
-    public int Create(float power, float mass, float len)
+    public int Create()
     {
-      Mass = mass;
-      Length = len;
-      MaxPower = power;
-
       Active = true;
 
       return ID;
     }
 
-    public void Close()
-    {
-      DroneThread = null;
-    }
-
     private float GetAngle(float a1, float a2, float az)
     {
       if (a2 == 0.0f && az == 0.0f)
@@ -186,9 +225,13 @@ 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(int tick)
+    public void Action(uint tick)
     {
-      float time = (tick - Timer) / 1000.0f;
+      uint period = tick - Timer;
+
+      if (period <= 0) return;
+
+      float time = period / 1000.0f;
       Timer = tick;
 
       if (!Active) return;
@@ -200,8 +243,6 @@ namespace DroneSimulator
         flow += flow * 0.1f; // Воздушная подушка
       }
 
-      
-
       float wind_x = 0, wind_y = 0, wind_z = 0;
       float wind_p = 0, wind_r = 0, wind_w = 0;
 
@@ -226,10 +267,10 @@ namespace DroneSimulator
         AccPRY.X -= wind_p; AccPRY.Y -= wind_r; AccPRY.Z -= wind_w;
       }
 
-      SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
+      SpdPRY += AccPRY * (Dynamic * time / (Physics.Mass * Physics.Length));
 
       Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
-      AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Mass);
+      AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Physics.Mass);
       SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
       PosXYZ += SpdXYZ * time;
 
@@ -239,39 +280,49 @@ namespace DroneSimulator
       if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
       if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
 
-      if (PosXYZ.Z < 0)
+      /*if (PosXYZ.Z < 0)
       {
         SpdPRY = Vector3.Zero;
         SpdXYZ.X = 0;
         SpdXYZ.Y = 0;
         Quat = Quaternion.Identity;
       }
-      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
+      else */
+        Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
 
       Vector4 ori = GetOrientation();
 
       Orientation = ori;
+      float range = 0;
 
-      if (PosXYZ.Z < 0)
+      if (PosXYZ.Z <= 0)
+      {
+        PosXYZ.Z = 0;
+        SpdXYZ.Z = 0;
+        LaserRange = 0;
+        Acc = new Vector3(0, 0, 1);
+      }
+
+      /*if (PosXYZ.Z < 0)
       {
         PosXYZ.Z = 0;
 
-        /*if (SpdXYZ.Z < -5)
-        {
-          Active = false; // Сильно ударился о землю
-        }*/
+        //if (SpdXYZ.Z < -5)
+        //{
+        //  Active = false; // Сильно ударился о землю
+        //}
 
-        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
-        {
-          Active = false; // Повредил винты при посадке
-        }*/
+        //if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
+        //{
+        //  Active = false; // Повредил винты при посадке
+        //}
 
         SpdXYZ.Z = 0;
 
         Acc = new Vector3(0, 0, 1);
         Gyr = Vector3.Zero;
         LaserRange = 0;
-      }
+      }*/
       else
       {
         if (ori.W < 0)
@@ -286,21 +337,28 @@ namespace DroneSimulator
 
         float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
 
-        if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
+        range = PosXYZ.Z / MathF.Cos(tilt);
+
+        if (tilt < 90 && ori.W > 0) LaserRange = range;
         else LaserRange = float.MaxValue;
       }
 
-      MoveOF.X += SpdXYZ.X - Gyr.Y;
-      MoveOF.Y += SpdXYZ.Y + Gyr.X;
+      RealAcc.Update(Acc, tick);
+      RealGyr.Update(Gyr, tick);
+      RealRange.Update(LaserRange, tick);
+      RealBar.Update(PosXYZ.Z, tick);
+      RealPos.Update(PosXYZ, tick);
 
-      RealAcc.Update(Acc, (uint)tick);
-      RealGyr.Update(Gyr, (uint)tick);
-      RealRange.Update(LaserRange, (uint)tick);
-      RealBar.Update(PosXYZ.Z * 11, (uint)tick);
-      RealPos.Update(PosXYZ, (uint)tick);
-      RealOF.Update(MoveOF, LaserRange, (uint)tick);
+      bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
 
-      DataTimer = (uint)tick;
+      if(of) lock (this)
+      {
+        MoveOF += RealOF.result;
+        CountOF += 1;
+      }
+      
+
+      DataTimer = tick;
     }
 
     private float Range(float pow)
@@ -308,7 +366,7 @@ namespace DroneSimulator
       if (pow > 1) pow = 1;
       if (pow < 0) pow = 0;
 
-      return pow * MaxPower;
+      return pow * Physics.MaxPower;
     }
 
     public void SetQadroPow(float ul, float ur, float dl, float dr)
@@ -336,7 +394,7 @@ namespace DroneSimulator
       acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
       acc.Head.Mode = DroneData.DataMode.Response;
       acc.Head.Type = DroneData.DataType.DataAcc;
-      acc.Head.Time = (uint)Environment.TickCount;
+      acc.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
       acc.Time = RealAcc.timer;
@@ -351,7 +409,7 @@ namespace DroneSimulator
       gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
       gyr.Head.Mode = DroneData.DataMode.Response;
       gyr.Head.Type = DroneData.DataType.DataGyr;
-      gyr.Head.Time = (uint)Environment.TickCount;
+      gyr.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
       gyr.Time = RealGyr.timer;
@@ -366,7 +424,7 @@ namespace DroneSimulator
       mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
       mag.Head.Mode = DroneData.DataMode.Response;
       mag.Head.Type = DroneData.DataType.DataMag;
-      mag.Head.Time = (uint)Environment.TickCount;
+      mag.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
       mag.Time = DataTimer;
@@ -381,7 +439,7 @@ namespace DroneSimulator
       range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
       range.Head.Mode = DroneData.DataMode.Response;
       range.Head.Type = DroneData.DataType.DataRange;
-      range.Head.Time = (uint)Environment.TickCount;
+      range.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       range.LiDAR = RealRange.result;
       range.Time = RealRange.timer;
@@ -396,7 +454,7 @@ namespace DroneSimulator
       local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
       local.Head.Mode = DroneData.DataMode.Response;
       local.Head.Type = DroneData.DataType.DataLocal;
-      local.Head.Time = (uint)Environment.TickCount;
+      local.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
       local.Time = RealPos.timer;
@@ -411,7 +469,7 @@ namespace DroneSimulator
       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.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       bar.Pressure = RealBar.result;
       bar.Time = RealBar.timer;
@@ -426,13 +484,17 @@ namespace DroneSimulator
       of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
       of.Head.Mode = DroneData.DataMode.Response;
       of.Head.Type = DroneData.DataType.DataOF;
-      of.Head.Time = (uint)Environment.TickCount;
+      of.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
-      of.X = RealOF.result.X;
-      of.Y = RealOF.result.Y;
-      of.Time = RealBar.timer;
+      lock (this)
+      {
+        of.X = MoveOF.X / CountOF;
+        of.Y = MoveOF.Y / CountOF;
+        of.Time = RealOF.timer;
 
         MoveOF = Vector2.Zero;
+        CountOF = 0;
+      }
 
       return getBytes(of);
     }
@@ -444,10 +506,20 @@ namespace DroneSimulator
       gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
       gps.Head.Mode = DroneData.DataMode.Response;
       gps.Head.Type = DroneData.DataType.DataGPS;
-      gps.Head.Time = (uint)Environment.TickCount;
+      gps.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      GPS.Point p = new GPS.Point();
+      p.x = RealPos.result.Y; p.y= RealPos.result.X;
+
+      GPS.GlobalCoords g = new GPS.GlobalCoords();
+      g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
+
+      g=GPS.localToGlobal(p, g);
+
+      gps.Lat = g.latitude; gps.Lon = g.longitude;
 
       gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
-      gps.Alt = GPS.Home.Alt + PosXYZ.Z;
+      gps.Alt = GPS.Home.Alt + RealPos.result.Z;
 
       DateTime tim = DateTime.Now;
       gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
@@ -460,6 +532,8 @@ namespace DroneSimulator
       gps.Vdop = GPS.State.Vdop;
       gps.Pdop = GPS.State.Pdop;
 
+      gps.Time = RealPos.timer;
+
       return getBytes(gps);
     }
 
@@ -470,13 +544,25 @@ namespace DroneSimulator
       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.Head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
 
       quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
 
       return getBytes(quat);
     }
 
+    private byte[] SendPingPong()
+    {
+      DroneData.DataHead head = new DroneData.DataHead();
+
+      head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
+      head.Mode = DroneData.DataMode.Response;
+      head.Type = DroneData.DataType.None;
+      head.Time = (uint)(Stopwatch.GetTimestamp() / (Stopwatch.Frequency / 1000));
+
+      return getBytes(head);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
       byte[] zero = Array.Empty<byte>();
@@ -516,6 +602,8 @@ namespace DroneSimulator
         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;
+
+        case DroneData.DataType.None: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); else return zero;
       }
 
       return zero;

DroneSimulator/FormMain.cs

@@ -17,8 +17,6 @@ namespace DroneSimulator
     NetServerClients netServerClient = new NetServerClients();
     NetServerVisual netServerVisual = new NetServerVisual();
 
-    List<Drone> AllDrones = new List<Drone>();
-
     public Form_Main()
     {
       InitializeComponent();
@@ -28,6 +26,11 @@ namespace DroneSimulator
       numericUpDown_Pos_Update(null, null);
       numericUpDown_Bar_Update(null, null);
       numericUpDown_Range_Update(null, null);
+      numericUpDown_OF_Update(null, null);
+      checkBox_Area_Freeze_CheckedChanged(null, null);
+      numericUpDown_Area_Wind_Update(null, null);
+      numericUpDown_GPS_ValueChanged(null, null);
+      numericUpDown_Physics_ValueChanged(null, null);
     }
 
     private void ClientConnectionCallback(object o)
@@ -42,25 +45,29 @@ namespace DroneSimulator
       if (data.Connect)
       {
         Drone drone = new Drone(data.ID);
-        drone.Create(1.0f, 0.5f, 1.0f);
+        drone.Create();
 
         screen2D.CreateDrone(Color.Red, data.ID);
 
-        AllDrones.Add(drone);
+        lock (Drone.AllDrones) Drone.AllDrones.Add(drone);
       }
       else
       {
-        foreach (Drone drone in AllDrones)
+        Drone? d = null;
+
+        lock (Drone.AllDrones)
+        {
+          foreach (Drone drone in Drone.AllDrones)
           {
             if (drone.ID != data.ID) continue;
-          drone.Close();
-
-          screen2D.RemoveDrone(data.ID);
-
-          AllDrones.Remove(drone);
+            d = drone;
+            Drone.AllDrones.Remove(drone);
             break;
           }
         }
+
+        if (d != null) screen2D.RemoveDrone(d.ID);
+      }
     }
 
     private void ClientReceiveCallback(object o)
@@ -69,12 +76,15 @@ namespace DroneSimulator
 
       Drone? drone = null;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
+      {
+        foreach (Drone d in Drone.AllDrones)
         {
           if (d.ID != data.ID) continue;
           drone = d;
           break;
         }
+      }
 
       if (drone == null) return;
 
@@ -118,6 +128,8 @@ namespace DroneSimulator
 
       if (done != NetServerClients.ServerState.Start) return;
 
+      Drone.StartThread();
+
       pictureBox_2D.Image = null;
 
       screen2D = new Screen2D(DrawCallback);
@@ -150,7 +162,9 @@ namespace DroneSimulator
 
       try
       {
-        foreach (Drone d in AllDrones)
+        lock (Drone.AllDrones)
+        {
+          foreach (Drone d in Drone.AllDrones)
           {
             screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
 
@@ -159,14 +173,14 @@ namespace DroneSimulator
             listBox_Drones.Items.Add(line);
           }
         }
+      }
       catch { }
 
+      label_Timing.Text = Drone.Timing.ToString() + " Hz";
+      label_Timing_Lag.Text = Drone.Lag.ToString();
+
       screen2D.DrawScene();
     }
-    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
-    {
-      foreach (Drone d in AllDrones) d.Close();
-    }
 
     private void VisualConnectionCallback(object o)
     {
@@ -174,7 +188,7 @@ namespace DroneSimulator
 
       Invoke((MethodInvoker)delegate
       {
-        label_Clients_Num.Text = data.Count.ToString();
+        label_Visual_Num.Text = data.Count.ToString();
       });
 
       if (data.Connect)
@@ -193,14 +207,17 @@ namespace DroneSimulator
 
       int index = 0;
 
-      foreach (Drone d in AllDrones)
+      lock (Drone.AllDrones)
       {
-        VisualData.VisualDrone v = d.GetVisual(AllDrones.Count, index++);
+        foreach (Drone d in Drone.AllDrones)
+        {
+          VisualData.VisualDrone v = d.GetVisual(Drone.AllDrones.Count, index++);
 
           try { data.Client.Send(Drone.getBytes(v)); }
           catch { }
         }
       }
+    }
 
     private void button_Visual_Start_Click(object sender, EventArgs e)
     {
@@ -301,11 +318,8 @@ namespace DroneSimulator
       RealMode.OpticalFlow.Lateness = (float)numericUpDown_OF_Laten.Value;
       RealMode.OpticalFlow.Enable = checkBox_OF_Enable.Checked;
 
-      RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10;
+      RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value;
       RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
-
-      RealMode.OpticalFlow.Error = (float)numericUpDown_OF_Error.Value * 10;
-      RealMode.OpticalFlow.Wait = (uint)numericUpDown_OF_Wait.Value * 1000;
     }
 
     private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
@@ -330,7 +344,7 @@ namespace DroneSimulator
     {
       GPS.Home.Lat = (double)numericUpDown_GPS_Lat.Value;
       GPS.Home.Lon = (double)numericUpDown_GPS_Lon.Value;
-      GPS.Home.Alt = (double)numericUpDown_GPS_Alt.Value;
+      GPS.Home.Alt = (float)numericUpDown_GPS_Alt.Value;
 
       GPS.State.Fix = (byte)numericUpDown_GPS_Fix.Value;
       GPS.State.SatVisible = (byte)numericUpDown_GPS_Vis.Value;
@@ -340,5 +354,23 @@ namespace DroneSimulator
       GPS.State.Vdop = (float)numericUpDown_GPS_VDOP.Value;
       GPS.State.Pdop = (float)numericUpDown_GPS_PDOP.Value;
     }
+
+    private void numericUpDown_Physics_ValueChanged(object sender, EventArgs e)
+    {
+      Drone.Physics.Mass = (float)numericUpDown_Physics_Mass.Value;
+      Drone.Physics.Length = (float)numericUpDown_Physics_Length.Value;
+      Drone.Physics.MaxPower = (float)numericUpDown_Physics_Power.Value;
+    }
+
+    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
+    {
+      Drone.StopThread();
+    }
+
+    private void numericUpDown_Timing_Freq_ValueChanged(object sender, EventArgs e)
+    {
+      Drone.Freq = (long)numericUpDown_Timing_Freq.Value;
+      Drone.Boost = checkBox_Freq_Boost.Checked;
+    }
   }
 }

DroneSimulator/FormMain.resx

@@ -117,9 +117,6 @@
   <resheader name="writer">
     <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </resheader>
-  <metadata name="menuStrip_Menu.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
-    <value>17, 17</value>
-  </metadata>
   <metadata name="timer_Test.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
     <value>162, 5</value>
   </metadata>

DroneSimulator/GPS.cs

@@ -3,12 +3,14 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Numerics;
 using System.Text;
+using System.Threading;
 using System.Threading.Tasks;
 
 namespace DroneSimulator
 {
     internal class GPS
     {
+        static double PI = 3.14159265358979323846;
         public struct Home
         {
             public static double Lat, Lon;
@@ -23,5 +25,50 @@ namespace DroneSimulator
             public static float Hdop, Vdop, Pdop; // Геометрический фактор
             public static float Noise; // Шум (db)
         }
+
+        public struct GlobalCoords
+        {
+            public double latitude, longitude;
+        }
+
+        public struct Point
+        {
+            public double x, y;
+        }
+
+        // Конвертация градусов в радианы
+        static double deg2rad(double deg)
+        {
+            return deg * PI / 180.0;
+        }
+
+        // Конвертация радиан в градусы
+        static double rad2deg(double rad)
+        {
+            return rad * 180.0 / PI;
+        }
+
+        // Перевод локальных координат в глобальные
+        public static GlobalCoords localToGlobal(Point local, GlobalCoords origin)
+        {
+            const double er = 6371000; // Radius of the earth in m
+
+            // Преобразование приращений координат
+            double dLat = local.x / er; // В радианах
+            double originLatRad = deg2rad(origin.latitude);
+
+            // Вычисление новой широты
+            double newLatRad = originLatRad + dLat;
+            double newLat = rad2deg(newLatRad);
+
+            // Вычисление новой долготы (с использованием средней широты для точности)
+            double avgLatRad = (originLatRad + newLatRad) / 2.0;
+            double dLon = local.y / (er * Math.Cos(avgLatRad)); // В радианах
+            double newLon = origin.longitude + rad2deg(dLon);
+            GlobalCoords coord = new GlobalCoords();
+            coord.latitude = newLat;
+            coord.longitude = newLon;
+            return coord;
+        }
     }
 }

DroneSimulator/RealMode.cs

@@ -225,7 +225,7 @@ namespace DroneSimulator
 
       public void Update(float value, uint time)
       {
-        value = Pressure - value;
+        value = Pressure - value * 12.15f;
 
         if (!Enable)
         {
@@ -273,8 +273,6 @@ namespace DroneSimulator
       public static uint Freq;
       public static float Noise;
       public static float Lateness;
-      public static float Error;
-      public static uint Wait;
       public static float Lens;
       public static bool RealSimulation;
 
@@ -290,31 +288,21 @@ namespace DroneSimulator
 
       public uint timer = 0;
       public Vector2 result;
-      public void Update(Vector2 value, float Range, uint time)
+      public bool Update(Vector2 value, float Range, uint time)
       {
+        value *= Lens;
+
         if (!Enable)
         {
           result = Vector2.NaN;
-          return;
+          return true;
         }
 
         if (!RealSimulation)
         {
           result = value;
           timer = time;
-          return;
-        }
-
-        value *= Lens;
-
-        if (rand.Next(0, 1000) < (Error * 10))
-        {
-          value = Vector2.Zero;
-          delay = time + Wait;
-        }
-        else if (delay > time)
-        {
-          value = Vector2.Zero;
+          return true;
         }
 
         if (Range > MaxHeight) value = Vector2.Zero;
@@ -344,7 +332,10 @@ namespace DroneSimulator
         {
           result = value;
           timer = time;
+          return true;
         }
+
+        return false;
       }
     }