1641 lines
68 KiB
C#
1641 lines
68 KiB
C#
using AgroBase.Models;
|
||
using Newtonsoft.Json;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Globalization;
|
||
using System.IO;
|
||
using System.IO.Ports;
|
||
using System.Linq;
|
||
using System.Net.Sockets;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using static AgroBase.Models.Enums;
|
||
|
||
namespace AgroBase.Services
|
||
{
|
||
public class GPSService
|
||
{
|
||
public static SerialPort PortaGPS = null;
|
||
public static bool Iniciado
|
||
{
|
||
get
|
||
{
|
||
if (PortaGPS != null && !PortaGPS.IsOpen)
|
||
{
|
||
try
|
||
{
|
||
PortaGPS.Open();
|
||
}
|
||
catch
|
||
{
|
||
PortaGPS = null;
|
||
}
|
||
}
|
||
return PortaGPS != null && PortaGPS.IsOpen;
|
||
}
|
||
}
|
||
|
||
public static GPSModel UltimaLeitura = new GPSModel();
|
||
public static GPSModel PenultimaLeitura = new GPSModel();
|
||
public static List<GPSModel> UltimasLeituras = new List<GPSModel>();
|
||
public static List<string> Logs = new List<string>();
|
||
|
||
public static int TaxaAmostragemHz { get; set; } = 5;
|
||
|
||
private static bool InverterHeading = true;
|
||
private static int rtk_timeout = 60;
|
||
private static int TempoMin_Ntrip = 10;
|
||
private static bool LoopRTK_Ntrip = false;
|
||
public static bool CorrecaoRTK_Ntrip = false;
|
||
public static DateTime UltimoEnvioCorrecaoRTK = DateTime.MinValue;
|
||
public static GeoLeverArm LeverArm = new GeoLeverArm(offsetFisicoFrontalCm: VariaveisEquipamento.LeverArmFrontalCm, offsetFisicoLateralCm: VariaveisEquipamento.LeverArmLateralCm);
|
||
|
||
public static void AtualizarPortaCOM(SerialPort Porta)
|
||
{
|
||
if (PortaGPS == null)
|
||
{
|
||
PortaGPS = new SerialPort();
|
||
}
|
||
else if (Iniciado)
|
||
{
|
||
PortaGPS.Close();
|
||
}
|
||
|
||
PortaGPS.BaudRate = Porta.BaudRate;
|
||
PortaGPS.PortName = Porta.PortName;
|
||
PortaGPS.ReadTimeout = 2000; // Timeout de 2 segundos
|
||
PortaGPS.WriteTimeout = 2000; // Timeout de 2 segundos
|
||
PortaGPS.DataReceived -= PortaGPS_DataReceived;
|
||
PortaGPS.DataReceived += PortaGPS_DataReceived;
|
||
|
||
Porta.Close();
|
||
|
||
if (Iniciado)
|
||
{
|
||
DefinirDispositivo();
|
||
Task.Run(async () => await ConfigurarModulo());
|
||
if (!Variaveis.IsAgroMonitor && CorrecaoRTK_Ntrip)
|
||
{
|
||
Task.Run(async () => await AplicarCorrecaoRTK_Ntrip());
|
||
}
|
||
}
|
||
}
|
||
|
||
private static void DefinirDispositivo()
|
||
{
|
||
if (Iniciado)
|
||
{
|
||
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
||
{
|
||
Dispositivo = T_Code.Gps,
|
||
Endereco = PortaGPS.PortName,
|
||
Versao = "1",
|
||
});
|
||
}
|
||
}
|
||
|
||
public static async Task ConfigurarModulo()
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.ConfigurarModulo] Iniciando configuração do módulo GPS...");
|
||
await ConfigurarModuloRover(comprimento_antena: 130);
|
||
}
|
||
|
||
private static async Task ConfigurarModuloRover(string porta_usb = "com3", string porta_entrada = "com2", int comprimento_antena = 100, int tolerancia_antena = 5)
|
||
{
|
||
string freq = (1.0 / TaxaAmostragemHz).ToString("0.0").Replace(",", ".");
|
||
string[] comandos = {
|
||
// Bauds
|
||
$"config com1 115200\r\n",
|
||
$"config com2 115200\r\n",
|
||
$"config com3 115200\r\n",
|
||
|
||
// limpa logs das portas
|
||
$"unlog com1\r\n",
|
||
$"unlog com2\r\n",
|
||
$"unlog com3\r\n",
|
||
|
||
// modo rover + RTK
|
||
$"mode rover uav\r\n",
|
||
|
||
// timeouts de correção
|
||
$"config rtk timeout {rtk_timeout}\r\n",
|
||
$"config dgps timeout 60\r\n", // ou 0 para desabilitar DGPS fallback
|
||
|
||
// heading 2 antenas
|
||
$"config heading fixlength\r\n",
|
||
//$"config heading tractor\r\n",
|
||
$"config heading length {comprimento_antena} {tolerancia_antena}\r\n",
|
||
// (se precisar, existe 'config heading offset <azim_off> <pitch_off>')
|
||
|
||
// NMEA só na USB (COM1)
|
||
$"gngga {porta_usb} {freq}\r\n",
|
||
$"gpths {porta_usb} {freq}\r\n",
|
||
$"gpvtg {porta_usb} {freq}\r\n",
|
||
|
||
$"saveconfig\r\n"
|
||
};
|
||
|
||
await Task.Delay(2000);
|
||
|
||
foreach (string comando in comandos)
|
||
{
|
||
try
|
||
{
|
||
byte[] bytesComando = Encoding.ASCII.GetBytes(comando);
|
||
PortaGPS?.Write(bytesComando, 0, bytesComando.Length);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.ConfigurarModuloRover] Erro ao enviar comando de configuração GNSS: {ex.Message}");
|
||
}
|
||
await Task.Delay(1000); // Pequeno delay para evitar sobrecarga na comunicação
|
||
}
|
||
}
|
||
|
||
private static async Task ConfigurarModuloBase(string porta_usb = "com3", string porta_saida = "com2", int tempo_fixacao = 60)
|
||
{
|
||
string base_id = "957";
|
||
string distancia_min = "0";
|
||
string[] comandos = {
|
||
// Bauds
|
||
$"config com1 115200\r\n",
|
||
$"config com2 115200\r\n",
|
||
$"config com3 115200\r\n",
|
||
|
||
// limpa logs das portas
|
||
$"unlog com1\r\n",
|
||
$"unlog com2\r\n",
|
||
$"unlog com3\r\n",
|
||
|
||
// Base com Survey-In (tempo + acurácia)
|
||
$"mode base {base_id} time {tempo_fixacao} {distancia_min}\r\n",
|
||
|
||
// RTCM perfil (comece leve; ative mais constelações se o LoRa aguentar)
|
||
$"RTCM1006 {porta_saida} 10\r\n",
|
||
$"RTCM1033 {porta_saida} 30\r\n",
|
||
$"RTCM1074 {porta_saida} 1\r\n", // GPS MSM4
|
||
$"RTCM1124 {porta_saida} 1\r\n", // BeiDou MSM4
|
||
|
||
// (Opcional) ativar mais constelações:
|
||
$"RTCM1094 {porta_saida} 1\r\n", // Galileo MSM4
|
||
$"RTCM1084 {porta_saida} 1\r\n", // GLONASS MSM4
|
||
//$"RTCM1230 {porta_saida} 10\r\n",// Bias GLONASS (OBRIGATÓRIO se 1084 estiver ativo)
|
||
|
||
// NMEA mínimo para debug na USB
|
||
$"gngga {porta_usb} 1\r\n",
|
||
|
||
$"saveconfig\r\n",
|
||
};
|
||
|
||
await Task.Delay(2000);
|
||
|
||
foreach (string cmd in comandos)
|
||
{
|
||
try
|
||
{
|
||
byte[] bytes = Encoding.ASCII.GetBytes(cmd);
|
||
PortaGPS.Write(bytes, 0, bytes.Length);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.ConfigurarModuloBase] Erro ao enviar comando de configuração GNSS: {ex.Message}");
|
||
}
|
||
await Task.Delay(500);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
private static StringBuilder _buffer = new StringBuilder();
|
||
|
||
private static void PortaGPS_DataReceived(object sender, SerialDataReceivedEventArgs e)
|
||
{
|
||
try
|
||
{
|
||
if (!PortaGPS.IsOpen)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// Lê os dados disponíveis na porta
|
||
string recebido = PortaGPS.ReadExisting();
|
||
|
||
//Console.WriteLine(recebido);
|
||
|
||
// Adiciona ao buffer
|
||
_buffer.Append(recebido);
|
||
|
||
// Processa mensagens completas (separadas por "\n")
|
||
string[] mensagens = _buffer.ToString().Split('\n');
|
||
|
||
// Processa todas as mensagens completas
|
||
for (int i = 0; i < mensagens.Length - 1; i++)
|
||
{
|
||
string mensagem = mensagens[i].Trim();
|
||
if (mensagem.Length > 0)
|
||
{
|
||
ProcessarDadosNMEA(mensagem);
|
||
}
|
||
}
|
||
|
||
if (mensagens.Length > 0)
|
||
{
|
||
// Mantém o que sobrou no buffer (última mensagem incompleta)
|
||
_buffer.Clear();
|
||
_buffer.Append(mensagens[mensagens.Length - 1]);
|
||
}
|
||
|
||
var obj = UltimaLeitura.Clone();
|
||
obj.Momento = DateTime.Now;
|
||
|
||
Logs.Add(JsonConvert.SerializeObject(obj));
|
||
VariaveisOperacao.RegistrarLogDispositivo(Logs, T_Code.Gps, ".json", 300);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.PortaGPS_DataReceived] Erro ao processar dados da porta serial: " + ex.Message);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
private static void ProcessarDadosNMEA(string nmeaData)
|
||
{
|
||
DateTime Agora = DateTime.Now;
|
||
var linhas = nmeaData.Split('\n');
|
||
foreach (var linha in linhas)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(linha)) continue;
|
||
|
||
var sentenca = linha.Trim();
|
||
//Console.WriteLine(sentenca);
|
||
|
||
// Identifica o tipo de sentença
|
||
|
||
// GPS Antigo
|
||
if (sentenca.StartsWith("$GPGGA"))
|
||
{
|
||
ProcessarGPGGA(sentenca);
|
||
AtualizarCoordenadasGPS();
|
||
}
|
||
// Coordenadas
|
||
else if (sentenca.StartsWith("$GNGGA") || sentenca.StartsWith("$GLGGA"))
|
||
{
|
||
ProcessarGNGGA(sentenca);
|
||
AtualizarCoordenadasGPS();
|
||
}
|
||
// Variação magnética
|
||
else if (sentenca.StartsWith("$GNRMC"))
|
||
{
|
||
ProcessarGNRMC(sentenca);
|
||
}
|
||
// Curso verdadeiro
|
||
else if (sentenca.StartsWith("$GNVTG") || sentenca.StartsWith("$GPVTG"))
|
||
{
|
||
ProcessarGNVTG(sentenca);
|
||
}
|
||
// Satélites em vista
|
||
else if (sentenca.StartsWith("$GPGSV") || sentenca.StartsWith("$GLGSV") || sentenca.StartsWith("$GBGSV") || sentenca.StartsWith("$GAGSV"))
|
||
{
|
||
ProcessarGSV(sentenca);
|
||
}
|
||
// Orientação real
|
||
else if (sentenca.StartsWith("$GNTHS") || sentenca.StartsWith("$GPTHS") || sentenca.StartsWith("$GATHS"))
|
||
{
|
||
ProcessarGNTHS(sentenca);
|
||
//AtualizarCoordenadasGPS();
|
||
}
|
||
// GLL (GP/GN/GL/GA/BD)
|
||
else if (sentenca.Length > 6 && sentenca[3] == 'G' && sentenca[4] == 'L' && sentenca[5] == 'L')
|
||
{
|
||
//ProcessarGNGLL(sentenca);
|
||
}
|
||
// GSA (GP/GN/GL/GA/BD)
|
||
else if (sentenca.Length > 6 && sentenca[3] == 'G' && sentenca[4] == 'S' && sentenca[5] == 'A')
|
||
{
|
||
ProcessarGxGSA(sentenca);
|
||
}
|
||
else if (sentenca.Length > 6 && sentenca[3] == 'R' && sentenca[4] == 'M' && sentenca[5] == 'C') // RMC
|
||
{
|
||
ProcessarGxRMC(sentenca);
|
||
}
|
||
// ACK de comando do UM982, não é sentença NMEA de navegação.
|
||
else if (sentenca.StartsWith("$command,", StringComparison.OrdinalIgnoreCase))
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.ProcessarDadosNMEA] {sentenca}");
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.ProcessarDadosNMEA] Sentença desconhecida: {sentenca}");
|
||
}
|
||
}
|
||
|
||
PenultimaLeitura.UltimoComandoRespondido = UltimaLeitura.UltimoComandoRespondido;
|
||
UltimaLeitura.UltimoComandoRespondido = Agora;
|
||
}
|
||
|
||
private static void ProcessarGPGGA(string sentenca)
|
||
{
|
||
string[] parts = sentenca.Split(',');
|
||
|
||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
|
||
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
||
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
||
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
|
||
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
|
||
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
||
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
|
||
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
||
PenultimaLeitura.NumeroSatelites = UltimaLeitura.NumeroSatelites;
|
||
|
||
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
|
||
double? lat = null;
|
||
double? lon = null;
|
||
if (parts[2] != "" && parts[3] != "")
|
||
{
|
||
lat = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2);
|
||
}
|
||
if (parts[4] != "" && parts[5] != "")
|
||
{
|
||
lon = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3);
|
||
}
|
||
if (lat != null && lon != null)
|
||
{
|
||
UltimaLeitura.LatitudeAnt = lat.Value;
|
||
UltimaLeitura.LongitudeAnt = lon.Value;
|
||
AplicarPosicaoCorrigida();
|
||
}
|
||
UltimaLeitura.NumeroSatelites = int.TryParse(parts[7], out int numSat) ? numSat : 0;
|
||
UltimaLeitura.PrecisaoHorizontal = double.TryParse(parts[8], NumberStyles.Float, CultureInfo.InvariantCulture, out double hdop) ? hdop : 0;
|
||
UltimaLeitura.Altitude = double.TryParse(parts[9], NumberStyles.Float, CultureInfo.InvariantCulture, out double alt) ? alt : 0;
|
||
}
|
||
|
||
private static void ProcessarGNGGA(string sentenca)
|
||
{
|
||
var ci = CultureInfo.InvariantCulture;
|
||
var campos = sentenca.Split(',');
|
||
|
||
string horaUTC = campos.Length > 1 ? campos[1] : "";
|
||
string latitudeRaw = campos.Length > 2 ? campos[2] : "";
|
||
string hemisferioLat = campos.Length > 3 ? campos[3] : "";
|
||
string longitudeRaw = campos.Length > 4 ? campos[4] : "";
|
||
string hemisferioLon = campos.Length > 5 ? campos[5] : "";
|
||
string qualidade = campos.Length > 6 ? campos[6] : "0";
|
||
string satelitesUsados = campos.Length > 7 ? campos[7] : "0";
|
||
string hdop = campos.Length > 8 ? campos[8] : "99.9";
|
||
string altitudeRaw = campos.Length > 9 ? campos[9] : "0";
|
||
string geoidSepRaw = campos.Length > 11 ? campos[11] : "0";
|
||
string idadeCorrecaoRaw = campos.Length > 13 ? campos[13] : "";
|
||
string base_id = campos.Length > 14 ? campos[14].Split('*')[0] : "";
|
||
|
||
// Conversão de Latitude (ddmm.mmmm)
|
||
double latitude = 0;
|
||
if (!string.IsNullOrEmpty(latitudeRaw))
|
||
{
|
||
// lat tem 2 dígitos de graus
|
||
var deg = double.Parse(latitudeRaw.Substring(0, 2), ci);
|
||
var min = double.Parse(latitudeRaw.Substring(2), ci);
|
||
latitude = deg + (min / 60.0);
|
||
if (hemisferioLat.Equals("S", StringComparison.OrdinalIgnoreCase)) latitude *= -1;
|
||
}
|
||
|
||
|
||
// Conversão de Longitude (dddmm.mmmm)
|
||
double longitude = 0;
|
||
if (!string.IsNullOrEmpty(longitudeRaw))
|
||
{
|
||
// lon tem 3 dígitos de graus
|
||
var deg = double.Parse(longitudeRaw.Substring(0, 3), ci);
|
||
var min = double.Parse(longitudeRaw.Substring(3), ci);
|
||
longitude = deg + (min / 60.0);
|
||
if (hemisferioLon.Equals("W", StringComparison.OrdinalIgnoreCase)) longitude *= -1;
|
||
}
|
||
|
||
// Altitude MSL (campo 9)
|
||
double altMSL = 0;
|
||
double.TryParse(altitudeRaw, NumberStyles.Float, ci, out altMSL);
|
||
|
||
// Geoid separation (campo 11)
|
||
double geoidSep = 0;
|
||
double.TryParse(geoidSepRaw, NumberStyles.Float, ci, out geoidSep);
|
||
|
||
// Altura elipsoidal = MSL + geoid separation
|
||
double altElipsoidal = altMSL + geoidSep;
|
||
|
||
// HDOP (adimensional)
|
||
double.TryParse(hdop, NumberStyles.Float, ci, out double hdopVal);
|
||
|
||
int.TryParse(satelitesUsados, out int nsatelites);
|
||
int.TryParse(qualidade, out int fixCode);
|
||
double idadeCorrecao = -1;
|
||
if (!string.IsNullOrWhiteSpace(idadeCorrecaoRaw))
|
||
double.TryParse(idadeCorrecaoRaw, NumberStyles.Float, ci, out idadeCorrecao);
|
||
|
||
|
||
//Console.WriteLine($"GNGGA: Hora={horaUTC}, Latitude={latitude}, Longitude={longitude}, Qualidade={qualidade}, Satélites={satelitesUsados}, HDOP={hdop}, Altitude={altitude}");
|
||
|
||
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
|
||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
||
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
||
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
|
||
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
|
||
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
||
PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal;
|
||
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
|
||
PenultimaLeitura.NumeroSatelites = UltimaLeitura.NumeroSatelites;
|
||
PenultimaLeitura.QualidadeFix = UltimaLeitura.QualidadeFix;
|
||
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
||
PenultimaLeitura.IdadeCorrecao = UltimaLeitura.IdadeCorrecao;
|
||
PenultimaLeitura.BaseID = UltimaLeitura.BaseID;
|
||
|
||
// Armazenar os valores na última leitura
|
||
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
UltimaLeitura.LatitudeAnt = latitude;
|
||
UltimaLeitura.LongitudeAnt = longitude;
|
||
UltimaLeitura.Altitude = altMSL;
|
||
UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
|
||
UltimaLeitura.PrecisaoHorizontal = hdopVal;
|
||
UltimaLeitura.NumeroSatelites = nsatelites;
|
||
UltimaLeitura.QualidadeFix = (TiposCorrecaoGPS)fixCode;
|
||
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
|
||
UltimaLeitura.BaseID = base_id;
|
||
|
||
AplicarPosicaoCorrigida();
|
||
|
||
// Hora UTC no formato HHmmss.ss (fração opcional)
|
||
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
|
||
{
|
||
// Pega HHmmss e, se houver, fração:
|
||
var hh = int.Parse(horaUTC.Substring(0, 2), ci);
|
||
var mm = int.Parse(horaUTC.Substring(2, 2), ci);
|
||
var ssStr = horaUTC.Substring(4); // "ss" ou "ss.ss"
|
||
double ss = double.Parse(ssStr, ci);
|
||
var ts = new TimeSpan(0, hh, mm, (int)Math.Floor(ss), (int)Math.Round((ss - Math.Floor(ss)) * 1000.0));
|
||
var currentDateUtc = DateTime.UtcNow.Date;
|
||
UltimaLeitura.DataHora = currentDateUtc.Add(ts).ToLocalTime();
|
||
}
|
||
|
||
// 1) define origem ENU na primeira leitura válida
|
||
if (!UltimaLeitura.EnuOriginSet && UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.RTKFixo) // tem fix
|
||
{
|
||
UltimaLeitura.Lat0 = latitude;
|
||
UltimaLeitura.Lon0 = longitude;
|
||
UltimaLeitura.EnuOriginSet = true;
|
||
}
|
||
|
||
}
|
||
|
||
private static void ProcessarGNRMC(string sentenca)
|
||
{
|
||
var campos = sentenca.Split(',');
|
||
|
||
string horaUTC = campos[1].Replace(".", ",");
|
||
string status = campos[2].Replace(".", ",");
|
||
string latitudeRaw = campos[3].Replace(".", ",");
|
||
string hemisferioLat = campos[4].Replace(".", ",");
|
||
string longitudeRaw = campos[5].Replace(".", ",");
|
||
string hemisferioLon = campos[6].Replace(".", ",");
|
||
string velocidadeSobreSolo = campos[7].Replace(".", ",");
|
||
string curso = campos[8].Replace(".", ",");
|
||
string data = campos[9].Replace(".", ",");
|
||
string variaçãoMagnetica = campos[10].Replace(".", ",");
|
||
|
||
double.TryParse(variaçãoMagnetica, out double variacaoMag);
|
||
|
||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||
PenultimaLeitura.VariacaoMagnetica = UltimaLeitura.VariacaoMagnetica;
|
||
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
UltimaLeitura.VariacaoMagnetica = variacaoMag;
|
||
|
||
//Console.WriteLine($"GNRMC: Hora={horaUTC}, Status={status}, Latitude={latitudeRaw}{hemisferioLat}, Longitude={longitudeRaw}{hemisferioLon}, Velocidade={velocidadeSobreSolo}, Curso={curso}, Data={data}, Variação Magnética={variaçãoMagnetica}");
|
||
}
|
||
|
||
private static void ProcessarGNVTG(string sentenca)
|
||
{
|
||
var campos = sentenca.Split(',');
|
||
|
||
string cursoVerdadeiro = campos[1].Replace(".", ",");
|
||
string referenciaCurso = campos[2].Replace(".", ","); // T = Verdadeiro, M = Magnético
|
||
string velocidadeSobreSoloKnots = campos[5].Replace(".", ",");
|
||
string velocidadeSobreSoloKmh = campos[7].Replace(".", ",");
|
||
|
||
//Console.WriteLine($"GNVTG: Curso Verdadeiro={cursoVerdadeiro}{referenciaCurso}, Velocidade (nós)={velocidadeSobreSoloKnots}, Velocidade (km/h)={velocidadeSobreSoloKmh}");
|
||
|
||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||
PenultimaLeitura.CursoVerdadeiro = UltimaLeitura.CursoVerdadeiro;
|
||
PenultimaLeitura.Velocidade = UltimaLeitura.Velocidade;
|
||
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
|
||
double.TryParse(cursoVerdadeiro, out double curso);
|
||
UltimaLeitura.CursoVerdadeiro = curso;
|
||
|
||
double.TryParse(velocidadeSobreSoloKmh, out double velocidade);
|
||
UltimaLeitura.Velocidade = velocidade;
|
||
}
|
||
|
||
private static void ProcessarGSV(string sentenca)
|
||
{
|
||
var campos = sentenca.Split(',');
|
||
|
||
string tipoSistema = sentenca.Substring(1, 2); // GP = GPS, GL = GLONASS, etc.
|
||
string totalSentencas = campos[1];
|
||
string sentencaAtual = campos[2];
|
||
string satelitesVisiveis = campos[3];
|
||
|
||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||
PenultimaLeitura.SatelitesEmVista = new List<GPSSatelitesEmVistaModel>(UltimaLeitura.SatelitesEmVista);
|
||
|
||
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
|
||
int.TryParse(sentencaAtual, out int sentAtual);
|
||
int.TryParse(totalSentencas, out int sentTotal);
|
||
int.TryParse(satelitesVisiveis, out int visiveis);
|
||
|
||
if (!UltimaLeitura.SatelitesEmVista.Any(x => x.TipoSistema == tipoSistema))
|
||
{
|
||
UltimaLeitura.SatelitesEmVista.Add(new GPSSatelitesEmVistaModel()
|
||
{
|
||
TipoSistema = tipoSistema,
|
||
Sentencas = new List<GPSSatelitesEmVistaSentencaModel>()
|
||
});
|
||
}
|
||
|
||
var leitura = UltimaLeitura.SatelitesEmVista.First(x => x.TipoSistema == tipoSistema);
|
||
|
||
//Console.WriteLine($"GSV: Sistema={tipoSistema}, Sentença {sentencaAtual}/{totalSentencas}, Satélites Visíveis={satelitesVisiveis}");
|
||
|
||
if (!leitura.Sentencas.Any(x => x.SentencaAtual == sentAtual))
|
||
{
|
||
leitura.Sentencas.Add(new GPSSatelitesEmVistaSentencaModel()
|
||
{
|
||
SentencaAtual = sentAtual,
|
||
SentencasTotal = sentTotal,
|
||
QuantidadeSatelites = visiveis,
|
||
Dados = new List<GPSSatelitesEmVistaDadosModel>()
|
||
});
|
||
}
|
||
|
||
var _sentenca = leitura.Sentencas.First(x => x.SentencaAtual == sentAtual);
|
||
|
||
_sentenca.Dados = new List<GPSSatelitesEmVistaDadosModel>();
|
||
|
||
for (int i = 4; i < campos.Length; i += 4)
|
||
{
|
||
if (i + 3 < campos.Length)
|
||
{
|
||
string prn = campos[i].Replace(".", ",");
|
||
string elevacaoRaw = campos[i + 1].Replace(".", ",");
|
||
string azimuteRaw = campos[i + 2].Replace(".", ",");
|
||
string snrRaw = campos[i + 3].Replace(".", ",");
|
||
|
||
//Console.WriteLine($" Satélite PRN={prn}, Elevação={elevacaoRaw}, Azimute={azimuteRaw}, SNR={snrRaw}");
|
||
|
||
double.TryParse(elevacaoRaw, out double elevacao);
|
||
double.TryParse(azimuteRaw, out double azimute);
|
||
double.TryParse(snrRaw, out double snr);
|
||
|
||
_sentenca.Dados.Add(new GPSSatelitesEmVistaDadosModel()
|
||
{
|
||
PRN = prn,
|
||
Elevacao = elevacao,
|
||
Azimute = azimute,
|
||
QualidadeSinal = snr
|
||
});
|
||
}
|
||
}
|
||
}
|
||
|
||
private static void ProcessarGNTHS(string sentenca)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(sentenca))
|
||
return;
|
||
|
||
try
|
||
{
|
||
/*
|
||
* Exemplos:
|
||
*
|
||
* Válido:
|
||
* $GPTHS,123.4567,A*XX
|
||
*
|
||
* Inválido:
|
||
* $GPTHS,,V*0E
|
||
*/
|
||
|
||
string conteudo = sentenca.Trim();
|
||
|
||
if (conteudo.StartsWith("$"))
|
||
conteudo = conteudo.Substring(1);
|
||
|
||
string[] partesChecksum = conteudo.Split(
|
||
new[] { '*' },
|
||
2
|
||
);
|
||
|
||
string corpo = partesChecksum[0];
|
||
|
||
string[] campos = corpo.Split(',');
|
||
|
||
// [0] = GPTHS/GNTHS
|
||
// [1] = heading
|
||
// [2] = status A/V
|
||
if (campos.Length < 3)
|
||
{
|
||
Variaveis.MostrarLog(
|
||
$"[GPSService.ProcessarGNTHS] Sentença incompleta: {sentenca}"
|
||
);
|
||
return;
|
||
}
|
||
|
||
string headingTexto = (campos[1] ?? string.Empty).Trim();
|
||
|
||
string statusRecebido = (campos[2] ?? string.Empty)
|
||
.Trim()
|
||
.ToUpperInvariant();
|
||
|
||
bool headingNumerico = double.TryParse(
|
||
headingTexto,
|
||
NumberStyles.Float,
|
||
CultureInfo.InvariantCulture,
|
||
out double headingTrue
|
||
);
|
||
|
||
bool headingValido =
|
||
statusRecebido == "A" &&
|
||
headingNumerico &&
|
||
!double.IsNaN(headingTrue) &&
|
||
!double.IsInfinity(headingTrue);
|
||
|
||
/*
|
||
* Toda sentença recebida gera uma nova leitura,
|
||
* mesmo quando o heading está inválido.
|
||
*
|
||
* Dessa forma, a frequência continua sendo calculada
|
||
* pela chegada das sentenças THS.
|
||
*/
|
||
|
||
PenultimaLeitura.TimestampOri.valor =
|
||
UltimaLeitura.TimestampOri.valor;
|
||
|
||
PenultimaLeitura.Momento =
|
||
UltimaLeitura.Momento;
|
||
|
||
PenultimaLeitura.OrientacaoReal =
|
||
UltimaLeitura.OrientacaoReal;
|
||
|
||
PenultimaLeitura.TipoOrientacao =
|
||
UltimaLeitura.TipoOrientacao;
|
||
|
||
UltimaLeitura.TimestampOri.valor =
|
||
Stopwatch.GetTimestamp() /
|
||
(double)Stopwatch.Frequency;
|
||
|
||
UltimaLeitura.Momento = DateTime.Now;
|
||
|
||
if (!headingValido)
|
||
{
|
||
/*
|
||
* Comunicação está funcionando, mas o UM982
|
||
* não possui solução válida de heading.
|
||
*/
|
||
UltimaLeitura.OrientacaoReal = 9999;
|
||
|
||
UltimaLeitura.TipoOrientacao = "V";
|
||
|
||
/*
|
||
* Não aplicar posição corrigida nem atualizar
|
||
* o ângulo usado pelo controle do rover.
|
||
*
|
||
* O valor 9999 existe apenas para telemetria
|
||
* e diagnóstico de saúde.
|
||
*/
|
||
return;
|
||
}
|
||
|
||
headingTrue = GPSUtils.NormalizarAngulo(
|
||
headingTrue
|
||
);
|
||
|
||
UltimaLeitura.OrientacaoReal =
|
||
InverterHeading
|
||
? GPSUtils.NormalizarAngulo(
|
||
headingTrue - 180.0
|
||
)
|
||
: headingTrue;
|
||
|
||
UltimaLeitura.TipoOrientacao = "A";
|
||
|
||
AplicarPosicaoCorrigida();
|
||
|
||
// Atualiza o ângulo usado pelo restante do sistema
|
||
// somente quando a solução é válida.
|
||
DefinirAnguloCarro();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog(
|
||
$"[GPSService.ProcessarGNTHS] " +
|
||
$"Erro ao processar a sentença '{sentenca}': {ex.Message}"
|
||
);
|
||
}
|
||
}
|
||
|
||
private static void ProcessarGNGLL(string sentenca)
|
||
{
|
||
// Aceita GP/GL/GN… qualquer “G?GLL”
|
||
var campos = sentenca.Split(',');
|
||
if (campos.Length < 7) return;
|
||
|
||
// lat/lon
|
||
string latRaw = campos[1];
|
||
string latHem = campos[2];
|
||
string lonRaw = campos[3];
|
||
string lonHem = campos[4];
|
||
string horaUTC = campos[5]; // hhmmss.ss
|
||
string status = campos[6]; // A/V
|
||
string mode = campos.Length > 7 ? campos[7].Split('*')[0] : ""; // pode não existir
|
||
|
||
bool valido = status == "A" && mode != "N";
|
||
|
||
if (valido && !string.IsNullOrWhiteSpace(latRaw) && !string.IsNullOrWhiteSpace(lonRaw))
|
||
{
|
||
double lat = GPSUtils.DmmToDecimal(latRaw, 2);
|
||
double lon = GPSUtils.DmmToDecimal(lonRaw, 3);
|
||
if (!double.IsInfinity(lat) && !double.IsNaN(lat) && !double.IsInfinity(lon) && !double.IsNaN(lon))
|
||
{
|
||
if (latHem == "S") lat = -lat;
|
||
if (lonHem == "W") lon = -lon;
|
||
|
||
// mantém histórico
|
||
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
||
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
||
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
|
||
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
|
||
|
||
UltimaLeitura.LatitudeAnt = lat;
|
||
UltimaLeitura.LongitudeAnt = lon;
|
||
|
||
AplicarPosicaoCorrigida();
|
||
}
|
||
}
|
||
|
||
// atualiza hora se veio no frame
|
||
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
|
||
{
|
||
// hhmmss(.ss)
|
||
var hh = horaUTC.Substring(0, 2);
|
||
var mm = horaUTC.Substring(2, 2);
|
||
var ss = horaUTC.Substring(4);
|
||
if (TimeSpan.TryParseExact($"{hh}:{mm}:{ss}", @"hh\:mm\:ss\.ff",
|
||
CultureInfo.InvariantCulture, out var tod)
|
||
|| TimeSpan.TryParseExact($"{hh}:{mm}:{ss}", @"hh\:mm\:ss",
|
||
CultureInfo.InvariantCulture, out tod))
|
||
{
|
||
UltimaLeitura.DataHora = DateTime.UtcNow.Date.Add(tod).ToLocalTime();
|
||
}
|
||
}
|
||
|
||
// mapeia “mode” (se existir) para sua enum, sem sobrepor GGA melhor
|
||
// Ex.: A=Autonomous(1), D=DGPS(2), R=RTK Fix(4), F=RTK Float(5)
|
||
if (!string.IsNullOrEmpty(mode))
|
||
{
|
||
if (mode == "R") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFixo;
|
||
else if (mode == "F") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFlutuante;
|
||
else if (mode == "D") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DGPS;
|
||
else if (mode == "E") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DeadReckoing;
|
||
else if (mode == "A") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.Autonomo;
|
||
else if (mode == "N") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
||
else UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
||
}
|
||
|
||
AtualizarCoordenadasGPS();
|
||
}
|
||
|
||
private static void ProcessarGxGSA(string sentenca)
|
||
{
|
||
// aceita $GPGSA, $GNGSA, $GLGSA, $GAGSA, $BDGSA…
|
||
var campos = sentenca.Split(',');
|
||
if (campos.Length < 17) return;
|
||
|
||
string modoSelecao = campos[1]; // M/A
|
||
TiposDimensaoCorrecaoGPS modoSolucao =(TiposDimensaoCorrecaoGPS)GPSUtils.ParseInt(campos[2]); // 1/2/3
|
||
// satélites usados: campos[3]..campos[14]
|
||
int satsUsados = 0;
|
||
for (int i = 3; i <= 14 && i < campos.Length; i++)
|
||
if (!string.IsNullOrWhiteSpace(campos[i])) satsUsados++;
|
||
|
||
double pdop = GPSUtils.ParseDouble(campos[15]);
|
||
double hdop = GPSUtils.ParseDouble(campos[16]);
|
||
double vdop = (campos.Length > 17) ? GPSUtils.ParseDouble(campos[17].Split('*')[0]) : double.NaN;
|
||
|
||
// Atualiza apenas o que faz sentido complementar
|
||
if (!double.IsInfinity(hdop) && !double.IsNaN(hdop)) UltimaLeitura.PrecisaoHorizontal = hdop;
|
||
if (satsUsados > 0) UltimaLeitura.NumeroSatelites = Math.Max(UltimaLeitura.NumeroSatelites, satsUsados);
|
||
|
||
// Se você quiser guardar os DOPs:
|
||
UltimaLeitura.PDOP = !double.IsInfinity(pdop) && !double.IsNaN(pdop) ? pdop : UltimaLeitura.PDOP;
|
||
UltimaLeitura.VDOP = !double.IsInfinity(vdop) && !double.IsNaN(vdop) ? vdop : UltimaLeitura.VDOP;
|
||
|
||
// Mapeia modoSolucao (não é igual ao “fix quality” do GGA!)
|
||
// 1=NoFix, 2=Fix2D, 3=Fix3D — pode guardar num campo próprio se tiver
|
||
UltimaLeitura.FixDimensao = modoSolucao; // crie int FixDimensao na sua struct, se não existir
|
||
}
|
||
|
||
private static void ProcessarGxRMC(string sentenca)
|
||
{
|
||
// Aceita $GPRMC, $GNRMC, $GLRMC, $GARMC, $BDRMC...
|
||
var raw = sentenca;
|
||
var campos = raw.Split(',');
|
||
|
||
if (campos.Length < 12) return;
|
||
|
||
string timeUTC = campos[1]; // hhmmss.ss
|
||
string status = campos[2]; // A=ativo, V=inválido
|
||
string latRaw = campos[3];
|
||
string latHem = campos[4];
|
||
string lonRaw = campos[5];
|
||
string lonHem = campos[6];
|
||
string spdKtsS = campos[7]; // knots
|
||
string cogS = campos[8]; // course over ground (graus)
|
||
string date = campos[9]; // ddmmyy
|
||
string magVarS = campos[10]; // pode estar vazio
|
||
string magHem = campos.Length > 11 ? campos[11] : "";
|
||
// mode pode vir no campo 12 (sem checksum) ou 12 com outro e 13 com checksum, depende do firmware
|
||
string mode = "";
|
||
if (campos.Length > 12)
|
||
{
|
||
var tmp = campos[12];
|
||
// tira checksum se estiver grudado
|
||
int asterix = tmp.IndexOf('*');
|
||
mode = (asterix >= 0 ? tmp.Substring(0, asterix) : tmp).Trim();
|
||
// alguns mandam mais um campo (navStatus) e o checksum só no final
|
||
if (mode.Length == 0 && campos.Length > 13)
|
||
{
|
||
tmp = campos[13];
|
||
asterix = tmp.IndexOf('*');
|
||
mode = (asterix >= 0 ? tmp.Substring(0, asterix) : tmp).Trim();
|
||
}
|
||
}
|
||
|
||
bool valido = status == "A";
|
||
|
||
// Converte lat/lon se válidos
|
||
if (valido && !string.IsNullOrWhiteSpace(latRaw) && !string.IsNullOrWhiteSpace(lonRaw))
|
||
{
|
||
if (latRaw.Length >= 4 && lonRaw.Length >= 5)
|
||
{
|
||
double lat = GPSUtils.DmmToDecimal(latRaw, 2);
|
||
double lon = GPSUtils.DmmToDecimal(lonRaw, 3);
|
||
if (!double.IsNaN(lat) && !double.IsInfinity(lat) && !double.IsNaN(lon) && !double.IsInfinity(lon))
|
||
{
|
||
if (latHem == "S") lat = -lat;
|
||
if (lonHem == "W") lon = -lon;
|
||
|
||
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
||
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
||
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
|
||
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
|
||
|
||
UltimaLeitura.LatitudeAnt = lat;
|
||
UltimaLeitura.LongitudeAnt = lon;
|
||
|
||
AplicarPosicaoCorrigida();
|
||
}
|
||
}
|
||
}
|
||
|
||
// Velocidade (knots -> m/s e km/h, se quiser guardar)
|
||
if (double.TryParse(spdKtsS, NumberStyles.Float, CultureInfo.InvariantCulture, out double spdKts))
|
||
{
|
||
UltimaLeitura.Velocidade = spdKts;
|
||
}
|
||
|
||
// Course over ground (graus)
|
||
if (double.TryParse(cogS, NumberStyles.Float, CultureInfo.InvariantCulture, out double cog))
|
||
UltimaLeitura.CursoVerdadeiro = cog;
|
||
|
||
// Data/hora (UTC)
|
||
// timeUTC: hhmmss(.ss), date: ddmmyy
|
||
DateTime? dt = null;
|
||
if (!string.IsNullOrEmpty(timeUTC) && timeUTC.Length >= 6 && !string.IsNullOrEmpty(date) && date.Length == 6)
|
||
{
|
||
string hh = timeUTC.Substring(0, 2);
|
||
string mm = timeUTC.Substring(2, 2);
|
||
string ss = timeUTC.Substring(4, 2);
|
||
|
||
string dd = date.Substring(0, 2);
|
||
string MM = date.Substring(2, 2);
|
||
string yy = date.Substring(4, 2);
|
||
|
||
// yy -> 20yy (assumindo 2000+; ajuste se precisar 19xx)
|
||
int year = 2000 + int.Parse(yy, CultureInfo.InvariantCulture);
|
||
if (int.TryParse(dd, out int d) && int.TryParse(MM, out int M) &&
|
||
int.TryParse(hh, out int H) && int.TryParse(mm, out int m) && int.TryParse(ss, out int s))
|
||
{
|
||
try
|
||
{
|
||
dt = new DateTime(year, M, d, H, m, s, DateTimeKind.Utc);
|
||
}
|
||
catch { /* ignora datas inválidas */ }
|
||
}
|
||
}
|
||
if (dt.HasValue) UltimaLeitura.DataHora = dt.Value.ToLocalTime();
|
||
|
||
// Variação magnética (se quiser armazenar)
|
||
if (double.TryParse(magVarS, NumberStyles.Float, CultureInfo.InvariantCulture, out double magVar))
|
||
{
|
||
if (magHem == "W") magVar = -magVar;
|
||
UltimaLeitura.VariacaoMagnetica = magVar;
|
||
}
|
||
|
||
// Mode → promove QualidadeFix (não rebaixa)
|
||
// A=Autônomo, D=DGPS, R=RTK Fix, F=RTK Float, N=No Fix
|
||
if (!string.IsNullOrEmpty(mode))
|
||
{
|
||
if (mode == "R") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFixo;
|
||
else if (mode == "F") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFlutuante;
|
||
else if (mode == "D") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DGPS;
|
||
else if (mode == "E") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DeadReckoing;
|
||
else if (mode == "A") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.Autonomo;
|
||
else if (mode == "N") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
||
else UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
||
}
|
||
|
||
AtualizarCoordenadasGPS();
|
||
}
|
||
|
||
|
||
private static async Task AplicarCorrecaoRTK_Ntrip()
|
||
{
|
||
if (!Iniciado || LoopRTK_Ntrip || !APIService.HasInternet)
|
||
return;
|
||
|
||
LoopRTK_Ntrip = true;
|
||
|
||
// Configurações do NTRIP caster para RTK2Go
|
||
string host = "gps-ntrip.ibge.gov.br";
|
||
int port = 2101;
|
||
string mountpoint = "EESC0";
|
||
string username = "Zendion"; // Geralmente vazio para RTK2Go
|
||
string password = "QD&m1p60"; // Geralmente vazio para RTK2Go
|
||
|
||
while (CorrecaoRTK_Ntrip && APIService.HasInternet) // Loop para reconectar em caso de falha
|
||
{
|
||
try
|
||
{
|
||
// Construa o cabeçalho da solicitação
|
||
string credentials = string.IsNullOrEmpty(username)
|
||
? ""
|
||
: Convert.ToBase64String(Encoding.ASCII.GetBytes($"{username}:{password}"));
|
||
|
||
string request = $"GET /{mountpoint} HTTP/1.0\r\n" +
|
||
$"User-Agent: NTRIP Client/2.0\r\n" +
|
||
$"Accept: */*\r\n" +
|
||
$"Connection: keep-alive\r\n" +
|
||
(!string.IsNullOrEmpty(credentials) ? $"Authorization: Basic {credentials}\r\n" : "") +
|
||
"\r\n";
|
||
|
||
// Estabeleça a conexão
|
||
using (TcpClient client = new TcpClient(host, port))
|
||
using (NetworkStream stream = client.GetStream())
|
||
using (StreamWriter writer = new StreamWriter(stream, Encoding.ASCII))
|
||
{
|
||
writer.Write(request);
|
||
writer.Flush();
|
||
|
||
// Leia a resposta
|
||
using (StreamReader reader = new StreamReader(stream, Encoding.ASCII))
|
||
{
|
||
string response = await reader.ReadLineAsync();
|
||
if (CorrecaoRTK_Ntrip)
|
||
{
|
||
if ((response ?? "").Contains("200 OK"))
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.AplicarCorrecaoRTK_Ntrip] Conexão bem-sucedida ao mountpoint!");
|
||
|
||
byte[] buffer = new byte[4096];
|
||
int bytesRead;
|
||
while (CorrecaoRTK_Ntrip && (bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length)) > 0)
|
||
{
|
||
try
|
||
{
|
||
if (PortaGPS?.IsOpen ?? false)
|
||
{
|
||
PortaGPS.Write(buffer, 0, bytesRead); // Envia os dados RTCM para o GPS
|
||
//Console.WriteLine($"Enviando {bytesRead} bytes de correção RTCM para o GPS");
|
||
UltimoEnvioCorrecaoRTK = DateTime.Now;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.AplicarCorrecaoRTK_Ntrip] Erro ao enviar correção RTCM para o módulo GNSS: {ex.Message}");
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.AplicarCorrecaoRTK_Ntrip] Falha na conexão com NTRIP: {response}");
|
||
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog($"[GPSService.AplicarCorrecaoRTK_Ntrip] Erro na conexão RTK: {ex.Message}");
|
||
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
||
}
|
||
}
|
||
|
||
LoopRTK_Ntrip = false;
|
||
}
|
||
|
||
public static void AplicarCorrecaoRTK_Mqtt(byte[] correcao, int bytesRead)
|
||
{
|
||
if (PortaGPS?.IsOpen ?? false)
|
||
{
|
||
PortaGPS.Write(correcao, 0, bytesRead);
|
||
UltimoEnvioCorrecaoRTK = DateTime.Now;
|
||
}
|
||
}
|
||
|
||
|
||
private static void AplicarPosicaoCorrigida()
|
||
{
|
||
(double latCor, double lonCorr) = LeverArm.FixLeverArmLatLon_Fast(UltimaLeitura.LatitudeAnt, UltimaLeitura.LongitudeAnt, UltimaLeitura.OrientacaoReal, UltimaLeitura.TimestampOri.frequencia);
|
||
UltimaLeitura.Latitude = latCor;
|
||
UltimaLeitura.Longitude = lonCorr;
|
||
}
|
||
|
||
public static void AtualizarCoordenadasGPS()
|
||
{
|
||
PenultimaLeitura.Ntrip_ativado = UltimaLeitura.Ntrip_ativado;
|
||
PenultimaLeitura.Heartbeat = UltimaLeitura.Heartbeat;
|
||
PenultimaLeitura.LeverArmFrontal = UltimaLeitura.LeverArmFrontal;
|
||
PenultimaLeitura.LeverArmLateral = UltimaLeitura.LeverArmLateral;
|
||
|
||
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
||
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
||
UltimaLeitura.LeverArmFrontal = LeverArm?.FrontalTotalCm ?? 0;
|
||
UltimaLeitura.LeverArmLateral = LeverArm?.LateralTotalCm ?? 0;
|
||
|
||
if (Variaveis.IsAgroMonitor)
|
||
{
|
||
//EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, false, LoRaBaseService.parametrosModel.address);
|
||
return;
|
||
}
|
||
|
||
UltimasLeituras.Add(UltimaLeitura.Clone());
|
||
if (UltimasLeituras.Count > TaxaAmostragemHz) UltimasLeituras.Remove(UltimasLeituras.First());
|
||
|
||
DefinirOrientacaoMovimento();
|
||
|
||
AtualizaDadosRedis();
|
||
|
||
if (Variaveis.OperacaoEmAndamento.Iniciado)
|
||
{
|
||
var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;
|
||
CorredorTrajetoriaModel CorredorAtual = _Trajetoria?.CorredorAtual;
|
||
var _GPSTrajetoria = Variaveis.OperacaoEmAndamento.GPSTrajetoria;
|
||
|
||
// ✅ Criando uma cópia eficiente de UltimaLeitura sem copiar manualmente cada propriedade
|
||
var novaLeitura = UltimaLeitura.Clone();
|
||
|
||
// ✅ Adiciona a nova leitura à trajetória GPS
|
||
_GPSTrajetoria.Add(novaLeitura);
|
||
|
||
// ✅ Calcula a distância entre os dois últimos pontos, mas só se houver pelo menos 2 pontos
|
||
if (_GPSTrajetoria.Count > 1)
|
||
{
|
||
int ultimo = _GPSTrajetoria.Count - 1;
|
||
int penultimo = _GPSTrajetoria.Count - 2;
|
||
|
||
double distancia = GPSUtils.DistanciaEntrePontos(
|
||
_GPSTrajetoria[ultimo], // Último ponto
|
||
_GPSTrajetoria[penultimo] // Penúltimo ponto
|
||
);
|
||
|
||
_Trajetoria?.AtualizarDistanciaPercorrida(distancia);
|
||
}
|
||
}
|
||
|
||
foreach (var bico in Variaveis.OperacaoEmAndamento.Controle?.Bicos ?? new List<Models.Modules.AtuadorBicoModel>())
|
||
{
|
||
if (bico.Inicializado && bico.Comandar && bico._EstadoLeitura == Estado.Ligado)
|
||
{
|
||
bico.TrechosAtuado[bico.TrechosAtuado.Count - 1].Add(new double[] { UltimaLeitura.Latitude, UltimaLeitura.Longitude });
|
||
}
|
||
}
|
||
|
||
Variaveis.OperacaoEmAndamento.Trajetoria?.LoopAtualizaDados();
|
||
|
||
AtualizarTrajetoriaDinamica();
|
||
|
||
int EnderecoEquipamento = Convert.ToInt32(Variaveis.LoraService?.ParametrosGet?.address ?? Variaveis.LoraService?.ParametrosSet?.address ?? 0x00);
|
||
EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, Variaveis.OperacaoEmAndamento.Iniciado, EnderecoEquipamento);
|
||
|
||
if (Variaveis.LoraService?.Iniciado ?? false && (Variaveis.LoraService?._ultimoRxDados ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60))
|
||
{
|
||
byte EnderecoBase = Variaveis.LoraBaseParametros.address;
|
||
EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.OrientacaoReal, false, EnderecoBase);
|
||
}
|
||
|
||
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
||
UltimaLeitura.Inicializado = Iniciado;
|
||
|
||
if (CorrecaoRTK_Ntrip && UltimoEnvioCorrecaoRTK.AddSeconds(TempoMin_Ntrip) < DateTime.Now)
|
||
{
|
||
UltimoEnvioCorrecaoRTK = DateTime.Now;
|
||
LoopRTK_Ntrip = false;
|
||
Task.Run(async () =>
|
||
{
|
||
await Task.Delay(5000);
|
||
await AplicarCorrecaoRTK_Ntrip();
|
||
});
|
||
}
|
||
}
|
||
|
||
|
||
|
||
public static void EnviarCoordenadasParaMapa(double Latitude, double Longitude, double Orientacao, bool EmFoco, int ID)
|
||
{
|
||
var Coordenadas = new
|
||
{
|
||
latitude = Latitude,
|
||
longitude = Longitude,
|
||
orientacao = GPSUtils.NormalizarAngulo(Orientacao - 180.0),
|
||
id = ID,
|
||
foco = EmFoco
|
||
};
|
||
Task.Run(async () =>
|
||
{
|
||
//Console.WriteLine(JsonConvert.SerializeObject(Coordenadas));
|
||
await Variaveis.MqttServiceLocal.PublishAsync(
|
||
Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoCoordenadasGPS),
|
||
JsonConvert.SerializeObject(Coordenadas)
|
||
);
|
||
});
|
||
}
|
||
|
||
public static void AtualizarTrajetoriaDinamica()
|
||
{
|
||
if (Variaveis.OperacaoEmAndamento.Trajetoria?._TrajetoriaDinamica?.Any() ?? false)
|
||
{
|
||
var Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria.TrajetoriaDinamica.Select(x => new
|
||
{
|
||
latitude = x.Latitude,
|
||
longitude = x.Longitude
|
||
}).ToArray();
|
||
|
||
Task.Run(async () =>
|
||
{
|
||
await Variaveis.MqttServiceLocal.PublishAsync(
|
||
Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoTrajetoriaDinamica),
|
||
JsonConvert.SerializeObject(Trajetoria)
|
||
);
|
||
});
|
||
}
|
||
}
|
||
|
||
public static void AtualizarRuasSelecionadas(List<string> RuasSelecionadas)
|
||
{
|
||
Task.Run(async () =>
|
||
{
|
||
await Variaveis.MqttServiceLocal.PublishAsync(
|
||
Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoSelecaoRuasMapa),
|
||
JsonConvert.SerializeObject("[" + string.Join(",", RuasSelecionadas.ToArray()) + "]"),
|
||
true
|
||
);
|
||
});
|
||
}
|
||
|
||
private static void DefinirOrientacaoMovimento()
|
||
{
|
||
if (UltimasLeituras.Count < 2)
|
||
{
|
||
double ang = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
|
||
double d = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
|
||
|
||
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
||
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
||
|
||
UltimaLeitura.OrientacaoMovimento = ang;
|
||
UltimaLeitura.Distancia = d; // aqui fica sendo o deslocamento dessa “janela” mínima
|
||
}
|
||
else
|
||
{
|
||
// Média circular ponderada pela distância de cada segmento da janela
|
||
double sumX = 0.0, sumY = 0.0;
|
||
double distAcum = 0.0;
|
||
|
||
for (int i = 0; i < UltimasLeituras.Count - 1; i++)
|
||
{
|
||
var a = UltimasLeituras[i];
|
||
var b = UltimasLeituras[i + 1];
|
||
|
||
double angSeg = GPSUtils.CalcularOrientacao(a, b); // em graus
|
||
double dSeg = GPSUtils.DistanciaEntrePontos(a, b); // em metros
|
||
if (dSeg <= 0) continue; // ignora degrau zero
|
||
|
||
double rad = angSeg * Math.PI / 180.0;
|
||
sumX += Math.Cos(rad) * dSeg; // peso = distância
|
||
sumY += Math.Sin(rad) * dSeg;
|
||
distAcum += dSeg;
|
||
}
|
||
|
||
// Se tudo foi zero (parado), caia para heading instantâneo
|
||
double anguloMovimento;
|
||
if (distAcum <= 0)
|
||
{
|
||
anguloMovimento = UltimaLeitura.OrientacaoReal; // heading como fallback parado
|
||
}
|
||
else
|
||
{
|
||
anguloMovimento = Math.Atan2(sumY, sumX) * 180.0 / Math.PI;
|
||
anguloMovimento = GPSUtils.NormalizarAngulo(anguloMovimento);
|
||
}
|
||
|
||
// Distância linear entre a 1ª e a última da janela (boa para "confiabilidade")
|
||
var first = UltimasLeituras[0];
|
||
var last = UltimasLeituras[UltimasLeituras.Count - 1];
|
||
double distLinear = GPSUtils.DistanciaEntrePontos(first, last);
|
||
|
||
// Atualiza campos
|
||
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
||
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
||
|
||
UltimaLeitura.OrientacaoMovimento = anguloMovimento;
|
||
UltimaLeitura.Distancia = distLinear; // use a linear como sinal de "Δpos confiável" para a fusão
|
||
}
|
||
|
||
DefinirAnguloCarro();
|
||
}
|
||
|
||
public static void DefinirAnguloCarro()
|
||
{
|
||
double anguloFinal = 0.0;
|
||
|
||
bool imuIniciado = Variaveis.OperacaoEmAndamento.Sensoriamento?.IMU?.Iniciado ?? false;
|
||
bool gpsIniciado = Iniciado;
|
||
bool headingValido = UltimaLeitura.TimestampOri.frequencia >= 1.0;
|
||
|
||
if (imuIniciado)
|
||
{
|
||
//Variaveis.OperacaoEmAndamento.DispSen.Dados?.DadosLeitura?.SensoresIMU?.FirstOrDefault()?.AtualizarOffset(UltimaLeitura);
|
||
}
|
||
|
||
if (Variaveis.OperacaoEmAndamento.Simulando)
|
||
{
|
||
anguloFinal = UltimaLeitura.OrientacaoReal;
|
||
}
|
||
else if (gpsIniciado && headingValido)
|
||
{
|
||
anguloFinal = UltimaLeitura.OrientacaoReal;
|
||
}
|
||
else if (gpsIniciado && UltimaLeitura.Distancia > 0.25)
|
||
{
|
||
anguloFinal = UltimaLeitura.OrientacaoMovimento;
|
||
}
|
||
else if (imuIniciado)
|
||
{
|
||
anguloFinal = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU.YawSeguro;
|
||
}
|
||
else
|
||
{
|
||
anguloFinal = UltimaLeitura.AnguloCarroDefinido;
|
||
}
|
||
|
||
// Atualiza o ângulo final
|
||
UltimaLeitura.AnguloCarroDefinido = anguloFinal; // GPSUtils.NormalizarAngulo(anguloFinal + HeadingOffsetSimulador + HeadingOffsetCorrecao);
|
||
}
|
||
|
||
public static double FundirHeadingComMovimento(
|
||
double headingDeg, // OrientacaoReal (antena/corpo)
|
||
double angMovDeg, // OrientacaoMovimento (calculada acima)
|
||
double distLinearJanela, // UltimaLeitura.Distancia (Δpos linear entre 1ª e última amostra)
|
||
double velPercent = 0.0, // se tiver velocidade filtrada, passe aqui; senão 0
|
||
bool rtkFix = true, // se tiver essa info
|
||
double angFundidoAnterior = double.NaN, // para low-pass; passe double.NaN para desabilitar
|
||
double alfaLowPass = 0.25 // 0→sem low-pass; 0.2–0.35 costuma ser bom
|
||
)
|
||
{
|
||
// Parâmetros sintonizados para 5 Hz
|
||
double distMin = 0.03; // ~3 cm → considera "parado"
|
||
double distFull = 0.25; // ~25 cm → deslocamento confiável
|
||
double pesoMinHeading = 0.30; // mantém um "fio" do heading mesmo em alta confiança no movimento
|
||
double fatorConfMovSemFix = 0.80; // penaliza confiança no movimento se não for RTK FIX
|
||
|
||
// Confiança em "movimento" por distância da janela (0..1)
|
||
double fd = Smoothstep(0, 1, Norm(distLinearJanela, distMin, distFull));
|
||
// Confiança por velocidade (0..1)
|
||
double fv = velPercent / 100.0;
|
||
|
||
double confMov = Math.Max(fd, fv);
|
||
if (!rtkFix) confMov *= fatorConfMovSemFix;
|
||
|
||
// Peso do heading = 1 - confMov, mas preservando contribuição mínima proporcional
|
||
double pesoHeading = 1.0 - confMov;
|
||
double minHeading = pesoMinHeading * confMov;
|
||
if (pesoHeading < minHeading) pesoHeading = minHeading;
|
||
if (pesoHeading > 1.0) pesoHeading = 1.0;
|
||
|
||
double angMisto = MisturarAngulosCircular(headingDeg, angMovDeg, pesoHeading);
|
||
|
||
if (!double.IsNaN(angFundidoAnterior) && alfaLowPass > 0)
|
||
angMisto = LowPassAngle0to360(angFundidoAnterior, angMisto, alfaLowPass);
|
||
else
|
||
angMisto = Normalize0To360(angMisto);
|
||
|
||
return angMisto;
|
||
|
||
// ---------- helpers locais ----------
|
||
double Norm(double v, double lo, double hi)
|
||
{
|
||
if (hi <= lo) return v >= hi ? 1.0 : 0.0;
|
||
double t = (v - lo) / (hi - lo);
|
||
if (t < 0) t = 0; else if (t > 1) t = 1;
|
||
return t;
|
||
}
|
||
double Smoothstep(double a, double b, double x)
|
||
{
|
||
// aqui x já normalizado 0..1 no uso acima
|
||
x = FuncoesMatematicas.Clamp(x, 0.0, 1.0);
|
||
return x * x * (3 - 2 * x);
|
||
}
|
||
double MisturarAngulosCircular(double aDeg, double bDeg, double pesoA /*0..1*/)
|
||
{
|
||
double a = aDeg * Math.PI / 180.0;
|
||
double b = bDeg * Math.PI / 180.0;
|
||
double x = Math.Cos(a) * pesoA + Math.Cos(b) * (1.0 - pesoA);
|
||
double y = Math.Sin(a) * pesoA + Math.Sin(b) * (1.0 - pesoA);
|
||
return Math.Atan2(y, x) * 180.0 / Math.PI;
|
||
}
|
||
double LowPassAngle(double atualDeg, double novoDeg, double alfa)
|
||
{
|
||
double diff = GPSUtils.NormalizarAngulo(novoDeg - atualDeg);
|
||
return GPSUtils.NormalizarAngulo(atualDeg + alfa * diff);
|
||
}
|
||
double LowPassAngle0to360(double atualDeg, double novoDeg, double alfa)
|
||
{
|
||
double diff = NormalizeSigned180(novoDeg - atualDeg); // delta curto
|
||
return Normalize0To360(atualDeg + alfa * diff);
|
||
}
|
||
double NormalizeSigned180(double angDeg)
|
||
{
|
||
angDeg = (angDeg + 180.0) % 360.0;
|
||
if (angDeg < 0) angDeg += 360.0;
|
||
return angDeg - 180.0; // [-180, +180)
|
||
}
|
||
double Normalize0To360(double angDeg)
|
||
{
|
||
angDeg %= 360.0;
|
||
if (angDeg < 0) angDeg += 360.0;
|
||
return angDeg; // [0, 360)
|
||
}
|
||
}
|
||
|
||
public static void AtualizaDadosRedis()
|
||
{
|
||
try
|
||
{
|
||
GPSModel posicaoAtual =
|
||
Variaveis.OperacaoEmAndamento.Simulando
|
||
? historicoPosicao.Peek()
|
||
: UltimaLeitura;
|
||
|
||
bool rtkValido =
|
||
posicaoAtual.IdadeCorrecao >= 0 &&
|
||
posicaoAtual.IdadeCorrecao < rtk_timeout &&
|
||
(
|
||
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFixo ||
|
||
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFlutuante ||
|
||
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.DGPS
|
||
);
|
||
|
||
// ---------------------------------------------------------
|
||
// ESTADO DO HEADING DUAL-ANTENNA
|
||
// ---------------------------------------------------------
|
||
|
||
string statusOrientacao =
|
||
(posicaoAtual.TipoOrientacao ?? string.Empty)
|
||
.Trim()
|
||
.ToUpperInvariant();
|
||
|
||
double valorOrientacao =
|
||
posicaoAtual.OrientacaoReal;
|
||
|
||
bool valorOrientacaoNumerico =
|
||
!double.IsNaN(valorOrientacao) &&
|
||
!double.IsInfinity(valorOrientacao);
|
||
|
||
bool valorOrientacaoDentroDaFaixa =
|
||
valorOrientacaoNumerico &&
|
||
valorOrientacao >= 0.0 &&
|
||
valorOrientacao < 360.0;
|
||
|
||
/*
|
||
* A solução só é utilizável quando:
|
||
*
|
||
* 1. O UM982 informou status "A";
|
||
* 2. O valor é numérico;
|
||
* 3. O valor está na faixa válida de heading.
|
||
*
|
||
* Assim, a sentinela 9999 nunca será considerada válida.
|
||
*/
|
||
bool orientacaoValida =
|
||
statusOrientacao == "A" &&
|
||
valorOrientacaoDentroDaFaixa;
|
||
|
||
RedisService.AtualizarCampos(
|
||
RedisService.ModKey(T_Code.Gps),
|
||
|
||
("conectado", Iniciado),
|
||
("freq_base", TaxaAmostragemHz),
|
||
|
||
// Posição e orientação utilizada pelo carro
|
||
("lat", posicaoAtual.Latitude),
|
||
("lon", posicaoAtual.Longitude),
|
||
("theta", posicaoAtual.AnguloCarroDefinido),
|
||
|
||
// Estado da solução GNSS
|
||
("fix", (int)posicaoAtual.QualidadeFix),
|
||
("rtk", rtkValido),
|
||
("hAcc", posicaoAtual.PrecisaoCm),
|
||
("nSatelites", posicaoAtual.NumeroSatelites),
|
||
("age", posicaoAtual.IdadeCorrecao),
|
||
|
||
// Frequências
|
||
("freq.posicao", posicaoAtual.TimestampPos.frequencia),
|
||
("freq.orientacao", posicaoAtual.TimestampOri.frequencia),
|
||
|
||
// Latências
|
||
("latency.posicao", posicaoAtual.TimestampPos.dt),
|
||
("latency.orientacao", posicaoAtual.TimestampOri.dt),
|
||
|
||
// Timestamps monotônicos
|
||
("timestamp.posicao", posicaoAtual.TimestampPos.valor),
|
||
("timestamp.orientacao", posicaoAtual.TimestampOri.valor),
|
||
|
||
// Estado específico do heading dual-antenna
|
||
("orientacao.status", statusOrientacao),
|
||
("orientacao.valida", orientacaoValida),
|
||
("orientacao.valor", valorOrientacao),
|
||
|
||
("heartbeat", posicaoAtual.Heartbeat)
|
||
);
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
Variaveis.MostrarLog(
|
||
$"[GPSService.AtualizarDadosRedis] " +
|
||
$"Erro ao salvar dados do GPS no Redis: {e.Message}"
|
||
);
|
||
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(
|
||
T_Code.Gps,
|
||
StatusModulo.Falha,
|
||
0,
|
||
$"Erro ao salvar dados do GPS no Redis: {e.Message}"
|
||
);
|
||
}
|
||
}
|
||
|
||
public static Queue<GPSModel> historicoPosicao = new Queue<GPSModel>();
|
||
public static void AtualizarAtrasoPosicoes(int errosConsiderar = 0)
|
||
{
|
||
historicoPosicao.Enqueue(UltimaLeitura);
|
||
while (historicoPosicao.Count > 1 && (historicoPosicao.Count > errosConsiderar || errosConsiderar == 0))
|
||
{
|
||
historicoPosicao.Dequeue();
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
public class GgaFix
|
||
{
|
||
public DateTime TsUtc { get; }
|
||
public double LatDeg { get; }
|
||
public double LonDeg { get; }
|
||
public double AltElipsoidalM { get; }
|
||
public double HeadingDeg { get; }
|
||
public TiposCorrecaoGPS FixQuality { get; }
|
||
|
||
public GgaFix(DateTime tsUtc, double latDeg, double lonDeg, double altElipsoidalM, double headingDeg, TiposCorrecaoGPS fixQuality)
|
||
{
|
||
TsUtc = tsUtc;
|
||
LatDeg = latDeg;
|
||
LonDeg = lonDeg;
|
||
AltElipsoidalM = altElipsoidalM;
|
||
HeadingDeg = headingDeg;
|
||
FixQuality = fixQuality;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
public class GeoLeverArm
|
||
{
|
||
// Lever arm físico fixo do equipamento
|
||
private readonly double xFisico; // +frente (m)
|
||
private readonly double yFisico; // +direita (m)
|
||
|
||
// Offset operacional/de campo
|
||
private readonly double xCampo; // +frente (m)
|
||
private readonly double yCampo; // +direita (m)
|
||
|
||
private readonly bool invertHeading;
|
||
private readonly bool mirrorLateral;
|
||
|
||
public GeoLeverArm(double offsetFisicoFrontalCm = 0, double offsetFisicoLateralCm = 0, double offsetCampoFrontalCm = 0, double offsetCampoLateralCm = 0, bool invH = false, bool mirrL = false)
|
||
{
|
||
// fixos do equipamento
|
||
xFisico = offsetFisicoFrontalCm / 100.0;
|
||
yFisico = offsetFisicoLateralCm / 100.0;
|
||
|
||
// ajustes de campo
|
||
xCampo = offsetCampoFrontalCm / 100.0;
|
||
yCampo = offsetCampoLateralCm / 100.0;
|
||
|
||
invertHeading = invH;
|
||
mirrorLateral = mirrL;
|
||
}
|
||
|
||
private double ToRad(double deg) => deg * Math.PI / 180.0;
|
||
|
||
public double FrontalTotalCm => (xFisico + xCampo) * 100.0;
|
||
public double LateralTotalCm => (yFisico + yCampo) * 100.0;
|
||
|
||
/// <summary>
|
||
/// Convenção:
|
||
/// - Entrada = posição da antena GNSS
|
||
/// - Saída = posição corrigida do centro/VRP
|
||
/// - heading: 0° = Norte, 90° = Leste, sentido horário
|
||
/// - frontal positivo = antena à frente do centro
|
||
/// - lateral positivo = antena à direita do centro
|
||
/// Consequência:
|
||
/// - frontal positivo desloca a posição corrigida para trás do robô
|
||
/// - lateral positivo desloca a posição corrigida para a esquerda do robô
|
||
/// </summary>
|
||
public (double lat, double lon) FixLeverArmLatLon_Fast(double latAnt_deg, double lonAnt_deg, double headingDeg, double headingFreq)
|
||
{
|
||
if (headingFreq < 1) return (latAnt_deg, lonAnt_deg);
|
||
|
||
double xTotal = xFisico + xCampo;
|
||
double yTotal = yFisico + yCampo;
|
||
|
||
if (mirrorLateral)
|
||
yTotal = -yTotal;
|
||
|
||
double th = ToRad(headingDeg);
|
||
if (invertHeading)
|
||
th = -th;
|
||
|
||
// Convenção NAV
|
||
double fE = Math.Sin(th); // forward east
|
||
double fN = Math.Cos(th); // forward north
|
||
|
||
double rE = fN; // right east
|
||
double rN = -fE; // right north
|
||
|
||
// deslocamento da antena no mundo
|
||
double dE = xTotal * fE + yTotal * rE;
|
||
double dN = xTotal * fN + yTotal * rN;
|
||
|
||
double latRad = ToRad(latAnt_deg);
|
||
|
||
double dLat_deg = (dN / GPSUtils.RaioDaTerra) * 180.0 / Math.PI;
|
||
double dLon_deg = (dE / (GPSUtils.RaioDaTerra * Math.Cos(latRad))) * 180.0 / Math.PI;
|
||
|
||
// retorna o centro/VRP
|
||
return (latAnt_deg - dLat_deg, lonAnt_deg - dLon_deg);
|
||
}
|
||
|
||
// lat/lon -> ENU (aproximação local, boa para ~km)
|
||
public (double x, double y) GeodeticToENU(double lat, double lon, double lat0, double lon0)
|
||
{
|
||
double latR = ToRad(lat), lonR = ToRad(lon);
|
||
double lat0R = ToRad(lat0), lon0R = ToRad(lon0);
|
||
double dLat = latR - lat0R, dLon = lonR - lon0R;
|
||
double xEast = dLon * Math.Cos(lat0R) * GPSUtils.RaioDaTerra;
|
||
double yNorth = dLat * GPSUtils.RaioDaTerra;
|
||
return (xEast, yNorth);
|
||
}
|
||
|
||
}
|
||
|
||
}
|