===

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