CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Mag active

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-07-26 02:35:15 +03:00
parent 65011e65ee
commit bb32111f29
4 changed files with 732 additions and 670 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
DroneSimulator/GPS.cs
View File

@ -8,67 +8,67 @@ using System.Threading.Tasks;
namespace DroneSimulator
{
internal class GPS
internal class GPS
{
static double PI = 3.14159265358979323846;
public struct Home
{
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;
}
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;
}
}
}