===

DroneSimulator/GPS.cs

@@ -3,25 +3,72 @@ 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
-  {
-    public struct Home
+    internal class GPS
     {
-      public static double Lat, Lon;
-      public static float Alt;
-    }
+        static double PI = 3.14159265358979323846;
+        public struct Home
+        {
+            public static double Lat, Lon;
+            public static float Alt;
+        }
 
-    public struct State
-    {
-      public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
-      public static byte SatVisible; // Количество видимых спутников
-      public static byte SatUsed; // Количество используемых спутников
-      public static float Hdop, Vdop, Pdop; // Геометрический фактор
-      public static float Noise; // Шум (db)
+        public struct State
+        {
+            public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
+            public static byte SatVisible; // Количество видимых спутников
+            public static byte SatUsed; // Количество используемых спутников
+            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;
+        }
     }
-  }
 }