75 lines
2.5 KiB
C#
75 lines
2.5 KiB
C#
using System;
|
||
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;
|
||
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 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;
|
||
}
|
||
}
|
||
}
|