485 lines
19 KiB
C#
485 lines
19 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class GPSModel
|
|
{
|
|
public DateTime Momento { get; set; }
|
|
public CanCommandFreqModel<double> TimestampPos { get; set; } = new CanCommandFreqModel<double>();
|
|
public CanCommandFreqModel<double> TimestampOri { get; set; } = new CanCommandFreqModel<double>();
|
|
public bool Inicializado { get; set; }
|
|
public bool Ntrip_ativado { get; set; }
|
|
public double Latitude { get; set; }
|
|
public double Longitude { get; set; }
|
|
public DateTime DataHora { get; set; }
|
|
public double Altitude { get; set; }
|
|
public double Velocidade { get; set; }
|
|
public double Distancia { get; set; }
|
|
public double OrientacaoMovimento { get; set; }
|
|
public int NumeroSatelites { get; set; }
|
|
//public CanCommandFreqModel<int> NumeroSatelites { get; set; } = new CanCommandFreqModel<int>();
|
|
public double PrecisaoHorizontal { get; set; }
|
|
public double CursoVerdadeiro { get; set; }
|
|
public double OrientacaoReal { get; set; }
|
|
public string TipoOrientacao { get; set; }
|
|
public double VariacaoMagnetica { get; set; }
|
|
public TiposCorrecaoGPS QualidadeFix { get; set; }
|
|
public double IdadeCorrecao { get; set; }
|
|
public double PDOP { get; set; }
|
|
public double VDOP { get; set; }
|
|
public TiposDimensaoCorrecaoGPS FixDimensao { get; set; }
|
|
public double PrecisaoCm
|
|
{
|
|
get
|
|
{
|
|
double precisao = 0;
|
|
|
|
double erroBase =
|
|
QualidadeFix == TiposCorrecaoGPS.Autonomo ? 10.0 : // Autônomo: ~10 metros
|
|
QualidadeFix == TiposCorrecaoGPS.DGPS ? 3.0 : // DGPS: ~1-3 metros
|
|
QualidadeFix == TiposCorrecaoGPS.PPS ? 1.0 : // PPS: ~50 cm - 1 metro
|
|
QualidadeFix == TiposCorrecaoGPS.RTKFlutuante ? 0.5 : // RTK Float: ~20-50 cm
|
|
QualidadeFix == TiposCorrecaoGPS.RTKFixo ? 0.01 : // RTK Fixed: ~1-2 cm
|
|
1; // Valor padrão para casos desconhecidos
|
|
|
|
// Calcula a precisão horizontal em metros e converte para centímetros
|
|
precisao = erroBase * PrecisaoHorizontal * 100;
|
|
|
|
return precisao;
|
|
}
|
|
}
|
|
public List<GPSSatelitesEmVistaModel> SatelitesEmVista { get; set; } = new List<GPSSatelitesEmVistaModel>();
|
|
public DateTime UltimoComandoRespondido { get; set; }
|
|
public double AnguloCarroDefinido { get; set; }
|
|
public int Heartbeat { get; set; } = 0;
|
|
|
|
public GPSModel Clone()
|
|
{
|
|
return new GPSModel()
|
|
{
|
|
TimestampPos = TimestampPos.Clone(),
|
|
TimestampOri = TimestampOri.Clone(),
|
|
Ntrip_ativado = Ntrip_ativado,
|
|
NumeroSatelites = NumeroSatelites,
|
|
Altitude = Altitude,
|
|
DataHora = DataHora,
|
|
Latitude = Latitude,
|
|
Longitude = Longitude,
|
|
PrecisaoHorizontal = PrecisaoHorizontal,
|
|
Velocidade = Velocidade,
|
|
CursoVerdadeiro = CursoVerdadeiro,
|
|
Distancia = Distancia,
|
|
Inicializado = Inicializado,
|
|
Momento = Momento,
|
|
OrientacaoReal = OrientacaoReal,
|
|
VariacaoMagnetica = VariacaoMagnetica,
|
|
QualidadeFix = QualidadeFix,
|
|
SatelitesEmVista = new List<GPSSatelitesEmVistaModel>(SatelitesEmVista),
|
|
TipoOrientacao = TipoOrientacao,
|
|
UltimoComandoRespondido = UltimoComandoRespondido,
|
|
OrientacaoMovimento = OrientacaoMovimento,
|
|
AnguloCarroDefinido = AnguloCarroDefinido,
|
|
Heartbeat = Heartbeat,
|
|
IdadeCorrecao = IdadeCorrecao,
|
|
FixDimensao = FixDimensao,
|
|
PDOP = PDOP,
|
|
VDOP = VDOP
|
|
};
|
|
}
|
|
}
|
|
|
|
public class GPSSatelitesEmVistaModel
|
|
{
|
|
public string TipoSistema { get; set; }
|
|
public List<GPSSatelitesEmVistaSentencaModel> Sentencas { get; set; } = new List<GPSSatelitesEmVistaSentencaModel>();
|
|
}
|
|
|
|
public class GPSSatelitesEmVistaSentencaModel
|
|
{
|
|
public int SentencaAtual { get; set; }
|
|
public int SentencasTotal { get; set; }
|
|
public int QuantidadeSatelites { get; set; }
|
|
public List<GPSSatelitesEmVistaDadosModel> Dados { get; set; } = new List<GPSSatelitesEmVistaDadosModel>();
|
|
}
|
|
|
|
public class GPSSatelitesEmVistaDadosModel
|
|
{
|
|
public string PRN { get; set; }
|
|
public double Elevacao { get; set; }
|
|
public double Azimute { get; set; }
|
|
public double QualidadeSinal { get; set; }
|
|
}
|
|
|
|
public class GPSUtils
|
|
{
|
|
public static double RaioDaTerra = 6378137.0; // Raio da Terra em Km
|
|
public static double ConversaoNosKmh = 1.852;
|
|
public static double ConversaoNosMs = 0.514;
|
|
|
|
public static double CalcularOrientacao(GPSModel P1, GPSModel P2)
|
|
{
|
|
// Converter latitudes e longitudes de graus para radianos
|
|
double latA = P1.Latitude * (Math.PI / 180.0);
|
|
double lonA = P1.Longitude * (Math.PI / 180.0);
|
|
double latB = P2.Latitude * (Math.PI / 180.0);
|
|
double lonB = P2.Longitude * (Math.PI / 180.0);
|
|
|
|
// Calcular a diferença de longitude
|
|
double deltaLon = lonB - lonA;
|
|
|
|
// Calcular a direção
|
|
double y = Math.Sin(deltaLon) * Math.Cos(latB);
|
|
double x = Math.Cos(latA) * Math.Sin(latB) - Math.Sin(latA) * Math.Cos(latB) * Math.Cos(deltaLon);
|
|
double direcaoRadianos = Math.Atan2(y, x);
|
|
|
|
// Converter a direção de radianos para graus
|
|
double direcaoGraus = direcaoRadianos * (180.0 / Math.PI);
|
|
|
|
// Normalizar a direção para que esteja no intervalo de 0 a 360 graus
|
|
direcaoGraus = (direcaoGraus + 360) % 360;
|
|
|
|
return direcaoGraus;
|
|
}
|
|
|
|
public static double CalcularMenorDistanciaAteTrecho(GPSModel posicaoRobo, List<GPSModel> rua, double espacoEntrePontos = 0.2)
|
|
{
|
|
double menorDistancia = double.MaxValue;
|
|
|
|
for (int i = 0; i < rua.Count - 1; i++)
|
|
{
|
|
GPSModel pontoA = rua[i];
|
|
GPSModel pontoB = rua[i + 1];
|
|
|
|
// Comprimento do segmento em metros
|
|
double distanciaSegmento = DistanciaEntrePontos(pontoA, pontoB);
|
|
|
|
// Quantidade de pontos a interpolar
|
|
int numInterpolacoes = (int)(distanciaSegmento / espacoEntrePontos);
|
|
|
|
for (int j = 0; j <= numInterpolacoes; j++)
|
|
{
|
|
double fator = numInterpolacoes == 0 ? 0 : ((double)j / numInterpolacoes);
|
|
GPSModel pontoInterpolado = InterpolarPonto(pontoA, pontoB, fator);
|
|
|
|
// Calcula a distância até o ponto interpolado
|
|
double distancia = DistanciaEntrePontos(posicaoRobo, pontoInterpolado);
|
|
menorDistancia = Math.Min(menorDistancia, distancia);
|
|
}
|
|
}
|
|
|
|
return menorDistancia;
|
|
}
|
|
|
|
public static double NormalizarAngulo(double angulo)
|
|
{
|
|
if (angulo < 0)
|
|
{
|
|
angulo += 360;
|
|
}
|
|
angulo %= 360;
|
|
return angulo;
|
|
}
|
|
|
|
public static double CalcularDiferencaAngulo(double anguloVariavel, double anguloRef)
|
|
{
|
|
// Calcula a diferença entre o ângulo variável e o ângulo de referência
|
|
double diferenca = anguloVariavel - anguloRef;
|
|
|
|
// Normaliza a diferença para estar entre -180 e 180 graus
|
|
diferenca = ((diferenca + 180) % 360) - 180;
|
|
|
|
// Se a diferença ainda estiver fora do intervalo, ajuste manualmente
|
|
if (diferenca > 180)
|
|
{
|
|
diferenca -= 360;
|
|
}
|
|
else if (diferenca < -180)
|
|
{
|
|
diferenca += 360;
|
|
}
|
|
|
|
return diferenca;
|
|
}
|
|
|
|
public static double DistanciaEntrePontos(GPSModel P1, GPSModel P2)
|
|
{
|
|
double lat1 = P1.Latitude;
|
|
double lat2 = P2.Latitude;
|
|
double lon1 = P1.Longitude;
|
|
double lon2 = P2.Longitude;
|
|
|
|
|
|
double dLat = FuncoesMatematicas.GrausParaRadianos(lat2 - lat1);
|
|
double dLon = FuncoesMatematicas.GrausParaRadianos(lon2 - lon1);
|
|
lat1 = FuncoesMatematicas.GrausParaRadianos(lat1);
|
|
lat2 = FuncoesMatematicas.GrausParaRadianos(lat2);
|
|
|
|
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
|
|
Math.Sin(dLon / 2) * Math.Sin(dLon / 2) * Math.Cos(lat1) * Math.Cos(lat2);
|
|
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
|
double distance = RaioDaTerra * c;
|
|
|
|
//distance = distance * 0.5;
|
|
|
|
return distance;
|
|
}
|
|
|
|
public static double DistanciaDoTrecho(List<GPSModel> Trecho)
|
|
{
|
|
double d = 0;
|
|
for (int i = 0; i < Trecho.Count - 1; i++)
|
|
{
|
|
double _d = DistanciaEntrePontos(Trecho[i], Trecho[i + 1]);
|
|
//Console.WriteLine($"{i} - {_d}");
|
|
d += _d;
|
|
}
|
|
return d;
|
|
}
|
|
|
|
public static GPSModel ProjetarPontoDeslocado(GPSModel pontoOriginal, double distancia, double angulo)
|
|
{
|
|
// Convertendo ângulo em radianos
|
|
double anguloRad = (Math.PI / 180) * angulo;
|
|
|
|
// Calculando deslocamento
|
|
double deltaLat = (distancia * Math.Cos(anguloRad)) / RaioDaTerra;
|
|
double deltaLon = (distancia * Math.Sin(anguloRad)) / (RaioDaTerra * Math.Cos(pontoOriginal.Latitude * Math.PI / 180));
|
|
|
|
// Convertendo deslocamento de radianos para graus
|
|
deltaLat = deltaLat * (180 / Math.PI);
|
|
deltaLon = deltaLon * (180 / Math.PI);
|
|
|
|
// Criando novo ponto com o deslocamento
|
|
GPSModel pontoDeslocado = new GPSModel
|
|
{
|
|
Latitude = pontoOriginal.Latitude + deltaLat,
|
|
Longitude = pontoOriginal.Longitude + deltaLon,
|
|
Altitude = pontoOriginal.Altitude,
|
|
DataHora = pontoOriginal.DataHora,
|
|
NumeroSatelites = pontoOriginal.NumeroSatelites,
|
|
PrecisaoHorizontal = pontoOriginal.PrecisaoHorizontal,
|
|
Velocidade = pontoOriginal.Velocidade
|
|
};
|
|
|
|
return pontoDeslocado;
|
|
}
|
|
|
|
public static GPSModel InterpolarPonto(GPSModel pontoInicial, GPSModel pontoFinal, double t)
|
|
{
|
|
// Interpolar latitude e longitude
|
|
double latInterpolada = pontoInicial.Latitude + t * (pontoFinal.Latitude - pontoInicial.Latitude);
|
|
double lonInterpolada = pontoInicial.Longitude + t * (pontoFinal.Longitude - pontoInicial.Longitude);
|
|
|
|
// Interpolar orientação, se necessário
|
|
double orientacaoInterpolada = pontoInicial.OrientacaoReal + t * (pontoFinal.OrientacaoReal - pontoInicial.OrientacaoReal);
|
|
|
|
return new GPSModel
|
|
{
|
|
Latitude = latInterpolada,
|
|
Longitude = lonInterpolada,
|
|
CursoVerdadeiro = orientacaoInterpolada
|
|
};
|
|
}
|
|
|
|
public static List<GPSModel> InterpolarRota(List<GPSModel> rotaOriginal, int novaQuantidadeDePontos)
|
|
{
|
|
var rotaInterpolada = new List<GPSModel>();
|
|
int quantidadeOriginal = rotaOriginal.Count;
|
|
|
|
for (int i = 0; i < novaQuantidadeDePontos; i++)
|
|
{
|
|
// Proporção do ponto na rota original
|
|
double t = (double)i / (novaQuantidadeDePontos - 1);
|
|
double indiceInterpolado = t * (quantidadeOriginal - 1);
|
|
|
|
int indiceInferior = (int)Math.Floor(indiceInterpolado);
|
|
int indiceSuperior = Math.Min(indiceInferior + 1, quantidadeOriginal - 1);
|
|
|
|
double proporcao = indiceInterpolado - indiceInferior;
|
|
|
|
var pontoInterpolado = InterpolarPonto(
|
|
rotaOriginal[indiceInferior],
|
|
rotaOriginal[indiceSuperior],
|
|
proporcao);
|
|
|
|
rotaInterpolada.Add(pontoInterpolado);
|
|
}
|
|
|
|
return rotaInterpolada;
|
|
}
|
|
|
|
public static GPSModel PontoMedio(GPSModel P1, GPSModel P2)
|
|
{
|
|
return new GPSModel()
|
|
{
|
|
Latitude = (P1.Latitude + P2.Latitude) / 2,
|
|
Longitude = (P1.Longitude + P2.Longitude) / 2,
|
|
};
|
|
}
|
|
|
|
public static GPSModel PontoMedioComOffset(GPSModel P1, GPSModel P2, double deslocamentoLateralGPS)
|
|
{
|
|
// Calcular o ponto médio padrão
|
|
var pontoMedio = new GPSModel()
|
|
{
|
|
Latitude = (P1.Latitude + P2.Latitude) / 2,
|
|
Longitude = (P1.Longitude + P2.Longitude) / 2,
|
|
};
|
|
|
|
// Ajustar o ponto médio com o deslocamento lateral
|
|
var anguloEntrePontos = CalcularOrientacao(P1, P2); // Angulo entre os dois pontos
|
|
var anguloPerpendicular = anguloEntrePontos - 90; // Perpendicular à direção da rua
|
|
|
|
// Aplicar deslocamento lateral
|
|
return MoverPonto(pontoMedio, deslocamentoLateralGPS, anguloPerpendicular);
|
|
}
|
|
|
|
public static GPSModel MoverPonto(GPSModel ponto, double deslocamento, double angulo)
|
|
{
|
|
// Converte o deslocamento de metros para graus (aproximação)
|
|
double deslocamentoLat = deslocamento * Math.Cos(angulo * Math.PI / 180) / 111320.0;
|
|
double deslocamentoLon = deslocamento * Math.Sin(angulo * Math.PI / 180) / (111320.0 * Math.Cos(ponto.Latitude * Math.PI / 180));
|
|
|
|
return new GPSModel()
|
|
{
|
|
Latitude = ponto.Latitude + deslocamentoLat,
|
|
Longitude = ponto.Longitude + deslocamentoLon,
|
|
};
|
|
}
|
|
|
|
public static GPSModel GerarPontoDeslocado(GPSModel pontoInicial, double angulo, double distancia)
|
|
{
|
|
double latitude = pontoInicial.Latitude;
|
|
double longitude = pontoInicial.Longitude;
|
|
|
|
// Conversão de ângulo de graus para radianos
|
|
double anguloRad = angulo * (Math.PI / 180);
|
|
|
|
// Raio da Terra em metros
|
|
double raioTerra = RaioDaTerra;
|
|
|
|
// Calcular deslocamento de latitude em radianos
|
|
double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
|
|
// Converter de radianos para graus
|
|
double latAtt = latitude + deltaLat * (180 / Math.PI);
|
|
|
|
// Calcular deslocamento de longitude em radianos
|
|
double deltaLong = distancia * Math.Sin(anguloRad) / (raioTerra * Math.Cos(latitude * (Math.PI / 180)));
|
|
// Converter de radianos para graus
|
|
double longAtt = longitude + deltaLong * (180 / Math.PI);
|
|
|
|
return new GPSModel()
|
|
{
|
|
Latitude = latAtt,
|
|
Longitude = longAtt,
|
|
};
|
|
}
|
|
|
|
public static (double x, double y) ConverterLatLongParaMetros(GPSModel ponto, GPSModel referencia)
|
|
{
|
|
double lat1 = FuncoesMatematicas.GrausParaRadianos(referencia.Latitude);
|
|
double lon1 = FuncoesMatematicas.GrausParaRadianos(referencia.Longitude);
|
|
double lat2 = FuncoesMatematicas.GrausParaRadianos(ponto.Latitude);
|
|
double lon2 = FuncoesMatematicas.GrausParaRadianos(ponto.Longitude);
|
|
|
|
double dLat = lat2 - lat1;
|
|
double dLon = lon2 - lon1;
|
|
|
|
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
|
|
Math.Cos(lat1) * Math.Cos(lat2) *
|
|
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
|
|
|
|
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
|
|
|
double distancia = RaioDaTerra * c;
|
|
|
|
// Calcular coordenadas x e y relativas ao ponto de referência
|
|
double x = distancia * Math.Cos(lat1) * Math.Sin(dLon);
|
|
double y = distancia * Math.Sin(dLat);
|
|
|
|
return (x, y);
|
|
}
|
|
|
|
public static double ConvertToDecimalDegrees(string nmeaCoordinate, string direction, int digits)
|
|
{
|
|
// Divide a string em graus e minutos
|
|
int degrees = int.Parse(nmeaCoordinate.Substring(0, digits));
|
|
double minutes = double.Parse(nmeaCoordinate.Substring(digits), CultureInfo.InvariantCulture);
|
|
|
|
// Converte para graus decimais
|
|
double decimalDegrees = degrees + (minutes / 60);
|
|
|
|
// Se a direção for Sul ou Oeste, o valor deve ser negativo
|
|
if (direction == "S" || direction == "W")
|
|
{
|
|
decimalDegrees *= -1;
|
|
}
|
|
|
|
return decimalDegrees;
|
|
}
|
|
|
|
public static double MisturarAngulosPorDistancia(double anguloFrente, double anguloMovimento, double distanciaMetros, double distanciaMaxima, double distanciaMinima = 0.05, double pesoMinFrente = 0.2)
|
|
{
|
|
// Se robô praticamente não se moveu, confia só na orientação
|
|
if (distanciaMetros < distanciaMinima)
|
|
return anguloFrente;
|
|
|
|
double fator = Math.Min(distanciaMetros / distanciaMaxima, 1.0);
|
|
|
|
double pesoFrente = (1.0 - fator) * (1.0 - pesoMinFrente) + pesoMinFrente;
|
|
|
|
return MisturarAngulos(anguloFrente, anguloMovimento, pesoFrente);
|
|
}
|
|
|
|
public static double MisturarAngulos(double angulo1, double angulo2, double pesoAngulo1 = 0.5)
|
|
{
|
|
// Converte os ângulos para vetores unitários no plano
|
|
double rad1 = angulo1 * Math.PI / 180.0;
|
|
double rad2 = angulo2 * Math.PI / 180.0;
|
|
|
|
double x = Math.Cos(rad1) * pesoAngulo1 + Math.Cos(rad2) * (1.0 - pesoAngulo1);
|
|
double y = Math.Sin(rad1) * pesoAngulo1 + Math.Sin(rad2) * (1.0 - pesoAngulo1);
|
|
|
|
double resultado = Math.Atan2(y, x) * 180.0 / Math.PI;
|
|
|
|
return GPSUtils.NormalizarAngulo(resultado);
|
|
}
|
|
|
|
public static (GPSModel ponto, int index) PontoMaisProximoTrechoComIndice(GPSModel pontoRef, List<GPSModel> trecho)
|
|
{
|
|
if (trecho == null || trecho.Count == 0)
|
|
return (null, -1);
|
|
|
|
var resultado = trecho
|
|
.Select((p, i) => (ponto: p, index: i, dist: DistanciaEntrePontos(pontoRef, p)))
|
|
.OrderBy(x => x.dist)
|
|
.First();
|
|
|
|
return (resultado.ponto, resultado.index);
|
|
}
|
|
|
|
public static double DmmToDecimal(string dmm, int degLen)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(dmm)) return double.NaN;
|
|
// NMEA: ddmm.mmmm (lat) / dddmm.mmmm (lon)
|
|
double deg = double.Parse(dmm.Substring(0, degLen), CultureInfo.InvariantCulture);
|
|
double min = double.Parse(dmm.Substring(degLen), CultureInfo.InvariantCulture);
|
|
return deg + (min / 60.0);
|
|
}
|
|
|
|
public static double ParseDouble(string s) =>
|
|
double.TryParse(s, NumberStyles.Float, CultureInfo.InvariantCulture, out var v) ? v : double.NaN;
|
|
|
|
public static int ParseInt(string s) =>
|
|
int.TryParse(s, NumberStyles.Integer, CultureInfo.InvariantCulture, out var v) ? v : 0;
|
|
|
|
|
|
}
|
|
|
|
}
|