1451 lines
59 KiB
C#
1451 lines
59 KiB
C#
|
using System.Text;
|
|||
|
|
using System.IO.Ports;
|
|||
|
|
using System.Diagnostics;
|
|||
|
|
using System.Globalization;
|
|||
|
|
using System.IO;
|
|||
|
|
using System.Net.Sockets;
|
|||
|
|
using AgroBase.Services;
|
|||
|
|
using AgroBase.Models;
|
|||
|
|
using static AgroBase.Models.Enums;
|
|||
|
|
using OperationControl.Models;
|
|||
|
|
using System.Windows;
|
|||
|
|
|
|||
|
|
namespace OperationControl.Services
|
|||
|
|
{
|
|||
|
|
public class GpsService
|
|||
|
|
{
|
|||
|
|
public GpsService()
|
|||
|
|
{
|
|||
|
|
tmrCheck.Elapsed += (_, __) => tmrCheck_Elapsed();
|
|||
|
|
tmrCheck.Start();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private readonly System.Timers.Timer tmrCheck = new(5000) { AutoReset = true };
|
|||
|
|
private bool tmrCheckRunning = false;
|
|||
|
|
|
|||
|
|
// Porta que o serviço encontrou e está usando
|
|||
|
|
public SerialPort PortaGps { get; private set; }
|
|||
|
|
public bool IsConnected => PortaGps != null && PortaGps.IsOpen;
|
|||
|
|
public string PortName => PortaGps?.PortName;
|
|||
|
|
|
|||
|
|
// Taxa padrão do UM982
|
|||
|
|
private const int DefaultBaudRate = 115200;
|
|||
|
|
|
|||
|
|
|
|||
|
|
private async void tmrCheck_Elapsed()
|
|||
|
|
{
|
|||
|
|
if (tmrCheckRunning) return;
|
|||
|
|
tmrCheckRunning = true;
|
|||
|
|
|
|||
|
|
if (!IsConnected)
|
|||
|
|
{
|
|||
|
|
bool conectado = await ScanAndConnectAsync();
|
|||
|
|
if (conectado)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"GPS conectado na porta {PortName}");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Nenhum GPS UM982 encontrado :(");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
tmrCheckRunning = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Faz uma varredura nas portas COM disponíveis tentando encontrar o UM982.
|
|||
|
|
/// Se encontrar, abre a porta e deixa o serviço marcado como conectado.
|
|||
|
|
/// </summary>
|
|||
|
|
public async Task<bool> ScanAndConnectAsync(int timeoutPorPortaMs = 1500, CancellationToken ct = default)
|
|||
|
|
{
|
|||
|
|
// Se já está conectado, não precisa procurar de novo
|
|||
|
|
if (IsConnected)
|
|||
|
|
return true;
|
|||
|
|
|
|||
|
|
var portas = SerialPort.GetPortNames()
|
|||
|
|
.OrderBy(p => p)
|
|||
|
|
.ToArray();
|
|||
|
|
|
|||
|
|
foreach (var portName in portas)
|
|||
|
|
{
|
|||
|
|
if (ct.IsCancellationRequested)
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
bool achou = await TryConnectOnPortAsync(portName, timeoutPorPortaMs, ct);
|
|||
|
|
if (achou)
|
|||
|
|
{
|
|||
|
|
await ConfigurarModulo();
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Tenta abrir uma porta específica, ler um pedaço de dados
|
|||
|
|
/// e verificar se tem "cara de GPS" (NMEA / UM982).
|
|||
|
|
/// </summary>
|
|||
|
|
private async Task<bool> TryConnectOnPortAsync(string portName, int timeoutMs, CancellationToken ct)
|
|||
|
|
{
|
|||
|
|
SerialPort porta = null;
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
porta = new SerialPort(portName, DefaultBaudRate)
|
|||
|
|
{
|
|||
|
|
ReadTimeout = timeoutMs,
|
|||
|
|
WriteTimeout = timeoutMs,
|
|||
|
|
NewLine = "\n"
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
porta.Open();
|
|||
|
|
|
|||
|
|
var nmea = $"gngga com3 1\r\n";
|
|||
|
|
porta.Write(Encoding.ASCII.GetBytes(nmea), 0, nmea.Length);
|
|||
|
|
|
|||
|
|
// Dá um tempinho pro módulo começar a cuspir NMEA
|
|||
|
|
await Task.Delay(200, ct);
|
|||
|
|
|
|||
|
|
string recebido = await LerAmostraAsync(porta, timeoutMs, ct);
|
|||
|
|
|
|||
|
|
if (EhGpsUm982OuNmea(recebido))
|
|||
|
|
{
|
|||
|
|
// Mantém essa porta como porta oficial do GPS
|
|||
|
|
PortaGps = porta; // não fecha
|
|||
|
|
PortaGps.DataReceived -= PortaGPS_DataReceived;
|
|||
|
|
PortaGps.DataReceived += PortaGPS_DataReceived;
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Não era GPS nessa porta, fecha
|
|||
|
|
porta.Close();
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
catch
|
|||
|
|
{
|
|||
|
|
try { porta?.Close(); } catch { }
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Lê alguns bytes da porta durante um tempo máximo.
|
|||
|
|
/// </summary>
|
|||
|
|
private async Task<string> LerAmostraAsync(SerialPort porta, int timeoutMs, CancellationToken ct)
|
|||
|
|
{
|
|||
|
|
var inicio = DateTime.UtcNow;
|
|||
|
|
var buffer = new StringBuilder();
|
|||
|
|
|
|||
|
|
while ((DateTime.UtcNow - inicio).TotalMilliseconds < timeoutMs && !ct.IsCancellationRequested)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
// Quantos bytes tem disponíveis?
|
|||
|
|
int bytes = porta.BytesToRead;
|
|||
|
|
if (bytes > 0)
|
|||
|
|
{
|
|||
|
|
byte[] temp = new byte[bytes];
|
|||
|
|
int lidos = porta.Read(temp, 0, bytes);
|
|||
|
|
if (lidos > 0)
|
|||
|
|
{
|
|||
|
|
buffer.Append(Encoding.ASCII.GetString(temp, 0, lidos));
|
|||
|
|
// Se já tem um "$G" da vida, já é o bastante
|
|||
|
|
if (buffer.ToString().Contains("$GP") ||
|
|||
|
|
buffer.ToString().Contains("$GN") ||
|
|||
|
|
buffer.ToString().Contains("$GNGGA") ||
|
|||
|
|
buffer.ToString().Contains("$GNTHS"))
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
await Task.Delay(50, ct);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return buffer.ToString();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Lógica de detecção baseada no código antigo:
|
|||
|
|
/// procura por sentenças NMEA típicas do módulo.
|
|||
|
|
/// </summary>
|
|||
|
|
private bool EhGpsUm982OuNmea(string recebido)
|
|||
|
|
{
|
|||
|
|
if (string.IsNullOrEmpty(recebido))
|
|||
|
|
return false;
|
|||
|
|
|
|||
|
|
// Mesmos "marcadores" que você usava no SerialService.ProcurarDispositivoGPS
|
|||
|
|
// ($GPTXT, $GPRMC, $GPGGA, $GPGLL, $GNGGA, $GPVTG, $GPGSV, $GNTHS etc.)
|
|||
|
|
return
|
|||
|
|
recebido.Contains("$GPTXT") ||
|
|||
|
|
recebido.Contains("$GPRMC") ||
|
|||
|
|
recebido.Contains("$GPGGA") ||
|
|||
|
|
recebido.Contains("$GPGLL") ||
|
|||
|
|
recebido.Contains("$GNGGA") ||
|
|||
|
|
recebido.Contains("$GPVTG") ||
|
|||
|
|
recebido.Contains("$GPGSV") ||
|
|||
|
|
recebido.Contains("$GNTHS");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Fecha a porta do GPS (se estiver aberta).
|
|||
|
|
/// </summary>
|
|||
|
|
public void Disconnect()
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (PortaGps != null)
|
|||
|
|
{
|
|||
|
|
if (PortaGps.IsOpen)
|
|||
|
|
PortaGps.Close();
|
|||
|
|
PortaGps.Dispose();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch { }
|
|||
|
|
finally
|
|||
|
|
{
|
|||
|
|
PortaGps = null;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
private bool InverterHeading = false;
|
|||
|
|
private bool LoopRTK_Ntrip = false;
|
|||
|
|
private bool CorrecaoRTK_Ntrip = false;
|
|||
|
|
public int TaxaAmostragemHz = 5;
|
|||
|
|
private int rtk_timeout = 60;
|
|||
|
|
public GPSModel UltimaLeitura = new GPSModel();
|
|||
|
|
public BaseFixService BaseFix;
|
|||
|
|
private GeoLeverArm LeverArm;
|
|||
|
|
|
|||
|
|
private readonly StringBuilder _nmeaBuffer = new();
|
|||
|
|
private bool _nmeaInSentence = false;
|
|||
|
|
private bool _nmeaWaitingType = false;
|
|||
|
|
private readonly object _lock = new();
|
|||
|
|
private List<byte> _rtcmMsg = new();
|
|||
|
|
private int _rtcmTotalBytes = -1;
|
|||
|
|
|
|||
|
|
public async Task ConfigurarModulo()
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Iniciando configuração do módulo GPS...");
|
|||
|
|
LeverArm = new GeoLeverArm(VariaveisControleOperacao.LeverArmFrontal, VariaveisControleOperacao.LeverArmLateral);
|
|||
|
|
BaseFix = new BaseFixService(PortaGps, this);
|
|||
|
|
bool sucesso = await BaseFix.FixarBaseViaNtripAsync(
|
|||
|
|
portaSaida: "com3",
|
|||
|
|
portaEntrada: "com3",
|
|||
|
|
startNtrip: async () =>
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Iniciando correção NTRIP...");
|
|||
|
|
CorrecaoRTK_Ntrip = true;
|
|||
|
|
Task.Run(async () => await AplicarCorrecaoRTK_Ntrip());
|
|||
|
|
},
|
|||
|
|
stopNtrip: async () =>
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Parando correção NTRIP...");
|
|||
|
|
CorrecaoRTK_Ntrip = false;
|
|||
|
|
},
|
|||
|
|
segsFixEstavel: 120
|
|||
|
|
);
|
|||
|
|
if (!sucesso)
|
|||
|
|
{
|
|||
|
|
await ConfigurarModuloBase(tempo_fixacao: 60, porta_saida: "com3");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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 {porta_usb} 115200\r\n",
|
|||
|
|
$"config {porta_saida} 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)
|
|||
|
|
{
|
|||
|
|
byte[] bytes = Encoding.ASCII.GetBytes(cmd);
|
|||
|
|
PortaGps.Write(bytes, 0, bytes.Length);
|
|||
|
|
await Task.Delay(500);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void PortaGPS_DataReceived(object sender, SerialDataReceivedEventArgs e)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (!PortaGps.IsOpen)
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
int bytesToRead = PortaGps.BytesToRead;
|
|||
|
|
if (bytesToRead <= 0)
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
byte[] buffer = new byte[bytesToRead];
|
|||
|
|
PortaGps.Read(buffer, 0, bytesToRead);
|
|||
|
|
|
|||
|
|
lock (_lock)
|
|||
|
|
{
|
|||
|
|
for (int i = 0; i < buffer.Length; i++)
|
|||
|
|
{
|
|||
|
|
byte b = buffer[i];
|
|||
|
|
|
|||
|
|
// 1) Tratar NMEA (ASCII, linha por linha)
|
|||
|
|
TratarByteNmea(b);
|
|||
|
|
|
|||
|
|
// 2) Tratar RTCM (binário, mensagem por mensagem)
|
|||
|
|
TratarByteRtcm(b);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|||
|
|
UltimaLeitura.Inicializado = true;
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Erro ao processar dados da porta serial: {ex.Message}");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void TratarByteNmea(byte b)
|
|||
|
|
{
|
|||
|
|
// 1) Detectar início potencial de NMEA
|
|||
|
|
if (b == (byte)'$')
|
|||
|
|
{
|
|||
|
|
// Começa uma possível sentença
|
|||
|
|
_nmeaBuffer.Clear();
|
|||
|
|
_nmeaBuffer.Append('$');
|
|||
|
|
_nmeaWaitingType = true; // próximo byte decide se é NMEA mesmo
|
|||
|
|
_nmeaInSentence = false;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2) Logo após o '$', validar se é realmente NMEA ($G...)
|
|||
|
|
if (_nmeaWaitingType)
|
|||
|
|
{
|
|||
|
|
_nmeaWaitingType = false;
|
|||
|
|
|
|||
|
|
if (b == (byte)'G') // Aceitamos apenas $G...
|
|||
|
|
{
|
|||
|
|
_nmeaBuffer.Append('G');
|
|||
|
|
_nmeaInSentence = true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Não é $G => descarta, era lixo dentro de RTCM
|
|||
|
|
_nmeaBuffer.Clear();
|
|||
|
|
_nmeaInSentence = false;
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 3) Se ainda não estamos dentro de uma sentença NMEA, ignora
|
|||
|
|
if (!_nmeaInSentence)
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
// 4) Já estamos no meio de uma sentença NMEA válida ($G...)
|
|||
|
|
// Ignora CR
|
|||
|
|
if (b == (byte)'\r')
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
// Fim de linha: processar sentença completa
|
|||
|
|
if (b == (byte)'\n')
|
|||
|
|
{
|
|||
|
|
string linha = _nmeaBuffer.ToString();
|
|||
|
|
_nmeaBuffer.Clear();
|
|||
|
|
_nmeaInSentence = false;
|
|||
|
|
|
|||
|
|
if (!string.IsNullOrWhiteSpace(linha))
|
|||
|
|
{
|
|||
|
|
ProcessarDadosNMEA(linha.Trim());
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Apenas caracteres ASCII “visíveis”
|
|||
|
|
if (b >= 0x20 && b <= 0x7E)
|
|||
|
|
{
|
|||
|
|
_nmeaBuffer.Append((char)b);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// (Opcional) se der algum BO e a frase ficar gigante, reseta:
|
|||
|
|
if (_nmeaBuffer.Length > 200)
|
|||
|
|
{
|
|||
|
|
_nmeaBuffer.Clear();
|
|||
|
|
_nmeaInSentence = false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void TratarByteRtcm(byte b)
|
|||
|
|
{
|
|||
|
|
// Se ainda não começamos uma mensagem, procuramos pelo preâmbulo 0xD3
|
|||
|
|
if (_rtcmMsg.Count == 0)
|
|||
|
|
{
|
|||
|
|
if (b != 0xD3)
|
|||
|
|
return; // ignora até achar 0xD3
|
|||
|
|
|
|||
|
|
_rtcmMsg.Add(b); // adiciona preâmbulo
|
|||
|
|
_rtcmTotalBytes = -1;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
_rtcmMsg.Add(b);
|
|||
|
|
|
|||
|
|
// Quando tivermos 3 bytes, conseguimos saber o tamanho do payload
|
|||
|
|
if (_rtcmMsg.Count == 3 && _rtcmTotalBytes < 0)
|
|||
|
|
{
|
|||
|
|
int len = ((_rtcmMsg[1] & 0x03) << 8) | _rtcmMsg[2]; // 10 bits de tamanho
|
|||
|
|
_rtcmTotalBytes = 3 + len + 3; // header (3) + payload (len) + crc (3)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Quando chegarmos ao tamanho esperado, fechamos a mensagem
|
|||
|
|
if (_rtcmTotalBytes > 0 && _rtcmMsg.Count == _rtcmTotalBytes)
|
|||
|
|
{
|
|||
|
|
byte[] msg = _rtcmMsg.ToArray();
|
|||
|
|
|
|||
|
|
// Aqui você já tem UMA mensagem RTCM completa
|
|||
|
|
OnRtcmMessage(msg);
|
|||
|
|
|
|||
|
|
// Reseta para esperar a próxima
|
|||
|
|
_rtcmMsg.Clear();
|
|||
|
|
_rtcmTotalBytes = -1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void OnRtcmMessage(byte[] msg)
|
|||
|
|
{
|
|||
|
|
//Models.Variaveis.MostrarLog($"RTCM msg recebida: {msg.Length} bytes");
|
|||
|
|
VariaveisControleOperacao.EnviarDadosCorrecaoRTCM(msg);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private void ProcessarDadosNMEA(string sentenca)
|
|||
|
|
{
|
|||
|
|
DateTime Agora = DateTime.Now;
|
|||
|
|
if (string.IsNullOrWhiteSpace(sentenca)) return;
|
|||
|
|
|
|||
|
|
//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);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Sentença desconhecida: {sentenca}");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
UltimaLeitura.UltimoComandoRespondido = Agora;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void ProcessarGPGGA(string sentenca)
|
|||
|
|
{
|
|||
|
|
string[] parts = sentenca.Split(',');
|
|||
|
|
|
|||
|
|
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|||
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|||
|
|
if (parts[2] != "" && parts[3] != "")
|
|||
|
|
{
|
|||
|
|
UltimaLeitura.Latitude = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2);
|
|||
|
|
}
|
|||
|
|
if (parts[4] != "" && parts[5] != "")
|
|||
|
|
{
|
|||
|
|
UltimaLeitura.Longitude = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3);
|
|||
|
|
}
|
|||
|
|
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 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] : "";
|
|||
|
|
|
|||
|
|
// 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}");
|
|||
|
|
|
|||
|
|
// Armazenar os valores na última leitura
|
|||
|
|
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|||
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|||
|
|
UltimaLeitura.LatitudeAnt = latitude;
|
|||
|
|
UltimaLeitura.Latitude = latitude;
|
|||
|
|
UltimaLeitura.LongitudeAnt = longitude;
|
|||
|
|
UltimaLeitura.Longitude = longitude;
|
|||
|
|
UltimaLeitura.Altitude = altMSL;
|
|||
|
|
UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
|
|||
|
|
UltimaLeitura.PrecisaoHorizontal = hdopVal;
|
|||
|
|
UltimaLeitura.NumeroSatelites = nsatelites;
|
|||
|
|
UltimaLeitura.QualidadeFix = (AgroBase.Models.Enums.TiposCorrecaoGPS)fixCode;
|
|||
|
|
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
|
|||
|
|
UltimaLeitura.BaseID = base_id;
|
|||
|
|
|
|||
|
|
// 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 == AgroBase.Models.Enums.TiposCorrecaoGPS.RTKFixo) // tem fix
|
|||
|
|
{
|
|||
|
|
UltimaLeitura.Lat0 = latitude;
|
|||
|
|
UltimaLeitura.Lon0 = longitude;
|
|||
|
|
UltimaLeitura.EnuOriginSet = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2) se já temos origem e um heading válido, aplica lever arm
|
|||
|
|
if (UltimaLeitura.EnuOriginSet)
|
|||
|
|
{
|
|||
|
|
(double latCor, double lonCorr) = LeverArm.FixLeverArmLatLon_Fast(latitude, longitude, UltimaLeitura.OrientacaoReal);
|
|||
|
|
UltimaLeitura.Latitude = latCor;
|
|||
|
|
UltimaLeitura.Longitude = lonCorr;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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);
|
|||
|
|
|
|||
|
|
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 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}");
|
|||
|
|
|
|||
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|||
|
|
|
|||
|
|
double.TryParse(cursoVerdadeiro, out double curso);
|
|||
|
|
UltimaLeitura.CursoVerdadeiro = curso;
|
|||
|
|
|
|||
|
|
double.TryParse(velocidadeSobreSoloKmh, out double velocidade);
|
|||
|
|
UltimaLeitura.Velocidade = velocidade;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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];
|
|||
|
|
|
|||
|
|
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 void ProcessarGNTHS(string sentenca)
|
|||
|
|
{
|
|||
|
|
//Console.WriteLine(sentenca);
|
|||
|
|
// Remove o caractere de início '$' e divide os campos
|
|||
|
|
var campos = sentenca.TrimStart('$').Split(',');
|
|||
|
|
|
|||
|
|
if (campos.Length < 2)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Sentença incompleta.");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (campos.Length > 2 && string.IsNullOrEmpty(campos[1]))
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
// Parsing dos campos
|
|||
|
|
double headingTrue = double.Parse(campos[1].Replace(".", ",")); // Campo <Heading>
|
|||
|
|
string status = campos[2].Split('*')[0]; // Campo <Status>
|
|||
|
|
string checksum = campos[2].Split('*')[1]; // Campo <Checksum>
|
|||
|
|
|
|||
|
|
UltimaLeitura.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|||
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|||
|
|
UltimaLeitura.OrientacaoReal = InverterHeading ? GPSUtils.NormalizarAngulo(headingTrue - 180.0) : headingTrue;
|
|||
|
|
UltimaLeitura.TipoOrientacao = status;
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Erro ao processar a sentença: {ex.Message}");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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;
|
|||
|
|
|
|||
|
|
UltimaLeitura.Latitude = lat;
|
|||
|
|
UltimaLeitura.Longitude = lon;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 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 = AgroBase.Models.Enums.TiposCorrecaoGPS.RTKFixo;
|
|||
|
|
else if (mode == "F") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.RTKFlutuante;
|
|||
|
|
else if (mode == "D") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.DGPS;
|
|||
|
|
else if (mode == "E") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.DeadReckoing;
|
|||
|
|
else if (mode == "A") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.Autonomo;
|
|||
|
|
else if (mode == "N") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.SemCorrecao;
|
|||
|
|
else UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.SemCorrecao;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
AtualizarCoordenadasGPS();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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
|
|||
|
|
AgroBase.Models.Enums.TiposDimensaoCorrecaoGPS modoSolucao = (AgroBase.Models.Enums.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 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;
|
|||
|
|
UltimaLeitura.Latitude = lat;
|
|||
|
|
UltimaLeitura.Longitude = lon;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 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 = AgroBase.Models.Enums.TiposCorrecaoGPS.RTKFixo;
|
|||
|
|
else if (mode == "F") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.RTKFlutuante;
|
|||
|
|
else if (mode == "D") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.DGPS;
|
|||
|
|
else if (mode == "E") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.DeadReckoing;
|
|||
|
|
else if (mode == "A") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.Autonomo;
|
|||
|
|
else if (mode == "N") UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.SemCorrecao;
|
|||
|
|
else UltimaLeitura.QualidadeFix = AgroBase.Models.Enums.TiposCorrecaoGPS.SemCorrecao;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
AtualizarCoordenadasGPS();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private async Task AplicarCorrecaoRTK_Ntrip()
|
|||
|
|
{
|
|||
|
|
if (!IsConnected || 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 = "vyNEF$5*"; // 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"))
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("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)
|
|||
|
|
{
|
|||
|
|
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");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Falha na conexão com NTRIP: {response}");
|
|||
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Erro na conexão RTK: {ex.Message}");
|
|||
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
LoopRTK_Ntrip = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void AtualizarCoordenadasGPS()
|
|||
|
|
{
|
|||
|
|
UltimaLeitura.AnguloCarroDefinido = UltimaLeitura.OrientacaoReal;
|
|||
|
|
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
|||
|
|
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
|||
|
|
|
|||
|
|
// Daqui pra baixo: jogar pra UI
|
|||
|
|
Application.Current.Dispatcher.BeginInvoke(new Action(() =>
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
((App)Application.Current).Shell.Main.AtualizarDadosTela_GNSS(UltimaLeitura, marker_id: VariaveisControleOperacao.BaseMarkerID, progresso: BaseFix.Progresso);
|
|||
|
|
((App)Application.Current).Shell.Main.AtualizarDadosTela_Heading(UltimaLeitura.AnguloCarroDefinido);
|
|||
|
|
}
|
|||
|
|
catch (Exception exUi)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog($"Erro ao atualizar UI GNSS: {exUi.Message}");
|
|||
|
|
}
|
|||
|
|
}));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public class BaseFixService
|
|||
|
|
{
|
|||
|
|
public BaseFixService(SerialPort Porta, GpsService _service)
|
|||
|
|
{
|
|||
|
|
_Porta = Porta;
|
|||
|
|
gpsService = _service;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private readonly SerialPort _Porta;
|
|||
|
|
private readonly GpsService gpsService;
|
|||
|
|
private int _lastReadHeartbeat = -1;
|
|||
|
|
public List<GgaFix> amostras_pos = new List<GgaFix>(1000);
|
|||
|
|
private DateTime? inicioProcesso = null;
|
|||
|
|
private DateTime? inicioFix = null;
|
|||
|
|
private DateTime? fimProcesso = null;
|
|||
|
|
private int segundosFixEstavel = 120;
|
|||
|
|
private int maxJanelaSegundos = 120;
|
|||
|
|
public double Progresso
|
|||
|
|
{
|
|||
|
|
get
|
|||
|
|
{
|
|||
|
|
double progresso = inicioFix is null ? 0 : fimProcesso != null ? 100 : (DateTime.UtcNow - inicioFix.Value).TotalSeconds / segundosFixEstavel * 100.0;
|
|||
|
|
return progresso;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
public double ProgressoGeral
|
|||
|
|
{
|
|||
|
|
get
|
|||
|
|
{
|
|||
|
|
double progresso = inicioProcesso is null ? 0 : (DateTime.UtcNow - inicioProcesso.Value).TotalSeconds / maxJanelaSegundos * 100.0;
|
|||
|
|
return progresso;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
public string ProgressoStr
|
|||
|
|
{
|
|||
|
|
get
|
|||
|
|
{
|
|||
|
|
string progresso = "";
|
|||
|
|
if (CorrecaoEmAndamento)
|
|||
|
|
{
|
|||
|
|
progresso = $"Recebendo correção RTK via Ntrip. Progresso geral: {ProgressoGeral.ToString("0.00")}%, Progresso correção: {Progresso.ToString("0.00")}%";
|
|||
|
|
}
|
|||
|
|
else if (CorrecaoAbsoluta && inicioProcesso.HasValue && fimProcesso.HasValue)
|
|||
|
|
{
|
|||
|
|
progresso = $"Correção absoluta concluída com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds.ToString("0.00")} segundos";
|
|||
|
|
}
|
|||
|
|
else if (inicioProcesso.HasValue && fimProcesso.HasValue)
|
|||
|
|
{
|
|||
|
|
progresso = $"Correção absoluta falhou com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds.ToString("0.00")} segundos";
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
progresso = $"Correção absoluta não realizada";
|
|||
|
|
}
|
|||
|
|
return progresso;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
public bool CorrecaoAbsoluta = false;
|
|||
|
|
public bool CorrecaoEmAndamento = false;
|
|||
|
|
public bool FixLiberado = false;
|
|||
|
|
|
|||
|
|
// ===== 1) Função principal =====
|
|||
|
|
public async Task<bool> FixarBaseViaNtripAsync(string portaUsb = "com3", string portaEntrada = "com2", string portaSaida = "com2", string baseId = "957", int segsFixEstavel = 120, int maxJanelaSegs = 600, double madK = 3.5, Func<Task> startNtrip = null, Func<Task> stopNtrip = null)
|
|||
|
|
{
|
|||
|
|
if (CorrecaoEmAndamento || !APIService.HasInternet)
|
|||
|
|
return false;
|
|||
|
|
|
|||
|
|
CorrecaoEmAndamento = true;
|
|||
|
|
fimProcesso = null;
|
|||
|
|
|
|||
|
|
segundosFixEstavel = segsFixEstavel;
|
|||
|
|
maxJanelaSegundos = maxJanelaSegs;
|
|||
|
|
|
|||
|
|
// 1.1 Config temporária como rover parado + NMEA
|
|||
|
|
await ConfigurarComoRoverParadoAsync(portaUsb, portaEntrada);
|
|||
|
|
|
|||
|
|
// 1.2 Ligar NTRIP (injeta RTCM na portaEntrada)
|
|||
|
|
if (startNtrip != null) await startNtrip();
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
// 2) Esperar FIX sustentado e coletar GNGGA
|
|||
|
|
var amostras = await EsperarFixEAmostrarAsync();
|
|||
|
|
|
|||
|
|
if (amostras.Count < 10)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Poucas amostras de RTK FIX coletadas. Tente aumentar o tempo ou verificar sinais.");
|
|||
|
|
CorrecaoAbsoluta = false;
|
|||
|
|
return CorrecaoAbsoluta;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 3) Filtro robusto (MAD) + média final
|
|||
|
|
var (lat, lon, h, nAmostras) = FiltrarEAgrupar(amostras, madK);
|
|||
|
|
|
|||
|
|
// 4) Alternar para base FIX + perfil RTCM
|
|||
|
|
await AplicarBaseFixAsync(portaUsb, portaSaida, baseId, lat, lon, h);
|
|||
|
|
|
|||
|
|
Models.Variaveis.MostrarLog($"[BASE/FIX] Coordenadas aplicadas (n={nAmostras}):");
|
|||
|
|
Models.Variaveis.MostrarLog($" lat = {lat:0.000000000}, lon = {lon:0.000000000}, h = {h:0.000}");
|
|||
|
|
CorrecaoAbsoluta = true;
|
|||
|
|
return CorrecaoAbsoluta;
|
|||
|
|
}
|
|||
|
|
finally
|
|||
|
|
{
|
|||
|
|
if (stopNtrip != null) await stopNtrip();
|
|||
|
|
fimProcesso = DateTime.UtcNow;
|
|||
|
|
CorrecaoEmAndamento = false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 1.1 Rover parado + NMEA + limpar logs =====
|
|||
|
|
private async Task ConfigurarComoRoverParadoAsync(string portaUsb, string portaEntrada)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Configurando base como modo rover parado...");
|
|||
|
|
string freq = "1.0";
|
|||
|
|
string[] cmds = {
|
|||
|
|
// Ajuste de bauds
|
|||
|
|
$"config {portaUsb} 115200\r\n",
|
|||
|
|
$"config {portaEntrada} 115200\r\n",
|
|||
|
|
|
|||
|
|
// Limpa logs
|
|||
|
|
$"unlog com1\r\n",
|
|||
|
|
$"unlog com2\r\n",
|
|||
|
|
$"unlog com3\r\n",
|
|||
|
|
|
|||
|
|
// Rover parado (vamos usar NTRIP p/ obter FIX)
|
|||
|
|
$"mode rover uav\r\n",
|
|||
|
|
|
|||
|
|
// NMEA na USB
|
|||
|
|
$"gngga {portaUsb} {freq}\r\n",
|
|||
|
|
$"gpths {portaUsb} {freq}\r\n",
|
|||
|
|
|
|||
|
|
$"saveconfig\r\n"
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
await Task.Delay(1000);
|
|||
|
|
foreach (var c in cmds)
|
|||
|
|
{
|
|||
|
|
var b = Encoding.ASCII.GetBytes(c);
|
|||
|
|
_Porta.Write(b, 0, b.Length);
|
|||
|
|
await Task.Delay(250);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 2) Coleta GNGGA com FIX sustentado =====
|
|||
|
|
private async Task<List<GgaFix>> EsperarFixEAmostrarAsync()
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Inciando coleta de dados...");
|
|||
|
|
|
|||
|
|
amostras_pos = new List<GgaFix>(1000);
|
|||
|
|
inicioProcesso = DateTime.UtcNow;
|
|||
|
|
inicioFix = null;
|
|||
|
|
|
|||
|
|
// Você já deve ter um leitor da COM que devolve linhas NMEA.
|
|||
|
|
// Abaixo, vamos supor um método async que lê GGA parseado.
|
|||
|
|
while (ProgressoGeral < 100)
|
|||
|
|
{
|
|||
|
|
// Lê próxima sentença (bloqueante/assíncrono)
|
|||
|
|
var gga = await LerProximoGgaAsync(); // implemente no seu stack
|
|||
|
|
|
|||
|
|
if (gga is null) continue;
|
|||
|
|
|
|||
|
|
// Considera "RTK FIX" como qualidade válida
|
|||
|
|
if (!FixLiberado || !new List<TiposCorrecaoGPS>() { TiposCorrecaoGPS.RTKFixo }.Contains(gga.FixQuality))
|
|||
|
|
{
|
|||
|
|
inicioFix = null; // reset
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Marca início da janela de FIX estável
|
|||
|
|
if (inicioFix is null)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("RTK Fixo definido! Iniciando coleta de dados com precisão...");
|
|||
|
|
inicioFix = DateTime.UtcNow;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
amostras_pos.Add(gga);
|
|||
|
|
Models.Variaveis.MostrarLog("Nova coordenada registrada!");
|
|||
|
|
|
|||
|
|
// Verifica se já temos FIX estável pelo período necessário
|
|||
|
|
if (Progresso >= 100)
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return amostras_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 2.1) Ajuste a assinatura se quiser passar timeout e CT de fora
|
|||
|
|
private async Task<GgaFix> LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default)
|
|||
|
|
{
|
|||
|
|
var sw = System.Diagnostics.Stopwatch.StartNew();
|
|||
|
|
int startHb = System.Threading.Volatile.Read(ref _lastReadHeartbeat);
|
|||
|
|
|
|||
|
|
// 1) Espera um novo heartbeat
|
|||
|
|
while (!ct.IsCancellationRequested)
|
|||
|
|
{
|
|||
|
|
int currentHb = gpsService.UltimaLeitura.Heartbeat; // <- leitura normal da propriedade
|
|||
|
|
if (currentHb != startHb) break;
|
|||
|
|
|
|||
|
|
if (sw.ElapsedMilliseconds >= timeoutMs)
|
|||
|
|
//throw new TimeoutException("Timeout aguardando nova leitura GGA.");
|
|||
|
|
return new GgaFix(DateTime.UtcNow, 0, 0, 0, TiposCorrecaoGPS.SemCorrecao);
|
|||
|
|
|
|||
|
|
await Task.Delay(75, ct).ConfigureAwait(false);
|
|||
|
|
}
|
|||
|
|
ct.ThrowIfCancellationRequested();
|
|||
|
|
|
|||
|
|
// 2) Snapshot consistente
|
|||
|
|
while (true)
|
|||
|
|
{
|
|||
|
|
var ultimaLeitura = gpsService.UltimaLeitura;
|
|||
|
|
int hbBefore = ultimaLeitura.Heartbeat;
|
|||
|
|
|
|||
|
|
// Captura TODOS os campos que você precisa em variáveis locais
|
|||
|
|
DateTime tsUtc = ultimaLeitura.DataHora.ToUniversalTime();
|
|||
|
|
double lat = ultimaLeitura.Latitude;
|
|||
|
|
double lon = ultimaLeitura.Longitude;
|
|||
|
|
double altElips = ultimaLeitura.AltitudeElipsoidal; // garanta que já é elipsoidal no parser
|
|||
|
|
var fixQual = ultimaLeitura.QualidadeFix; // enum? ok.
|
|||
|
|
|
|||
|
|
int hbAfter = ultimaLeitura.Heartbeat;
|
|||
|
|
|
|||
|
|
// Se o heartbeat não mudou durante o snapshot, temos dados coerentes
|
|||
|
|
if (hbBefore == hbAfter)
|
|||
|
|
{
|
|||
|
|
// marca como lido
|
|||
|
|
System.Threading.Volatile.Write(ref _lastReadHeartbeat, hbAfter);
|
|||
|
|
|
|||
|
|
// monta o DTO
|
|||
|
|
return new GgaFix(
|
|||
|
|
tsUtc: tsUtc,
|
|||
|
|
latDeg: lat,
|
|||
|
|
lonDeg: lon,
|
|||
|
|
altElipsoidalM: altElips,
|
|||
|
|
fixQuality: fixQual
|
|||
|
|
);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// caso contrário, alguém atualizou no meio — tenta de novo rápido
|
|||
|
|
await Task.Yield();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 3) Filtro robusto (MAD) + média =====
|
|||
|
|
private (double lat, double lon, double h, int n) FiltrarEAgrupar(List<GgaFix> amostras, double madK = 3.5)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Filtrando dados aferidos...");
|
|||
|
|
// Medianas
|
|||
|
|
var lats = amostras.Select(a => a.LatDeg).OrderBy(x => x).ToArray();
|
|||
|
|
var lons = amostras.Select(a => a.LonDeg).OrderBy(x => x).ToArray();
|
|||
|
|
var hs = amostras.Select(a => a.AltElipsoidalM).OrderBy(x => x).ToArray();
|
|||
|
|
|
|||
|
|
double medLat = Mediana(lats);
|
|||
|
|
double medLon = Mediana(lons);
|
|||
|
|
double medH = Mediana(hs);
|
|||
|
|
|
|||
|
|
// Desvios absolutos da mediana (MAD)
|
|||
|
|
var dLat = amostras.Select(a => Math.Abs(a.LatDeg - medLat)).OrderBy(x => x).ToArray();
|
|||
|
|
var dLon = amostras.Select(a => Math.Abs(a.LonDeg - medLon)).OrderBy(x => x).ToArray();
|
|||
|
|
var dH = amostras.Select(a => Math.Abs(a.AltElipsoidalM - medH)).OrderBy(x => x).ToArray();
|
|||
|
|
|
|||
|
|
double madLat = Mediana(dLat) + 1e-12;
|
|||
|
|
double madLon = Mediana(dLon) + 1e-12;
|
|||
|
|
double madHgt = Mediana(dH) + 1e-12;
|
|||
|
|
|
|||
|
|
// Filtra outliers (|x - med| / MAD <= madK)
|
|||
|
|
var filtradas = amostras.Where(a =>
|
|||
|
|
(Math.Abs(a.LatDeg - medLat) / madLat) <= madK &&
|
|||
|
|
(Math.Abs(a.LonDeg - medLon) / madLon) <= madK &&
|
|||
|
|
(Math.Abs(a.AltElipsoidalM - medH) / madHgt) <= madK
|
|||
|
|
).ToList();
|
|||
|
|
|
|||
|
|
// Média final
|
|||
|
|
double lat = filtradas.Average(a => a.LatDeg);
|
|||
|
|
double lon = filtradas.Average(a => a.LonDeg);
|
|||
|
|
double h = filtradas.Average(a => a.AltElipsoidalM);
|
|||
|
|
|
|||
|
|
return (lat, lon, h, filtradas.Count);
|
|||
|
|
|
|||
|
|
double Mediana(double[] arr)
|
|||
|
|
{
|
|||
|
|
int n = arr.Length;
|
|||
|
|
if (n == 0) return double.NaN;
|
|||
|
|
return (n % 2 == 1) ? arr[n / 2] : 0.5 * (arr[n / 2 - 1] + arr[n / 2]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 4) Aplicar base FIX + RTCM + save =====
|
|||
|
|
public async Task AplicarBaseFixAsync(string portaUsb, string portaSaida, string baseId, double latDeg, double lonDeg, double hEllipsM)
|
|||
|
|
{
|
|||
|
|
Models.Variaveis.MostrarLog("Aplicando dados de correção...");
|
|||
|
|
var ci = System.Globalization.CultureInfo.InvariantCulture;
|
|||
|
|
// Desliga logs antes de trocar modo
|
|||
|
|
string[] pre = {
|
|||
|
|
$"unlog com1\r\n",
|
|||
|
|
$"unlog com2\r\n",
|
|||
|
|
$"unlog com3\r\n"
|
|||
|
|
};
|
|||
|
|
foreach (var c in pre) { _Porta.Write(Encoding.ASCII.GetBytes(c), 0, c.Length); await Task.Delay(150); }
|
|||
|
|
|
|||
|
|
var latStr = latDeg.ToString("0.000000000", ci);
|
|||
|
|
var lonStr = lonDeg.ToString("0.000000000", ci);
|
|||
|
|
var hStr = hEllipsM.ToString("0.000", ci);
|
|||
|
|
|
|||
|
|
var fix = Encoding.ASCII.GetBytes($"mode base {baseId} {latStr} {lonStr} {hStr}\r\n");
|
|||
|
|
_Porta.Write(fix, 0, fix.Length);
|
|||
|
|
await Task.Delay(250);
|
|||
|
|
|
|||
|
|
// Reativar RTCM no canal de saída para o LoRa
|
|||
|
|
string[] rtcmCmds = {
|
|||
|
|
// RTCM perfil (comece leve; ative mais constelações se o LoRa aguentar)
|
|||
|
|
$"RTCM1006 {portaSaida} 10\r\n",
|
|||
|
|
$"RTCM1033 {portaSaida} 30\r\n",
|
|||
|
|
$"RTCM1074 {portaSaida} 1\r\n", // GPS MSM4
|
|||
|
|
$"RTCM1124 {portaSaida} 1\r\n", // BeiDou MSM4
|
|||
|
|
|
|||
|
|
// (Opcional) ativar mais constelações:
|
|||
|
|
$"RTCM1094 {portaSaida} 1\r\n", // Galileo MSM4
|
|||
|
|
$"RTCM1084 {portaSaida} 1\r\n", // GLONASS MSM4
|
|||
|
|
//$"RTCM1230 {portaSaida} 10\r\n",// Bias GLONASS (OBRIGATÓRIO se 1084 estiver ativo)
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
foreach (var c in rtcmCmds) { var b = Encoding.ASCII.GetBytes(c); _Porta.Write(b, 0, b.Length); await Task.Delay(200); }
|
|||
|
|
|
|||
|
|
// NMEA mínimo na USB p/ debug
|
|||
|
|
var nmea = $"gngga {portaUsb} 1\r\n";
|
|||
|
|
_Porta.Write(Encoding.ASCII.GetBytes(nmea), 0, nmea.Length);
|
|||
|
|
await Task.Delay(150);
|
|||
|
|
|
|||
|
|
// Persistir
|
|||
|
|
var save = "saveconfig\r\n";
|
|||
|
|
_Porta.Write(Encoding.ASCII.GetBytes(save), 0, save.Length);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|