===

DroneSimulator/Drone.cs

@@ -446,6 +446,16 @@ namespace DroneSimulator
       gps.Head.Type = DroneData.DataType.DataGPS;
       gps.Head.Time = (uint)Environment.TickCount;
 
+      GPS.Point p = new GPS.Point();
+      p.x = PosXYZ.Y; p.y= PosXYZ.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;
 

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;
+        }
     }
-  }
 }