Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
agrobot_base/AgroBase/OperationControl/Services/GpsService.cs

4149 lines
147 KiB
C#
Raw Blame History

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;
using System.Threading.Tasks;
using System.Timers;
using System.Windows;
using AgroBase.Models;
using AgroBase.Services;
using OperationControl.Models;
using static AgroBase.Models.Enums;
using static OperationControl.Services.BaseFixService;
using Application = System.Windows.Application;
namespace OperationControl.Services
{
/// <summary>
/// Serviço do UM982 da base.
///
/// Princípios desta implementação:
/// 1) O callback serial apenas lê, separa e enfileira. Nunca publica na rede.
/// 2) NMEA e RTCM são demultiplexados por uma única máquina de estados.
/// 3) RTCM só é encaminhado após validação CRC24Q.
/// 4) A fila RTCM mantém apenas a mensagem mais nova de cada tipo.
/// 5) Mensagens RTCM antigas são descartadas antes da publicação.
/// 6) Reconectar a USB não altera silenciosamente o papel do GNSS.
/// 7) Todas as escritas no UM982 passam por um único lock.
/// 8) O cliente NTRIP possui cancelamento real e não mistura StreamReader com dados binários.
/// </summary>
public sealed class GpsService : IDisposable
{
// =========================================================
// CONFIGURAÇÃO GERAL
// =========================================================
private const int DefaultBaudRate = 115200;
private static readonly TimeSpan SerialSilenceReconnectTimeout = TimeSpan.FromSeconds(15);
private static readonly TimeSpan SerialReconnectGrace = TimeSpan.FromSeconds(10);
private static readonly TimeSpan RtcmAssemblyTimeout = TimeSpan.FromSeconds(1.5);
private static readonly TimeSpan NmeaAssemblyTimeout = TimeSpan.FromSeconds(2.0);
private static readonly TimeSpan RtcmMaxQueueAge = TimeSpan.FromSeconds(2.5);
private readonly System.Timers.Timer _tmrCheck = new(2000) { AutoReset = true };
private readonly SemaphoreSlim _scanGate = new(1, 1);
private readonly SemaphoreSlim _serialWriteGate = new(1, 1);
private readonly object _portLock = new();
private readonly object _parserLock = new();
private readonly object _modelLock = new();
private readonly object _rtcmQueueLock = new();
private readonly object _ntripLock = new();
private readonly CancellationTokenSource _lifetimeCts = new();
private readonly SemaphoreSlim _rtcmSignal = new(0);
private readonly Dictionary<int, RtcmEnvelope> _rtcmLatestByType = new();
private readonly Task _rtcmPublisherTask;
private int _timerRunning;
private int _disposed;
private int _uiUpdatePending;
private double _serialConnectedMono;
private double _lastSerialRxMono;
private double _lastValidNmeaMono;
private double _lastValidGgaMono;
private double _lastValidRtcmMono;
private double _lastForwardedRtcmMono;
private long _ggaSequence;
private long _rtcmSequence;
public GpsService()
{
BaseFix = new BaseFixService(null, this);
_rtcmPublisherTask = Task.Run(
() => RtcmPublisherLoopAsync(_lifetimeCts.Token),
_lifetimeCts.Token);
_tmrCheck.Elapsed += (_, __) => _ = CheckConnectionAsync();
_tmrCheck.Start();
}
// =========================================================
// ESTADO PÚBLICO / COMPATIBILIDADE
// =========================================================
public SerialPort PortaGps { get; private set; }
public bool IsConnected
{
get
{
lock (_portLock)
return PortaGps != null && PortaGps.IsOpen;
}
}
public string PortName
{
get
{
lock (_portLock)
return PortaGps?.PortName;
}
}
private bool InverterHeading = false;
private volatile bool CorrecaoRTK_Ntrip = false;
private int TempoSurveryIn = 120;
public volatile bool Ntrip_Conectado = false;
public int TaxaAmostragemHz = 5;
private int rtk_timeout = 60;
public GPSModel UltimaLeitura = new GPSModel();
public BaseFixService BaseFix;
public GeoLeverArm LeverArm = new GeoLeverArm();
/// <summary>
/// Estratégia de fonte NTRIP.
///
/// IbgeOnly:
/// usa somente o RBMC-IP.
///
/// Rtk2goOnly:
/// usa somente o RTK2go.
///
/// IbgeThenRtk2go:
/// tenta primeiro todos os candidatos válidos do IBGE, em ordem de distância;
/// se nenhum permanecer ativo, tenta RTK2go também em ordem de distância.
/// </summary>
public NtripProviderMode NtripMode { get; set; } = NtripProviderMode.IbgeThenRtk2go;
/// <summary>
/// Quando true, baixa a sourcetable do caster e escolhe automaticamente
/// os mountpoints mais próximos da posição GNSS atual.
///
/// Quando false, usa apenas NtripMountpoint / NtripRtk2goMountpoint.
/// </summary>
public bool NtripAutoSelecionarMountpoint { get; set; } = true;
/// <summary>
/// Quantidade máxima de estações que serão tentadas por provedor,
/// sempre ordenadas da menor para a maior distância.
/// </summary>
public int NtripMaxCandidatosPorProvedor { get; set; } = 5;
/// <summary>
/// Distância máxima para NTRIP em levantamento móvel.
/// É deliberadamente maior que o limite usado para fixação absoluta.
/// </summary>
public double NtripDistanciaMaximaKm { get; set; } = 120.0;
/// <summary>
/// Distância máxima para usar uma estação na FIXAÇÃO ABSOLUTA DA BASE.
/// Mantida conservadora porque queremos coordenada absoluta confiável.
/// </summary>
public double NtripDistanciaMaximaFixacaoBaseKm { get; set; } = 80.0;
/// <summary>
/// Tempo máximo aguardando uma posição GGA recente antes de selecionar
/// automaticamente as estações.
///
/// Durante esta janela o serviço também reforça o comando GGA no UM982
/// caso ainda não tenha recebido uma posição válida.
/// </summary>
public int NtripEsperaPosicaoAutoSegundos { get; set; } = 15;
/// <summary>
/// Porta interna do UM982 usada para solicitar GGA durante a descoberta
/// automática de mountpoints. Deve acompanhar a porta USB configurada
/// no restante do serviço.
/// </summary>
public string NtripPortaGgaAutoSelecao { get; set; } = "com2";
/// <summary>
/// Cache da sourcetable. A lista de estações muda pouco e não precisa
/// ser baixada em todo reconnect.
/// </summary>
public int NtripSourcetableCacheSegundos { get; set; } = 600;
/// <summary>
/// Se uma sessão responder HTTP 200 mas não entregar nenhum byte de
/// correção dentro deste prazo, ela é abandonada e o próximo candidato
/// é tentado.
/// </summary>
public int NtripPrimeirosDadosTimeoutSegundos { get; set; } = 8;
/// <summary>
/// Tempo durante o qual um mountpoint que falhou fica temporariamente
/// fora da lista de tentativas. Evita repetir 401/404/503/timeouts em
/// sequência contra a mesma estação.
///
/// Use 0 para desabilitar o cooldown.
/// </summary>
public int NtripCooldownMountpointSegundos { get; set; } = 60;
/// <summary>
/// Libera RTK2go como fallback também durante FixarBaseViaNtripAsync().
/// Mesmo liberado, o seletor automático ainda aplica o limite
/// NtripDistanciaMaximaFixacaoBaseKm.
/// </summary>
public bool NtripRtk2goPermitidoParaFixacaoBase { get; set; } = false;
/// <summary>
/// Configuração IBGE / RBMC-IP.
/// NtripMountpoint é mantido como override/fallback manual.
/// </summary>
public string NtripHost { get; set; } = "gps-ntrip.ibge.gov.br";
public int NtripPort { get; set; } = 2101;
public string NtripMountpoint { get; set; } = "EESC0";
public string NtripUsername { get; set; } =
Environment.GetEnvironmentVariable("AGRO_NTRIP_USERNAME") ?? string.Empty;
public string NtripPassword { get; set; } =
Environment.GetEnvironmentVariable("AGRO_NTRIP_PASSWORD") ?? string.Empty;
/// <summary>
/// Configuração RTK2go.
/// NtripRtk2goMountpoint é mantido como override manual para diagnóstico.
/// Com seleção automática ativa ele normalmente não será usado.
/// </summary>
public string NtripRtk2goHost { get; set; } = "rtk2go.com";
public int NtripRtk2goPort { get; set; } = 2101;
public string NtripRtk2goMountpoint { get; set; } = "SPLI1";
public string NtripRtk2goUsername { get; set; } =
Environment.GetEnvironmentVariable("AGRO_RTK2GO_EMAIL") ?? string.Empty;
public string NtripRtk2goPassword { get; set; } = "none";
/// <summary>
/// Provedor/mountpoint atualmente conectados.
/// </summary>
public string NtripProviderAtivo { get; private set; } = string.Empty;
public string NtripMountpointAtivo { get; private set; } = string.Empty;
public double? NtripDistanciaMountpointAtivoKm { get; private set; }
/// <summary>
/// Configura credenciais somente em memória para a sessão atual.
/// Nenhum valor é gravado em arquivo por este método.
/// </summary>
public void ConfigurarCredenciaisNtripEmMemoria(
string ibgeUsuario = null,
string ibgeSenha = null,
string rtk2goEmail = null)
{
if (ibgeUsuario != null)
NtripUsername = ibgeUsuario.Trim();
if (ibgeSenha != null)
NtripPassword = ibgeSenha;
if (rtk2goEmail != null)
NtripRtk2goUsername = rtk2goEmail.Trim();
}
private GnssExpectedRole _expectedRole = GnssExpectedRole.PreserveCurrentConfiguration;
private BaseFixedConfiguration _lastBaseConfiguration;
// =========================================================
// MÉTRICAS
// =========================================================
private long _serialBytesReceived;
private long _serialReadErrors;
private long _nmeaValid;
private long _nmeaInvalid;
private long _nmeaChecksumErrors;
private long _rtcmValid;
private long _rtcmCrcErrors;
private long _rtcmInvalidLength;
private long _rtcmResyncs;
private long _rtcmQueued;
private long _rtcmReplaced;
private long _rtcmDroppedStale;
private long _rtcmForwarded;
private long _rtcmPublishErrors;
private long _serialReconnects;
private long _ntripBytesReceived;
private long _ntripReconnects;
private long _ntripErrors;
public GpsTransportMetrics GetTransportMetrics()
{
int queueDepth;
double oldestQueueAgeMs = 0;
lock (_rtcmQueueLock)
{
queueDepth = _rtcmLatestByType.Count;
if (queueDepth > 0)
{
double now = MonotonicNow();
oldestQueueAgeMs = _rtcmLatestByType.Values
.Max(x => Math.Max(0, (now - x.ReceivedMono) * 1000.0));
}
}
double nowMono = MonotonicNow();
return new GpsTransportMetrics
{
SerialConnected = IsConnected,
PortName = PortName,
SerialLastRxAgeMs = AgeMs(nowMono, _lastSerialRxMono),
LastValidNmeaAgeMs = AgeMs(nowMono, _lastValidNmeaMono),
LastValidGgaAgeMs = AgeMs(nowMono, _lastValidGgaMono),
LastValidRtcmAgeMs = AgeMs(nowMono, _lastValidRtcmMono),
LastForwardedRtcmAgeMs = AgeMs(nowMono, _lastForwardedRtcmMono),
SerialBytesReceived = Interlocked.Read(ref _serialBytesReceived),
SerialReadErrors = Interlocked.Read(ref _serialReadErrors),
NmeaValid = Interlocked.Read(ref _nmeaValid),
NmeaInvalid = Interlocked.Read(ref _nmeaInvalid),
NmeaChecksumErrors = Interlocked.Read(ref _nmeaChecksumErrors),
RtcmValid = Interlocked.Read(ref _rtcmValid),
RtcmCrcErrors = Interlocked.Read(ref _rtcmCrcErrors),
RtcmInvalidLength = Interlocked.Read(ref _rtcmInvalidLength),
RtcmResyncs = Interlocked.Read(ref _rtcmResyncs),
RtcmQueued = Interlocked.Read(ref _rtcmQueued),
RtcmReplaced = Interlocked.Read(ref _rtcmReplaced),
RtcmDroppedStale = Interlocked.Read(ref _rtcmDroppedStale),
RtcmForwarded = Interlocked.Read(ref _rtcmForwarded),
RtcmPublishErrors = Interlocked.Read(ref _rtcmPublishErrors),
RtcmQueueDepth = queueDepth,
RtcmOldestQueueAgeMs = oldestQueueAgeMs,
SerialReconnects = Interlocked.Read(ref _serialReconnects),
NtripConnected = Ntrip_Conectado,
NtripBytesReceived = Interlocked.Read(ref _ntripBytesReceived),
NtripReconnects = Interlocked.Read(ref _ntripReconnects),
NtripErrors = Interlocked.Read(ref _ntripErrors),
NtripProvider = NtripProviderAtivo,
NtripMountpoint = NtripMountpointAtivo,
NtripMountpointDistanceKm = NtripDistanciaMountpointAtivoKm,
ExpectedRole = _expectedRole.ToString(),
};
}
// =========================================================
// DESCOBERTA / RECONEXÃO SERIAL
// =========================================================
private async Task CheckConnectionAsync()
{
if (Interlocked.CompareExchange(ref _timerRunning, 1, 0) != 0)
return;
try
{
if (Volatile.Read(ref _disposed) != 0)
return;
bool connected = IsConnected;
double now = MonotonicNow();
if (connected)
{
bool graceEnded =
_serialConnectedMono > 0 &&
(now - _serialConnectedMono) >= SerialReconnectGrace.TotalSeconds;
double lastActivity = _lastSerialRxMono > 0
? _lastSerialRxMono
: _serialConnectedMono;
bool silentTooLong =
lastActivity > 0 &&
(now - lastActivity) >= SerialSilenceReconnectTimeout.TotalSeconds;
if (!graceEnded || !silentTooLong)
return;
Models.Variaveis.MostrarLog(
$"GNSS sem dados há {(now - _lastSerialRxMono):F1}s. Reiniciando conexão serial.");
DisconnectInternal();
}
bool found = await ScanAndConnectAsync(
timeoutPorPortaMs: 1500,
ct: _lifetimeCts.Token).ConfigureAwait(false);
if (found)
{
Models.Variaveis.MostrarLog($"GPS conectado na porta {PortName}");
await RestoreExpectedRoleAfterReconnectAsync(_lifetimeCts.Token).ConfigureAwait(false);
}
else
{
Models.Variaveis.MostrarLog("Nenhum GPS UM982 encontrado.");
}
}
catch (OperationCanceledException)
{
}
catch (Exception ex)
{
Models.Variaveis.MostrarLog($"Erro ao verificar conexão do GNSS: {ex.Message}");
}
finally
{
Interlocked.Exchange(ref _timerRunning, 0);
}
}
public async Task<bool> ScanAndConnectAsync(
int timeoutPorPortaMs = 1500,
CancellationToken ct = default)
{
if (IsConnected)
return true;
await _scanGate.WaitAsync(ct).ConfigureAwait(false);
try
{
if (IsConnected)
return true;
string[] portas = SerialPort.GetPortNames()
.OrderBy(p => p)
.ToArray();
foreach (string portName in portas)
{
ct.ThrowIfCancellationRequested();
if (await TryConnectOnPortAsync(portName, timeoutPorPortaMs, ct)
.ConfigureAwait(false))
{
Interlocked.Increment(ref _serialReconnects);
return true;
}
}
return false;
}
finally
{
_scanGate.Release();
}
}
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",
DtrEnable = false,
RtsEnable = false,
ReadBufferSize = 64 * 1024,
WriteBufferSize = 16 * 1024,
};
porta.Open();
if (!await EhUm982PorVersionAsync(porta, timeoutMs, ct).ConfigureAwait(false))
{
// Fallback não persistente: pede apenas GGA para confirmar que é GNSS.
byte[] nmea = Encoding.ASCII.GetBytes("gngga com2 1\r\n");
porta.Write(nmea, 0, nmea.Length);
await Task.Delay(200, ct).ConfigureAwait(false);
string sample = await LerAmostraAsync(porta, timeoutMs, ct).ConfigureAwait(false);
if (!EhGpsUm982OuNmea(sample))
{
porta.Close();
porta.Dispose();
return false;
}
}
RegisterConnectedPort(porta);
return true;
}
catch (OperationCanceledException)
{
try { porta?.Close(); } catch { }
try { porta?.Dispose(); } catch { }
throw;
}
catch
{
try { porta?.Close(); } catch { }
try { porta?.Dispose(); } catch { }
return false;
}
}
private void RegisterConnectedPort(SerialPort porta)
{
DisconnectInternal();
lock (_portLock)
{
PortaGps = porta;
PortaGps.DataReceived -= PortaGPS_DataReceived;
PortaGps.DataReceived += PortaGPS_DataReceived;
}
ResetParsers();
_serialConnectedMono = MonotonicNow();
_lastSerialRxMono = 0;
}
private async Task<string> LerAmostraAsync(
SerialPort porta,
int timeoutMs,
CancellationToken ct)
{
double start = MonotonicNow();
StringBuilder buffer = new();
while ((MonotonicNow() - start) * 1000.0 < timeoutMs)
{
ct.ThrowIfCancellationRequested();
try
{
int available = porta.BytesToRead;
if (available > 0)
{
byte[] temp = new byte[available];
int read = porta.Read(temp, 0, temp.Length);
if (read > 0)
{
buffer.Append(Encoding.ASCII.GetString(temp, 0, read));
string text = buffer.ToString();
if (text.Contains("UM982", StringComparison.OrdinalIgnoreCase) ||
text.Contains("$GP", StringComparison.OrdinalIgnoreCase) ||
text.Contains("$GN", StringComparison.OrdinalIgnoreCase))
{
break;
}
}
}
}
catch
{
break;
}
await Task.Delay(50, ct).ConfigureAwait(false);
}
return buffer.ToString();
}
private static bool EhGpsUm982OuNmea(string recebido)
{
if (string.IsNullOrEmpty(recebido))
return false;
return recebido.Contains("UM982", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPTXT", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPRMC", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPGGA", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPGLL", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GNGGA", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPVTG", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GPGSV", StringComparison.OrdinalIgnoreCase) ||
recebido.Contains("$GNTHS", StringComparison.OrdinalIgnoreCase);
}
private async Task<bool> EhUm982PorVersionAsync(
SerialPort porta,
int timeoutMs,
CancellationToken ct)
{
try
{
porta.DiscardInBuffer();
byte[] cmd = Encoding.ASCII.GetBytes("version\r\n");
porta.Write(cmd, 0, cmd.Length);
await Task.Delay(100, ct).ConfigureAwait(false);
string response = await LerAmostraAsync(porta, timeoutMs, ct).ConfigureAwait(false);
return !string.IsNullOrEmpty(response) &&
response.Contains("UM982", StringComparison.OrdinalIgnoreCase);
}
catch
{
return false;
}
}
public void Disconnect()
{
DisconnectInternal();
}
private void DisconnectInternal()
{
SerialPort port;
lock (_portLock)
{
port = PortaGps;
PortaGps = null;
}
if (port == null)
return;
try { port.DataReceived -= PortaGPS_DataReceived; } catch { }
try { if (port.IsOpen) port.Close(); } catch { }
try { port.Dispose(); } catch { }
}
private async Task RestoreExpectedRoleAfterReconnectAsync(CancellationToken ct)
{
// A regra mais importante é: uma reconexão nunca muda o papel do módulo
// para rover por conta própria.
if (_expectedRole == GnssExpectedRole.BaseFixed && _lastBaseConfiguration != null)
{
Models.Variaveis.MostrarLog("Restaurando configuração conhecida da base fixa após reconexão.");
await BaseFix.AplicarBaseFixAsync(
_lastBaseConfiguration.PortaUsb,
_lastBaseConfiguration.PortaSaida,
_lastBaseConfiguration.BaseId,
_lastBaseConfiguration.Latitude,
_lastBaseConfiguration.Longitude,
_lastBaseConfiguration.AltitudeElipsoidal,
ct).ConfigureAwait(false);
}
else if (_expectedRole == GnssExpectedRole.RoverTemporary)
{
Models.Variaveis.MostrarLog("Restaurando modo rover temporário após reconexão.");
await BaseFix.ConfigurarComoRoverParadoAsync("com2", "com2", ct)
.ConfigureAwait(false);
}
// PreserveCurrentConfiguration: não envia unlog, mode rover ou saveconfig.
}
// =========================================================
// ESCRITA SERIAL SERIALIZADA
// =========================================================
internal async Task WriteSerialAsync(
byte[] data,
CancellationToken ct = default)
{
if (data == null || data.Length == 0)
return;
await _serialWriteGate.WaitAsync(ct).ConfigureAwait(false);
try
{
SerialPort port;
lock (_portLock)
port = PortaGps;
if (port == null || !port.IsOpen)
throw new IOException("Porta do GNSS não está conectada.");
port.Write(data, 0, data.Length);
}
finally
{
_serialWriteGate.Release();
}
}
internal Task WriteSerialCommandAsync(
string command,
CancellationToken ct = default)
{
return WriteSerialAsync(Encoding.ASCII.GetBytes(command), ct);
}
internal async Task SendCommandsAsync(
IEnumerable<string> commands,
int delayMs,
CancellationToken ct = default)
{
foreach (string command in commands)
{
ct.ThrowIfCancellationRequested();
await WriteSerialCommandAsync(command, ct).ConfigureAwait(false);
if (delayMs > 0)
await Task.Delay(delayMs, ct).ConfigureAwait(false);
}
}
// =========================================================
// CONFIGURAÇÃO DO MÓDULO
// =========================================================
public async Task ConfigurarModulo(
bool fixar,
MetodoFixacaoBase metodo = MetodoFixacaoBase.Ntrip,
double? lat = null,
double? lon = null,
double? alt = null,
bool? offset = false)
{
Models.Variaveis.MostrarLog("Iniciando configuração do módulo GPS...");
BaseFix ??= new BaseFixService(PortaGps, this);
BaseFix.FixLiberado = fixar;
const string portaUsb = "com2";
const string portaSaida = "com2";
const string portaEntrada = "com2";
const string baseId = "957";
if (!fixar)
{
Models.Variaveis.MostrarLog(
"Configurando explicitamente o módulo como rover parado."
);
await BaseFix
.ConfigurarComoRoverParadoAsync(
portaUsb,
portaEntrada,
_lifetimeCts.Token
)
.ConfigureAwait(false);
DefinirPapelEsperado(
GnssExpectedRole.RoverTemporary
);
return;
}
/*
* Vamos iniciar uma configuração de BASE.
*
* Se estávamos fazendo levantamento móvel
* com NTRIP, encerramos a injeção RTCM antes
* de começar a enviar os comandos da base.
*
* No método NTRIP, FixarBaseViaNtripAsync()
* iniciará novamente a correção no momento
* correto.
*/
await StopNtripAsync().ConfigureAwait(false);
bool sucesso = false;
string mensagem;
try
{
BaseFix.fimProcesso = null;
BaseFix.inicioProcesso = DateTime.UtcNow;
BaseFix.inicioFix = DateTime.UtcNow;
BaseFix.CorrecaoEmAndamento = true;
Models.Variaveis.MostrarLog($"Aplicando posição fixa da base com o método {metodo}...");
switch (metodo)
{
case MetodoFixacaoBase.Ntrip:
BaseFix.DefinirTempos(
segsFixEstavel: TempoSurveryIn,
segsJanelaSegs: 600,
metodo: metodo);
sucesso = await BaseFix.FixarBaseViaNtripAsync(
portaUsb: portaUsb,
portaSaida: portaSaida,
portaEntrada: portaEntrada,
baseId: baseId,
startNtrip: () => StartNtripAsync(paraFixacaoBase: true),
stopNtrip: StopNtripAsync,
ct: _lifetimeCts.Token).ConfigureAwait(false);
break;
case MetodoFixacaoBase.SurveyIn:
BaseFix.DefinirTempos(
segsFixEstavel: TempoSurveryIn,
segsJanelaSegs: TempoSurveryIn + 30,
metodo: metodo);
await ConfigurarModuloBase(
tempo_fixacao: TempoSurveryIn,
porta_usb: portaUsb,
porta_saida: portaSaida,
ct: _lifetimeCts.Token).ConfigureAwait(false);
DateTime surveyDeadline = DateTime.UtcNow.AddSeconds(TempoSurveryIn + 30);
while (DateTime.UtcNow < surveyDeadline &&
GetFixQualitySnapshot() != TiposCorrecaoGPS.BaseFix)
{
await Task.Delay(500, _lifetimeCts.Token).ConfigureAwait(false);
}
sucesso = GetFixQualitySnapshot() == TiposCorrecaoGPS.BaseFix;
break;
case MetodoFixacaoBase.Manual:
if (lat.HasValue && lon.HasValue && alt.HasValue)
{
BaseFix.DefinirTempos(10, 10, metodo);
await BaseFix.AplicarBaseFixAsync(
portaUsb,
portaSaida,
baseId,
lat.Value,
lon.Value,
alt.Value,
_lifetimeCts.Token).ConfigureAwait(false);
sucesso = true;
}
break;
}
if (sucesso)
{
DateTime deadline = DateTime.UtcNow.AddSeconds(10);
while (DateTime.UtcNow < deadline &&
GetFixQualitySnapshot() != TiposCorrecaoGPS.BaseFix)
{
await Task.Delay(500, _lifetimeCts.Token).ConfigureAwait(false);
}
sucesso = GetFixQualitySnapshot() == TiposCorrecaoGPS.BaseFix;
}
}
catch (OperationCanceledException)
{
sucesso = false;
}
catch (Exception ex)
{
sucesso = false;
Models.Variaveis.MostrarLog($"Erro ao definir posição da base: {ex.Message}");
}
finally
{
BaseFix.fimProcesso = DateTime.UtcNow;
BaseFix.FixLiberado = false;
BaseFix.CorrecaoEmAndamento = false;
if (sucesso)
{
var snapshot = GetPositionSnapshot();
DefinirPapelEsperado(
GnssExpectedRole.BaseFixed
);
_lastBaseConfiguration = new BaseFixedConfiguration
{
PortaUsb = portaUsb,
PortaSaida = portaSaida,
BaseId = baseId,
Latitude = snapshot.Latitude,
Longitude = snapshot.Longitude,
AltitudeElipsoidal = snapshot.AltitudeElipsoidal,
};
if (!(offset ?? false))
{
BaseFix.LatitudeFix = snapshot.Latitude;
BaseFix.LongitudeFix = snapshot.Longitude;
BaseFix.AltitudeElpsoidalFix = snapshot.AltitudeElipsoidal;
BaseFix.OrientacaoFix = snapshot.Orientacao;
}
}
if (!(offset ?? false) &&
AppShell.Mock &&
lat.HasValue && lon.HasValue && alt.HasValue)
{
lock (_modelLock)
{
UltimaLeitura.Latitude = lat.Value;
UltimaLeitura.Longitude = lon.Value;
UltimaLeitura.AltitudeElipsoidal = alt.Value;
}
AtualizarCoordenadasGPS();
sucesso = true;
}
mensagem = sucesso
? $"Posição da base aplicada com sucesso com o método {metodo}."
: $"Falha ao aplicar posição da base com o método {metodo}.";
Models.Variaveis.MostrarLog(mensagem);
Models.Variaveis.Dock?._vm?.FinalizarFixacaoBase(sucesso, mensagem);
}
}
private async Task ConfigurarModuloBase(
string porta_usb = "com3",
string porta_saida = "com2",
int tempo_fixacao = 60,
CancellationToken ct = default)
{
const string baseId = "957";
const string distanciaMin = "0";
string[] commands =
{
"config com1 115200\r\n",
"config com2 115200\r\n",
"config com3 115200\r\n",
"unlog com1\r\n",
"unlog com2\r\n",
"unlog com3\r\n",
$"mode base {baseId} time {tempo_fixacao} {distanciaMin}\r\n",
$"RTCM1006 {porta_saida} 10\r\n",
$"RTCM1033 {porta_saida} 30\r\n",
$"RTCM1074 {porta_saida} 1\r\n",
$"RTCM1084 {porta_saida} 1\r\n",
$"RTCM1094 {porta_saida} 1\r\n",
$"RTCM1124 {porta_saida} 1\r\n",
$"RTCM1230 {porta_saida} 10\r\n",
$"gngga {porta_usb} 1\r\n",
"saveconfig\r\n",
};
await Task.Delay(1000, ct).ConfigureAwait(false);
await SendCommandsAsync(commands, 300, ct).ConfigureAwait(false);
if (AppShell.Mock)
_expectedRole = GnssExpectedRole.BaseFixed;
}
public async Task ConfigurarComoMovelParaLevantamentoAsync(bool usarNtrip = true)
{
if (!IsConnected)
{
throw new InvalidOperationException(
"O GNSS não está conectado."
);
}
if (
BaseFix?.CorrecaoEmAndamento ==
true
)
{
throw new InvalidOperationException(
"Existe uma fixação da base em andamento."
);
}
Models.Variaveis.MostrarLog(
"[GNSS][PAPEL] " +
"Alterando para modo móvel / levantamento..."
);
/*
* Se existia uma sessão NTRIP anterior,
* encerramos antes de enviar comandos ASCII.
*
* Isso evita RTCM binário concorrendo com
* configuração do UM982.
*/
await StopNtripAsync()
.ConfigureAwait(false);
BaseFix?.ReiniciarFix();
/*
* ConfigurarModulo(false):
*
* - unlog
* - mode rover uav
* - GGA
* - heading
* - papel esperado = RoverTemporary
*/
await ConfigurarModulo(
fixar: false
)
.ConfigureAwait(false);
/*
* Para levantamento queremos precisão RTK.
*
* Portanto o receptor volta a receber
* correções do caster depois de estar em
* modo rover.
*/
if (usarNtrip)
{
await StartNtripAsync()
.ConfigureAwait(false);
}
Models.Variaveis.MostrarLog(
"[GNSS][PAPEL] " +
$"Modo móvel ativo | " +
$"NTRIP={(usarNtrip ? "SOLICITADO" : "NÃO")}"
);
}
// =========================================================
// LEITURA SERIAL E DEMULTIPLEXAÇÃO
// =========================================================
private ParserMode _parserMode = ParserMode.Idle;
private readonly List<byte> _rtcmBuffer = new(1100);
private readonly List<byte> _nmeaBuffer = new(256);
private int _rtcmExpectedBytes = -1;
private double _frameStartedMono;
private void PortaGPS_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
try
{
SerialPort port;
lock (_portLock)
port = PortaGps;
if (port == null || !port.IsOpen)
return;
int available = port.BytesToRead;
if (available <= 0)
return;
byte[] buffer = new byte[available];
int bytesRead = port.Read(buffer, 0, buffer.Length);
if (bytesRead <= 0)
return;
double now = MonotonicNow();
_lastSerialRxMono = now;
Interlocked.Add(ref _serialBytesReceived, bytesRead);
List<string> nmeaLines = new();
List<byte[]> rtcmMessages = new();
lock (_parserLock)
{
for (int i = 0; i < bytesRead; i++)
{
ProcessIncomingByte(buffer[i], nmeaLines, rtcmMessages);
}
}
foreach (string line in nmeaLines)
ProcessNmeaSafely(line);
foreach (byte[] message in rtcmMessages)
EnqueueRtcm(message);
lock (_modelLock)
{
UltimaLeitura.Momento = DateTime.Now;
UltimaLeitura.Inicializado = true;
}
}
catch (Exception ex)
{
Interlocked.Increment(ref _serialReadErrors);
Models.Variaveis.MostrarLog($"Erro ao processar dados da porta serial: {ex.Message}");
}
}
private void ProcessIncomingByte(
byte b,
List<string> nmeaLines,
List<byte[]> rtcmMessages)
{
double now = MonotonicNow();
if (_parserMode == ParserMode.Rtcm &&
(now - _frameStartedMono) > RtcmAssemblyTimeout.TotalSeconds)
{
ResetParserState();
Interlocked.Increment(ref _rtcmResyncs);
}
else if (_parserMode == ParserMode.Nmea &&
(now - _frameStartedMono) > NmeaAssemblyTimeout.TotalSeconds)
{
ResetParserState();
Interlocked.Increment(ref _nmeaInvalid);
}
switch (_parserMode)
{
case ParserMode.Idle:
StartFrameIfApplicable(b, now);
break;
case ParserMode.Nmea:
// Uma sentença quebrada não pode esconder um RTCM novo.
if (b == 0xD3)
{
Interlocked.Increment(ref _nmeaInvalid);
StartRtcm(now);
return;
}
if (b == (byte)'$')
{
// Reinicia na sentença NMEA mais recente.
StartNmea(now);
return;
}
if (b == (byte)'\n')
{
_nmeaBuffer.Add(b);
string line = Encoding.ASCII.GetString(_nmeaBuffer.ToArray()).Trim();
ResetParserState();
if (!string.IsNullOrWhiteSpace(line))
nmeaLines.Add(line);
return;
}
if (b == (byte)'\r' || (b >= 0x20 && b <= 0x7E))
{
_nmeaBuffer.Add(b);
}
else
{
ResetParserState();
Interlocked.Increment(ref _nmeaInvalid);
StartFrameIfApplicable(b, now);
return;
}
if (_nmeaBuffer.Count > 512)
{
ResetParserState();
Interlocked.Increment(ref _nmeaInvalid);
}
break;
case ParserMode.Rtcm:
_rtcmBuffer.Add(b);
if (_rtcmBuffer.Count == 3)
{
// Os seis bits superiores do byte 1 são reservados e devem ser zero.
if ((_rtcmBuffer[1] & 0xFC) != 0)
{
Interlocked.Increment(ref _rtcmInvalidLength);
ResetAndResyncFromCurrentBuffer(now);
return;
}
int payloadLength = ((_rtcmBuffer[1] & 0x03) << 8) | _rtcmBuffer[2];
_rtcmExpectedBytes = 3 + payloadLength + 3;
if (payloadLength <= 0 || _rtcmExpectedBytes > 1029)
{
Interlocked.Increment(ref _rtcmInvalidLength);
ResetAndResyncFromCurrentBuffer(now);
return;
}
}
if (_rtcmExpectedBytes > 0 && _rtcmBuffer.Count == _rtcmExpectedBytes)
{
byte[] message = _rtcmBuffer.ToArray();
ResetParserState();
if (ValidateRtcmCrc24Q(message))
{
Interlocked.Increment(ref _rtcmValid);
_lastValidRtcmMono = now;
rtcmMessages.Add(message);
}
else
{
Interlocked.Increment(ref _rtcmCrcErrors);
Interlocked.Increment(ref _rtcmResyncs);
}
}
else if (_rtcmExpectedBytes > 0 && _rtcmBuffer.Count > _rtcmExpectedBytes)
{
Interlocked.Increment(ref _rtcmResyncs);
ResetAndResyncFromCurrentBuffer(now);
}
break;
}
}
private void StartFrameIfApplicable(byte b, double now)
{
if (b == 0xD3)
StartRtcm(now);
else if (b == (byte)'$')
StartNmea(now);
}
private void StartNmea(double now)
{
ResetParserState();
_parserMode = ParserMode.Nmea;
_frameStartedMono = now;
_nmeaBuffer.Add((byte)'$');
}
private void StartRtcm(double now)
{
ResetParserState();
_parserMode = ParserMode.Rtcm;
_frameStartedMono = now;
_rtcmBuffer.Add(0xD3);
}
private void ResetAndResyncFromCurrentBuffer(double now)
{
byte[] candidate = _rtcmBuffer.Skip(1).ToArray();
ResetParserState();
Interlocked.Increment(ref _rtcmResyncs);
foreach (byte b in candidate)
{
if (_parserMode == ParserMode.Idle)
{
if (b == 0xD3)
{
StartRtcm(now);
}
else if (b == (byte)'$')
{
StartNmea(now);
}
}
else if (_parserMode == ParserMode.Rtcm)
{
_rtcmBuffer.Add(b);
if (_rtcmBuffer.Count == 3)
{
int len = ((_rtcmBuffer[1] & 0x03) << 8) | _rtcmBuffer[2];
_rtcmExpectedBytes = 3 + len + 3;
}
}
else if (_parserMode == ParserMode.Nmea)
{
if (b == (byte)'\n')
{
ResetParserState();
}
else if (b == (byte)'\r' || (b >= 0x20 && b <= 0x7E))
{
_nmeaBuffer.Add(b);
}
}
}
}
private void ResetParsers()
{
lock (_parserLock)
ResetParserState();
}
private void ResetParserState()
{
_parserMode = ParserMode.Idle;
_rtcmBuffer.Clear();
_nmeaBuffer.Clear();
_rtcmExpectedBytes = -1;
_frameStartedMono = 0;
}
// =========================================================
// RTCM: VALIDAÇÃO, FILA E PUBLICAÇÃO
// =========================================================
private void EnqueueRtcm(byte[] message)
{
int type = GetRtcmMessageType(message);
if (type < 0)
{
Interlocked.Increment(ref _rtcmInvalidLength);
return;
}
RtcmEnvelope envelope = new()
{
Type = type,
Data = message,
ReceivedMono = MonotonicNow(),
Sequence = Interlocked.Increment(ref _rtcmSequence),
};
bool shouldSignal;
lock (_rtcmQueueLock)
{
shouldSignal = _rtcmLatestByType.Count == 0;
if (_rtcmLatestByType.ContainsKey(type))
Interlocked.Increment(ref _rtcmReplaced);
_rtcmLatestByType[type] = envelope;
Interlocked.Increment(ref _rtcmQueued);
}
if (shouldSignal)
{
try { _rtcmSignal.Release(); } catch (SemaphoreFullException) { }
}
}
private async Task RtcmPublisherLoopAsync(CancellationToken ct)
{
while (!ct.IsCancellationRequested)
{
try
{
await _rtcmSignal.WaitAsync(ct).ConfigureAwait(false);
}
catch (OperationCanceledException)
{
break;
}
while (!ct.IsCancellationRequested)
{
List<RtcmEnvelope> batch;
lock (_rtcmQueueLock)
{
if (_rtcmLatestByType.Count == 0)
break;
batch = _rtcmLatestByType.Values
.OrderBy(x => x.Sequence)
.ToList();
_rtcmLatestByType.Clear();
}
foreach (RtcmEnvelope item in batch)
{
if (ct.IsCancellationRequested)
break;
double age = MonotonicNow() - item.ReceivedMono;
if (age > RtcmMaxQueueAge.TotalSeconds)
{
Interlocked.Increment(ref _rtcmDroppedStale);
continue;
}
try
{
// A chamada externa fica isolada neste worker. Se MQTT bloquear,
// a serial continua lendo e a fila mantém somente o RTCM mais novo por tipo.
VariaveisControleOperacao.EnviarDadosCorrecaoRTCM(item.Data);
_lastForwardedRtcmMono = MonotonicNow();
Interlocked.Increment(ref _rtcmForwarded);
}
catch (Exception ex)
{
Interlocked.Increment(ref _rtcmPublishErrors);
Models.Variaveis.MostrarLog(
$"Erro ao encaminhar RTCM tipo {item.Type}: {ex.Message}");
}
}
}
}
}
private static int GetRtcmMessageType(byte[] message)
{
if (message == null || message.Length < 8 || message[0] != 0xD3)
return -1;
return (message[3] << 4) | (message[4] >> 4);
}
private static bool ValidateRtcmCrc24Q(byte[] message)
{
if (message == null || message.Length < 6)
return false;
uint calculated = ComputeCrc24Q(message, 0, message.Length - 3);
uint received =
((uint)message[^3] << 16) |
((uint)message[^2] << 8) |
message[^1];
return calculated == received;
}
private static uint ComputeCrc24Q(byte[] data, int offset, int count)
{
uint crc = 0;
for (int i = offset; i < offset + count; i++)
{
crc ^= (uint)data[i] << 16;
for (int bit = 0; bit < 8; bit++)
{
crc <<= 1;
if ((crc & 0x1000000) != 0)
crc ^= 0x1864CFB;
}
}
return crc & 0xFFFFFF;
}
// =========================================================
// NMEA
// =========================================================
private void ProcessNmeaSafely(string sentence)
{
if (string.IsNullOrWhiteSpace(sentence))
return;
try
{
if (!TryValidateNmeaChecksum(sentence, out bool checksumPresent))
{
Interlocked.Increment(ref _nmeaChecksumErrors);
Interlocked.Increment(ref _nmeaInvalid);
return;
}
ProcessarDadosNMEA(sentence);
_lastValidNmeaMono = MonotonicNow();
Interlocked.Increment(ref _nmeaValid);
if (sentence.StartsWith("$GNGGA", StringComparison.Ordinal) ||
sentence.StartsWith("$GPGGA", StringComparison.Ordinal) ||
sentence.StartsWith("$GLGGA", StringComparison.Ordinal))
{
_lastValidGgaMono = MonotonicNow();
Interlocked.Increment(ref _ggaSequence);
}
}
catch (Exception ex)
{
Interlocked.Increment(ref _nmeaInvalid);
Models.Variaveis.MostrarLog($"Sentença NMEA inválida descartada: {ex.Message}");
}
}
private static bool TryValidateNmeaChecksum(string sentence, out bool checksumPresent)
{
checksumPresent = false;
if (string.IsNullOrWhiteSpace(sentence) || sentence[0] != '$')
return false;
int star = sentence.IndexOf('*');
if (star < 0)
{
// Alguns comandos/respostas do fabricante podem não trazer checksum.
return true;
}
checksumPresent = true;
if (star + 2 >= sentence.Length)
return false;
byte checksum = 0;
for (int i = 1; i < star; i++)
checksum ^= (byte)sentence[i];
string expectedText = sentence.Substring(star + 1, 2);
return byte.TryParse(
expectedText,
NumberStyles.HexNumber,
CultureInfo.InvariantCulture,
out byte expected) &&
checksum == expected;
}
private void ProcessarDadosNMEA(string sentenca)
{
DateTime agora = DateTime.Now;
if (sentenca.StartsWith("$GPGGA", StringComparison.Ordinal))
{
ProcessarGPGGA(sentenca);
AtualizarCoordenadasGPS();
}
else if (sentenca.StartsWith("$GNGGA", StringComparison.Ordinal) ||
sentenca.StartsWith("$GLGGA", StringComparison.Ordinal))
{
ProcessarGNGGA(sentenca);
AtualizarCoordenadasGPS();
}
else if (sentenca.StartsWith("$GNRMC", StringComparison.Ordinal))
{
ProcessarGxRMC(sentenca);
}
else if (sentenca.StartsWith("$GNVTG", StringComparison.Ordinal) ||
sentenca.StartsWith("$GPVTG", StringComparison.Ordinal))
{
ProcessarGNVTG(sentenca);
}
else if (sentenca.StartsWith("$GPGSV", StringComparison.Ordinal) ||
sentenca.StartsWith("$GLGSV", StringComparison.Ordinal) ||
sentenca.StartsWith("$GBGSV", StringComparison.Ordinal) ||
sentenca.StartsWith("$GAGSV", StringComparison.Ordinal))
{
ProcessarGSV(sentenca);
}
else if (sentenca.StartsWith("$GNTHS", StringComparison.Ordinal) ||
sentenca.StartsWith("$GPTHS", StringComparison.Ordinal) ||
sentenca.StartsWith("$GATHS", StringComparison.Ordinal))
{
ProcessarGNTHS(sentenca);
}
else if (sentenca.Length > 6 &&
sentenca[3] == 'G' && sentenca[4] == 'L' && sentenca[5] == 'L')
{
ProcessarGNGLL(sentenca);
}
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')
{
ProcessarGxRMC(sentenca);
}
lock (_modelLock)
UltimaLeitura.UltimoComandoRespondido = agora;
}
private void ProcessarGPGGA(string sentenca)
{
string[] parts = sentenca.Split(',');
if (parts.Length < 10)
throw new FormatException("GPGGA incompleta.");
lock (_modelLock)
{
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now;
if (!string.IsNullOrWhiteSpace(parts[2]) && !string.IsNullOrWhiteSpace(parts[3]))
UltimaLeitura.Latitude = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2);
if (!string.IsNullOrWhiteSpace(parts[4]) && !string.IsNullOrWhiteSpace(parts[5]))
UltimaLeitura.Longitude = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3);
UltimaLeitura.NumeroSatelites = ParseInt(parts[7], 0);
UltimaLeitura.PrecisaoHorizontal = ParseDouble(parts[8], 0);
UltimaLeitura.Altitude = ParseDouble(parts[9], 0);
}
}
private void ProcessarGNGGA(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 15)
throw new FormatException("GNGGA incompleta.");
string horaUtc = campos[1];
string latitudeRaw = campos[2];
string hemisferioLat = campos[3];
string longitudeRaw = campos[4];
string hemisferioLon = campos[5];
int fixCode = ParseInt(campos[6], 0);
int satelites = ParseInt(campos[7], 0);
double hdop = ParseDouble(campos[8], 99.9);
double altMsl = ParseDouble(campos[9], 0);
double geoidSep = ParseDouble(campos[11], 0);
string idadeRaw = campos[13];
string baseId = campos[14].Split('*')[0];
double latitude = ParseDmm(latitudeRaw, hemisferioLat, 2);
double longitude = ParseDmm(longitudeRaw, hemisferioLon, 3);
double idadeCorrecao = -1;
if (!string.IsNullOrWhiteSpace(idadeRaw) &&
double.TryParse(
idadeRaw,
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double parsedAge))
{
idadeCorrecao = parsedAge;
}
double altElipsoidal = altMsl + geoidSep;
lock (_modelLock)
{
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now;
if (!double.IsNaN(latitude))
{
UltimaLeitura.LatitudeAnt = UltimaLeitura.Latitude;
UltimaLeitura.Latitude = latitude;
}
if (!double.IsNaN(longitude))
{
UltimaLeitura.LongitudeAnt = UltimaLeitura.Longitude;
UltimaLeitura.Longitude = longitude;
}
UltimaLeitura.Altitude = altMsl;
UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
UltimaLeitura.PrecisaoHorizontal = hdop;
UltimaLeitura.NumeroSatelites = satelites;
UltimaLeitura.QualidadeFix = (TiposCorrecaoGPS)fixCode;
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
UltimaLeitura.BaseID = baseId;
if (TryParseNmeaTime(horaUtc, out TimeSpan tod))
UltimaLeitura.DataHora = DateTime.UtcNow.Date.Add(tod).ToLocalTime();
if (!UltimaLeitura.EnuOriginSet &&
UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.RTKFixo &&
!double.IsNaN(latitude) &&
!double.IsNaN(longitude))
{
UltimaLeitura.Lat0 = latitude;
UltimaLeitura.Lon0 = longitude;
UltimaLeitura.EnuOriginSet = true;
}
if (UltimaLeitura.EnuOriginSet &&
!double.IsNaN(latitude) &&
!double.IsNaN(longitude))
{
(double latCor, double lonCor) = LeverArm.FixLeverArmLatLon_Fast(
latitude,
longitude,
UltimaLeitura.OrientacaoReal,
UltimaLeitura.TimestampOri.frequencia);
UltimaLeitura.Latitude = latCor;
UltimaLeitura.Longitude = lonCor;
}
}
}
private void ProcessarGNVTG(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 8)
throw new FormatException("VTG incompleta.");
double curso = ParseDouble(campos[1], 0);
double velocidadeKmh = ParseDouble(campos[7].Split('*')[0], 0);
lock (_modelLock)
{
UltimaLeitura.Momento = DateTime.Now;
UltimaLeitura.CursoVerdadeiro = curso;
UltimaLeitura.Velocidade = velocidadeKmh;
}
}
private void ProcessarGSV(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 4 || sentenca.Length < 3)
throw new FormatException("GSV incompleta.");
string sistema = sentenca.Substring(1, 2);
int total = ParseInt(campos[1], 0);
int atual = ParseInt(campos[2], 0);
int visiveis = ParseInt(campos[3], 0);
lock (_modelLock)
{
UltimaLeitura.Momento = DateTime.Now;
var leitura = UltimaLeitura.SatelitesEmVista
.FirstOrDefault(x => x.TipoSistema == sistema);
if (leitura == null)
{
leitura = new GPSSatelitesEmVistaModel
{
TipoSistema = sistema,
Sentencas = new List<GPSSatelitesEmVistaSentencaModel>(),
};
UltimaLeitura.SatelitesEmVista.Add(leitura);
}
var item = leitura.Sentencas.FirstOrDefault(x => x.SentencaAtual == atual);
if (item == null)
{
item = new GPSSatelitesEmVistaSentencaModel
{
SentencaAtual = atual,
SentencasTotal = total,
QuantidadeSatelites = visiveis,
Dados = new List<GPSSatelitesEmVistaDadosModel>(),
};
leitura.Sentencas.Add(item);
}
item.SentencasTotal = total;
item.QuantidadeSatelites = visiveis;
item.Dados = new List<GPSSatelitesEmVistaDadosModel>();
for (int i = 4; i + 3 < campos.Length; i += 4)
{
item.Dados.Add(new GPSSatelitesEmVistaDadosModel
{
PRN = campos[i],
Elevacao = ParseDouble(campos[i + 1], 0),
Azimute = ParseDouble(campos[i + 2], 0),
QualidadeSinal = ParseDouble(campos[i + 3].Split('*')[0], 0),
});
}
}
}
private void ProcessarGNTHS(string sentenca)
{
string[] campos = sentenca.TrimStart('$').Split(',');
if (campos.Length < 3)
throw new FormatException("THS incompleta.");
string status = campos[2].Split('*')[0];
bool headingValido = double.TryParse(
campos[1],
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double heading);
lock (_modelLock)
{
UltimaLeitura.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now;
UltimaLeitura.TipoOrientacao = status;
if (headingValido)
{
UltimaLeitura.OrientacaoReal = InverterHeading
? GPSUtils.NormalizarAngulo(heading - 180.0)
: heading;
}
else
{
// Mantém o último heading numérico, mas atualiza o status V/A.
// Assim a saúde consegue distinguir dado inválido de ausência de sentença.
}
}
}
private void ProcessarGNGLL(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 7)
throw new FormatException("GLL incompleta.");
string mode = campos.Length > 7 ? campos[7].Split('*')[0] : string.Empty;
bool valido = campos[6] == "A" && mode != "N";
lock (_modelLock)
{
if (valido)
{
double lat = ParseDmm(campos[1], campos[2], 2);
double lon = ParseDmm(campos[3], campos[4], 3);
if (!double.IsNaN(lat)) UltimaLeitura.Latitude = lat;
if (!double.IsNaN(lon)) UltimaLeitura.Longitude = lon;
}
if (TryParseNmeaTime(campos[5], out TimeSpan tod))
UltimaLeitura.DataHora = DateTime.UtcNow.Date.Add(tod).ToLocalTime();
ApplyModeToFixQuality(mode);
}
AtualizarCoordenadasGPS();
}
private void ProcessarGxGSA(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 17)
throw new FormatException("GSA incompleta.");
int satsUsados = 0;
for (int i = 3; i <= 14 && i < campos.Length; i++)
if (!string.IsNullOrWhiteSpace(campos[i])) satsUsados++;
double pdop = ParseDouble(campos[15], double.NaN);
double hdop = ParseDouble(campos[16], double.NaN);
double vdop = campos.Length > 17
? ParseDouble(campos[17].Split('*')[0], double.NaN)
: double.NaN;
var fixDim = (TiposDimensaoCorrecaoGPS)ParseInt(campos[2], 1);
lock (_modelLock)
{
if (!double.IsNaN(hdop)) UltimaLeitura.PrecisaoHorizontal = hdop;
if (satsUsados > 0) UltimaLeitura.NumeroSatelites = Math.Max(UltimaLeitura.NumeroSatelites, satsUsados);
if (!double.IsNaN(pdop)) UltimaLeitura.PDOP = pdop;
if (!double.IsNaN(vdop)) UltimaLeitura.VDOP = vdop;
UltimaLeitura.FixDimensao = fixDim;
}
}
private void ProcessarGxRMC(string sentenca)
{
string[] campos = sentenca.Split(',');
if (campos.Length < 12)
throw new FormatException("RMC incompleta.");
string mode = string.Empty;
if (campos.Length > 12)
mode = campos[12].Split('*')[0].Trim();
bool valido = campos[2] == "A";
lock (_modelLock)
{
if (valido)
{
double lat = ParseDmm(campos[3], campos[4], 2);
double lon = ParseDmm(campos[5], campos[6], 3);
if (!double.IsNaN(lat)) UltimaLeitura.Latitude = lat;
if (!double.IsNaN(lon)) UltimaLeitura.Longitude = lon;
}
if (double.TryParse(campos[7], NumberStyles.Float, CultureInfo.InvariantCulture, out double speedKnots))
UltimaLeitura.Velocidade = speedKnots * 1.852; // km/h
if (double.TryParse(campos[8], NumberStyles.Float, CultureInfo.InvariantCulture, out double course))
UltimaLeitura.CursoVerdadeiro = course;
if (TryParseRmcDateTime(campos[1], campos[9], out DateTime dt))
UltimaLeitura.DataHora = dt.ToLocalTime();
if (double.TryParse(campos[10], NumberStyles.Float, CultureInfo.InvariantCulture, out double magVar))
{
if (campos[11].StartsWith("W", StringComparison.OrdinalIgnoreCase))
magVar = -magVar;
UltimaLeitura.VariacaoMagnetica = magVar;
}
ApplyModeToFixQuality(mode);
}
AtualizarCoordenadasGPS();
}
private void ApplyModeToFixQuality(string mode)
{
if (string.IsNullOrWhiteSpace(mode))
return;
UltimaLeitura.QualidadeFix = mode switch
{
"R" => TiposCorrecaoGPS.RTKFixo,
"F" => TiposCorrecaoGPS.RTKFlutuante,
"D" => TiposCorrecaoGPS.DGPS,
"E" => TiposCorrecaoGPS.DeadReckoing,
"A" => TiposCorrecaoGPS.Autonomo,
"N" => TiposCorrecaoGPS.SemCorrecao,
_ => UltimaLeitura.QualidadeFix,
};
}
// =========================================================
// NTRIP: FAILOVER DE PROVEDOR + AUTOSELEÇÃO DE MOUNTPOINT
// =========================================================
private CancellationTokenSource _ntripCts;
private Task _ntripTask;
private TcpClient _ntripClient;
private bool _ntripParaFixacaoBase;
private readonly object _ntripSourcetableLock = new();
private readonly Dictionary<string, NtripSourcetableCacheEntry> _ntripSourcetableCache =
new(StringComparer.OrdinalIgnoreCase);
private readonly object _ntripCooldownLock = new();
private readonly Dictionary<string, DateTime> _ntripCooldownAteUtc =
new(StringComparer.OrdinalIgnoreCase);
private Task StartNtripAsync(bool paraFixacaoBase = false)
{
lock (_ntripLock)
{
if (_ntripTask != null && !_ntripTask.IsCompleted)
return Task.CompletedTask;
CorrecaoRTK_Ntrip = true;
_ntripParaFixacaoBase = paraFixacaoBase;
_ntripCts = CancellationTokenSource.CreateLinkedTokenSource(_lifetimeCts.Token);
_ntripTask = Task.Run(
() => AplicarCorrecaoRTK_NtripAsync(_ntripCts.Token),
_ntripCts.Token);
}
return Task.CompletedTask;
}
private async Task StopNtripAsync()
{
Task task;
TcpClient client;
lock (_ntripLock)
{
CorrecaoRTK_Ntrip = false;
_ntripCts?.Cancel();
client = _ntripClient;
task = _ntripTask;
}
try { client?.Close(); } catch { }
if (task != null)
{
try { await task.ConfigureAwait(false); }
catch (OperationCanceledException) { }
catch { }
}
lock (_ntripLock)
{
_ntripClient = null;
_ntripTask = null;
_ntripCts?.Dispose();
_ntripCts = null;
_ntripParaFixacaoBase = false;
Ntrip_Conectado = false;
LimparNtripAtivo();
}
}
private void LimparNtripAtivo()
{
NtripProviderAtivo = string.Empty;
NtripMountpointAtivo = string.Empty;
NtripDistanciaMountpointAtivoKm = null;
}
private List<NtripProviderConfig> ObterSequenciaProvedoresNtrip()
{
NtripProviderConfig ibge = new()
{
Nome = "IBGE",
Host = NtripHost,
Port = NtripPort,
MountpointManual = NtripMountpoint,
Username = NtripUsername,
Password = NtripPassword,
UsarNtripV2 = false,
Comunitario = false,
ExigeUsuario = true,
ExigeSenha = true,
FiltroPais = "BRA",
PermitirFallbackManualSeAutoFalhar = true,
};
NtripProviderConfig rtk2go = new()
{
Nome = "RTK2go",
Host = NtripRtk2goHost,
Port = NtripRtk2goPort,
MountpointManual = NtripRtk2goMountpoint,
Username = NtripRtk2goUsername,
Password = NtripRtk2goPassword,
UsarNtripV2 = false,
Comunitario = true,
ExigeUsuario = true,
ExigeSenha = false,
FiltroPais = "BRA",
// Para RTK2go não usamos SPLI1 silenciosamente se a descoberta falhar.
// O mountpoint manual continua disponível quando AutoSelecionar=false.
PermitirFallbackManualSeAutoFalhar = false,
};
List<NtripProviderConfig> providers = NtripMode switch
{
NtripProviderMode.Rtk2goOnly =>
new List<NtripProviderConfig> { rtk2go },
NtripProviderMode.IbgeThenRtk2go =>
new List<NtripProviderConfig> { ibge, rtk2go },
_ =>
new List<NtripProviderConfig> { ibge },
};
if (_ntripParaFixacaoBase && !NtripRtk2goPermitidoParaFixacaoBase)
{
int removidos = providers.RemoveAll(x => x.Comunitario);
if (removidos > 0)
{
Models.Variaveis.MostrarLog(
"[NTRIP][SEGURANÇA] RTK2go removido da fixação absoluta. " +
"Para liberar, defina NtripRtk2goPermitidoParaFixacaoBase=true.");
}
}
List<NtripProviderConfig> validos = new();
foreach (NtripProviderConfig provider in providers)
{
if (!ConfiguracaoNtripEstruturalValida(provider))
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider?.Nome ?? "?"}] Configuração inválida. " +
"Verifique host e porta.");
continue;
}
if (provider.ExigeUsuario && string.IsNullOrWhiteSpace(provider.Username))
{
string dica = provider.Nome == "RTK2go"
? "Defina AGRO_RTK2GO_EMAIL ou NtripRtk2goUsername."
: "Defina AGRO_NTRIP_USERNAME ou NtripUsername.";
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}] Usuário não configurado. {dica}");
continue;
}
if (provider.ExigeSenha && string.IsNullOrWhiteSpace(provider.Password))
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}] Senha não configurada. " +
"Defina AGRO_NTRIP_PASSWORD ou NtripPassword.");
continue;
}
validos.Add(provider);
}
return validos;
}
private static bool ConfiguracaoNtripEstruturalValida(NtripProviderConfig provider)
{
return provider != null &&
!string.IsNullOrWhiteSpace(provider.Host) &&
provider.Port > 0;
}
private async Task AplicarCorrecaoRTK_NtripAsync(CancellationToken ct)
{
if (!IsConnected || !APIService.HasInternet)
return;
double backoffSeconds = 1.0;
while (!ct.IsCancellationRequested && CorrecaoRTK_Ntrip)
{
if (!APIService.HasInternet)
{
Ntrip_Conectado = false;
LimparNtripAtivo();
await Task.Delay(TimeSpan.FromSeconds(2), ct).ConfigureAwait(false);
continue;
}
List<NtripProviderConfig> providers = ObterSequenciaProvedoresNtrip();
if (providers.Count == 0)
{
Ntrip_Conectado = false;
LimparNtripAtivo();
Models.Variaveis.MostrarLog(
"[NTRIP] Nenhum provedor possui configuração válida.");
await Task.Delay(TimeSpan.FromSeconds(5), ct).ConfigureAwait(false);
continue;
}
bool algumaSessaoFoiEstabelecida = false;
foreach (NtripProviderConfig provider in providers)
{
if (ct.IsCancellationRequested || !CorrecaoRTK_Ntrip)
break;
List<NtripCandidate> candidatos;
try
{
candidatos = await ObterCandidatosNtripAsync(provider, ct)
.ConfigureAwait(false);
}
catch (OperationCanceledException) when (ct.IsCancellationRequested)
{
return;
}
catch (Exception ex)
{
Interlocked.Increment(ref _ntripErrors);
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Falha preparando candidatos: {ex.Message}");
continue;
}
if (candidatos.Count == 0)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}] Nenhum mountpoint elegível.");
continue;
}
for (int i = 0; i < candidatos.Count; i++)
{
if (ct.IsCancellationRequested || !CorrecaoRTK_Ntrip)
break;
NtripCandidate candidato = candidatos[i];
if (TryObterCooldownRestante(
provider.Nome,
candidato.Mountpoint,
out TimeSpan cooldownRestante))
{
Models.Variaveis.MostrarLog(
$"[NTRIP][COOLDOWN] {provider.Nome}/{candidato.Mountpoint} " +
$"ignorado por mais {Math.Ceiling(cooldownRestante.TotalSeconds):F0}s.");
continue;
}
try
{
string distanciaTexto = candidato.DistanciaKm.HasValue
? $" | {candidato.DistanciaKm.Value:F1} km"
: string.Empty;
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}] Tentando candidato {i + 1}/{candidatos.Count}: " +
$"{candidato.Mountpoint}{distanciaTexto}...");
await ExecutarSessaoNtripAsync(provider, candidato, ct)
.ConfigureAwait(false);
algumaSessaoFoiEstabelecida = true;
}
catch (OperationCanceledException) when (ct.IsCancellationRequested)
{
return;
}
catch (Exception ex)
{
Ntrip_Conectado = false;
LimparNtripAtivo();
Interlocked.Increment(ref _ntripErrors);
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}/{candidato.Mountpoint}] Falha: {ex.Message}");
MarcarMountpointEmCooldown(
provider.Nome,
candidato.Mountpoint,
ex);
if (i + 1 < candidatos.Count)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][FALLBACK] Tentando próximo mountpoint de {provider.Nome}: " +
$"{candidatos[i + 1].Mountpoint}.");
}
}
}
if (!Ntrip_Conectado &&
!ct.IsCancellationRequested &&
CorrecaoRTK_Ntrip)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][FALLBACK] Candidatos de {provider.Nome} esgotados.");
}
}
if (ct.IsCancellationRequested || !CorrecaoRTK_Ntrip)
break;
Interlocked.Increment(ref _ntripReconnects);
double jitter = ((uint)Environment.TickCount % 500) / 1000.0;
if (algumaSessaoFoiEstabelecida)
backoffSeconds = 1.0;
await Task.Delay(
TimeSpan.FromSeconds(backoffSeconds + jitter),
ct).ConfigureAwait(false);
backoffSeconds = Math.Min(20.0, backoffSeconds * 1.8);
}
Ntrip_Conectado = false;
LimparNtripAtivo();
}
private async Task<List<NtripCandidate>> ObterCandidatosNtripAsync(
NtripProviderConfig provider,
CancellationToken ct)
{
if (!NtripAutoSelecionarMountpoint)
{
if (string.IsNullOrWhiteSpace(provider.MountpointManual))
return new List<NtripCandidate>();
return new List<NtripCandidate>
{
new()
{
Mountpoint = provider.MountpointManual.Trim(),
DistanciaKm = null,
Formato = string.Empty,
SistemasGnss = string.Empty,
OrigemAuto = false,
}
};
}
NtripApproxPosition pos = await AguardarPosicaoParaSelecaoNtripAsync(ct)
.ConfigureAwait(false);
if (pos == null)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Sem posição GGA recente para ordenar estações.");
return CriarFallbackManualSePermitido(provider);
}
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Posição aproximada: " +
$"lat={pos.Latitude:F6}, lon={pos.Longitude:F6}.");
List<NtripMountpointInfo> streams;
try
{
streams = await ObterSourcetableAsync(provider, ct).ConfigureAwait(false);
}
catch (Exception ex)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Sourcetable indisponível: {ex.Message}");
return CriarFallbackManualSePermitido(provider);
}
double maxKm = _ntripParaFixacaoBase
? Math.Max(1.0, NtripDistanciaMaximaFixacaoBaseKm)
: Math.Max(1.0, NtripDistanciaMaximaKm);
List<NtripMountpointInfo> ranqueados = streams
.Where(x => CoordenadaValida(x.Latitude, x.Longitude))
.Where(x => string.IsNullOrWhiteSpace(provider.FiltroPais) ||
string.Equals(
x.Pais,
provider.FiltroPais,
StringComparison.OrdinalIgnoreCase))
.Where(x => FormatoRtcm3Compativel(x.Formato))
.Where(x => !x.ExigeGgaCliente)
.Select(x =>
{
x.DistanciaKm = HaversineKm(
pos.Latitude,
pos.Longitude,
x.Latitude,
x.Longitude);
return x;
})
.OrderBy(x => x.DistanciaKm)
.ToList();
if (ranqueados.Count == 0)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Nenhuma estação RTCM 3.x " +
"com coordenada válida foi encontrada.");
return CriarFallbackManualSePermitido(provider);
}
NtripMountpointInfo maisProxima = ranqueados[0];
if (maisProxima.DistanciaKm > maxKm)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Estação mais próxima é " +
$"{maisProxima.Mountpoint} a {maisProxima.DistanciaKm:F1} km, " +
$"acima do limite de {maxKm:F1} km.");
// Em seleção automática nunca furamos o limite por um fallback manual silencioso.
return new List<NtripCandidate>();
}
int maxCandidates = Math.Max(1, NtripMaxCandidatosPorProvedor);
List<NtripMountpointInfo> dentroDoLimite = ranqueados
.Where(x => x.DistanciaKm <= maxKm)
.ToList();
List<NtripMountpointInfo> elegiveis = new(maxCandidates);
foreach (NtripMountpointInfo item in dentroDoLimite)
{
if (TryObterCooldownRestante(
provider.Nome,
item.Mountpoint,
out TimeSpan restante))
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO][COOLDOWN] " +
$"{item.Mountpoint} ({item.DistanciaKm:F1} km) ignorado por " +
$"mais {Math.Ceiling(restante.TotalSeconds):F0}s.");
continue;
}
elegiveis.Add(item);
if (elegiveis.Count >= maxCandidates)
break;
}
if (elegiveis.Count == 0)
{
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Todas as estações dentro de " +
$"{maxKm:F1} km estão temporariamente em cooldown.");
return new List<NtripCandidate>();
}
string ranking = string.Join(
" | ",
elegiveis.Select(
(x, i) => $"{i + 1}.{x.Mountpoint} {x.DistanciaKm:F1}km"));
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Candidatos: {ranking}");
return elegiveis
.Select(x => new NtripCandidate
{
Mountpoint = x.Mountpoint,
DistanciaKm = x.DistanciaKm,
Formato = x.Formato,
SistemasGnss = x.SistemasGnss,
OrigemAuto = true,
})
.ToList();
}
private static string CriarChaveCooldownNtrip(
string providerNome,
string mountpoint)
{
return $"{providerNome?.Trim()}::{mountpoint?.Trim()}";
}
private bool TryObterCooldownRestante(
string providerNome,
string mountpoint,
out TimeSpan restante)
{
restante = TimeSpan.Zero;
if (NtripCooldownMountpointSegundos <= 0 ||
string.IsNullOrWhiteSpace(providerNome) ||
string.IsNullOrWhiteSpace(mountpoint))
{
return false;
}
string chave = CriarChaveCooldownNtrip(
providerNome,
mountpoint);
lock (_ntripCooldownLock)
{
if (!_ntripCooldownAteUtc.TryGetValue(
chave,
out DateTime ateUtc))
{
return false;
}
DateTime agora = DateTime.UtcNow;
if (ateUtc <= agora)
{
_ntripCooldownAteUtc.Remove(chave);
return false;
}
restante = ateUtc - agora;
return true;
}
}
private void MarcarMountpointEmCooldown(
string providerNome,
string mountpoint,
Exception motivo = null)
{
int segundos = Math.Max(
0,
NtripCooldownMountpointSegundos);
if (segundos <= 0 ||
string.IsNullOrWhiteSpace(providerNome) ||
string.IsNullOrWhiteSpace(mountpoint))
{
return;
}
DateTime ateUtc = DateTime.UtcNow.AddSeconds(segundos);
string chave = CriarChaveCooldownNtrip(
providerNome,
mountpoint);
lock (_ntripCooldownLock)
{
_ntripCooldownAteUtc[chave] = ateUtc;
}
string motivoTexto = motivo == null
? string.Empty
: $" | motivo={motivo.GetType().Name}";
Models.Variaveis.MostrarLog(
$"[NTRIP][COOLDOWN] {providerNome}/{mountpoint} suspenso por " +
$"{segundos}s{motivoTexto}.");
}
private List<NtripCandidate> CriarFallbackManualSePermitido(
NtripProviderConfig provider)
{
if (!provider.PermitirFallbackManualSeAutoFalhar ||
string.IsNullOrWhiteSpace(provider.MountpointManual))
{
return new List<NtripCandidate>();
}
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Usando mountpoint manual " +
$"{provider.MountpointManual} apenas como fallback de descoberta.");
return new List<NtripCandidate>
{
new()
{
Mountpoint = provider.MountpointManual.Trim(),
DistanciaKm = null,
Formato = string.Empty,
SistemasGnss = string.Empty,
OrigemAuto = false,
}
};
}
private async Task<NtripApproxPosition> AguardarPosicaoParaSelecaoNtripAsync(
CancellationToken ct)
{
// Se já existe um GGA suficientemente recente, não introduz atraso.
NtripApproxPosition existente = TryGetApproxPositionForNtrip();
if (existente != null)
return existente;
int esperaSegundos = Math.Max(
3,
NtripEsperaPosicaoAutoSegundos);
DateTime inicioUtc = DateTime.UtcNow;
DateTime deadlineUtc = inicioUtc.AddSeconds(esperaSegundos);
DateTime proximoReforcoUtc = DateTime.MinValue;
Models.Variaveis.MostrarLog(
$"[NTRIP][AUTO] Aguardando posição GGA válida por até " +
$"{esperaSegundos}s antes de selecionar mountpoints.");
while (!ct.IsCancellationRequested &&
DateTime.UtcNow <= deadlineUtc)
{
NtripApproxPosition pos = TryGetApproxPositionForNtrip();
if (pos != null)
{
double decorrido = Math.Max(
0,
(DateTime.UtcNow - inicioUtc).TotalSeconds);
Models.Variaveis.MostrarLog(
$"[NTRIP][AUTO] Posição GGA disponível após " +
$"{decorrido:F1}s.");
return pos;
}
/*
* O UM982 acabou de ser colocado em modo rover temporário
* antes desta etapa. Em alguns boots/reconfigurações o primeiro
* GGA pode demorar ou o comando anterior pode não ter produzido
* saída imediatamente.
*
* Como ainda NÃO existe sessão NTRIP injetando RTCM neste ponto,
* é seguro reforçar periodicamente apenas o comando de saída GGA.
*/
if (DateTime.UtcNow >= proximoReforcoUtc)
{
string porta = string.IsNullOrWhiteSpace(
NtripPortaGgaAutoSelecao)
? "com2"
: NtripPortaGgaAutoSelecao.Trim();
try
{
await WriteSerialCommandAsync(
$"gngga {porta} 1\r\n",
ct).ConfigureAwait(false);
}
catch (OperationCanceledException) when (ct.IsCancellationRequested)
{
throw;
}
catch (Exception ex)
{
// Não aborta a descoberta por uma falha pontual de escrita.
// A reconexão serial / fallback existente continua responsável
// pela recuperação do transporte.
Models.Variaveis.MostrarLog(
$"[NTRIP][AUTO] Não foi possível reforçar GGA em " +
$"{porta}: {ex.Message}");
}
// Evita martelar o receptor enquanto esperamos a primeira leitura.
proximoReforcoUtc = DateTime.UtcNow.AddSeconds(3);
}
await Task.Delay(200, ct).ConfigureAwait(false);
}
Models.Variaveis.MostrarLog(
$"[NTRIP][AUTO] Nenhuma posição GGA válida recebida em " +
$"{esperaSegundos}s. A descoberta automática usará o fallback seguro.");
return null;
}
private NtripApproxPosition TryGetApproxPositionForNtrip()
{
double now = MonotonicNow();
double ggaAgeMs = AgeMs(now, _lastValidGgaMono);
/*
* Para escolher uma estação, não precisamos de precisão RTK.
* Uma posição GGA autônoma com até 30 s é mais que suficiente
* para distinguir uma base a 10 km de outra a 200 km.
*/
if (ggaAgeMs < 0 || ggaAgeMs > 30_000)
return null;
lock (_modelLock)
{
double lat = UltimaLeitura.Latitude;
double lon = UltimaLeitura.Longitude;
if (!CoordenadaValida(lat, lon))
return null;
return new NtripApproxPosition
{
Latitude = lat,
Longitude = lon,
};
}
}
private async Task<List<NtripMountpointInfo>> ObterSourcetableAsync(
NtripProviderConfig provider,
CancellationToken ct)
{
string cacheKey = $"{provider.Host}:{provider.Port}";
int cacheSeconds = Math.Max(30, NtripSourcetableCacheSegundos);
lock (_ntripSourcetableLock)
{
if (_ntripSourcetableCache.TryGetValue(
cacheKey,
out NtripSourcetableCacheEntry cached) &&
(DateTime.UtcNow - cached.ObtidaUtc).TotalSeconds <= cacheSeconds)
{
return cached.Streams
.Select(CloneMountpointInfo)
.ToList();
}
}
List<NtripMountpointInfo> streams = await BaixarSourcetableAsync(provider, ct)
.ConfigureAwait(false);
lock (_ntripSourcetableLock)
{
_ntripSourcetableCache[cacheKey] = new NtripSourcetableCacheEntry
{
ObtidaUtc = DateTime.UtcNow,
Streams = streams
.Select(CloneMountpointInfo)
.ToList(),
};
}
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}][AUTO] Sourcetable carregada: " +
$"{streams.Count} streams.");
return streams;
}
private static NtripMountpointInfo CloneMountpointInfo(NtripMountpointInfo x)
{
return new NtripMountpointInfo
{
Mountpoint = x.Mountpoint,
Identificador = x.Identificador,
Formato = x.Formato,
SistemasGnss = x.SistemasGnss,
Rede = x.Rede,
Pais = x.Pais,
Latitude = x.Latitude,
Longitude = x.Longitude,
ExigeGgaCliente = x.ExigeGgaCliente,
Autenticacao = x.Autenticacao,
TaxaBits = x.TaxaBits,
DistanciaKm = x.DistanciaKm,
};
}
private async Task<List<NtripMountpointInfo>> BaixarSourcetableAsync(
NtripProviderConfig provider,
CancellationToken ct)
{
using TcpClient client = new();
await ConnectTcpWithTimeoutAsync(
client,
provider.Host,
provider.Port,
TimeSpan.FromSeconds(8),
ct).ConfigureAwait(false);
using NetworkStream stream = client.GetStream();
string credentials = string.IsNullOrWhiteSpace(provider.Username)
? string.Empty
: Convert.ToBase64String(
Encoding.UTF8.GetBytes(
$"{provider.Username}:{provider.Password ?? string.Empty}"));
string request =
"GET / HTTP/1.1\r\n" +
$"Host: {provider.Host}:{provider.Port}\r\n" +
"Ntrip-Version: Ntrip/2.0\r\n" +
"User-Agent: NTRIP AgroBase/2.1\r\n" +
"Accept: */*\r\n" +
(!string.IsNullOrEmpty(credentials)
? $"Authorization: Basic {credentials}\r\n"
: string.Empty) +
"Connection: close\r\n" +
"\r\n";
byte[] requestBytes = Encoding.ASCII.GetBytes(request);
await stream.WriteAsync(
requestBytes,
0,
requestBytes.Length,
ct).ConfigureAwait(false);
await stream.FlushAsync(ct).ConfigureAwait(false);
(string header, byte[] firstBodyBytes) =
await ReadHttpHeaderAsync(stream, 32 * 1024, ct).ConfigureAwait(false);
string firstLine = header
.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None)
.FirstOrDefault() ?? string.Empty;
bool accepted =
firstLine.Contains("200 OK", StringComparison.OrdinalIgnoreCase) ||
firstLine.StartsWith("SOURCETABLE 200", StringComparison.OrdinalIgnoreCase);
if (!accepted)
throw new IOException(
$"sourcetable recusada: {firstLine}");
const int maxBodyBytes = 2 * 1024 * 1024;
List<byte> body = new(firstBodyBytes.Length + 64 * 1024);
body.AddRange(firstBodyBytes);
byte[] temp = new byte[8192];
while (body.Count < maxBodyBytes)
{
int read = await ReadNetworkWithTimeoutAsync(
stream,
temp,
TimeSpan.FromSeconds(2),
ct).ConfigureAwait(false);
if (read <= 0)
break;
int canTake = Math.Min(read, maxBodyBytes - body.Count);
for (int i = 0; i < canTake; i++)
body.Add(temp[i]);
string tail = Encoding.ASCII.GetString(
body.ToArray(),
Math.Max(0, body.Count - Math.Min(body.Count, 256)),
Math.Min(body.Count, 256));
if (tail.Contains("ENDSOURCETABLE", StringComparison.OrdinalIgnoreCase))
break;
}
byte[] payload = body.ToArray();
if (HeaderTemTransferEncodingChunked(header))
payload = DecodeChunkedHttpBody(payload);
string sourcetable = Encoding.UTF8.GetString(payload);
List<NtripMountpointInfo> parsed = ParseNtripSourcetable(sourcetable);
if (parsed.Count == 0)
throw new InvalidDataException(
"sourcetable não contém registros STR utilizáveis.");
return parsed;
}
private static List<NtripMountpointInfo> ParseNtripSourcetable(string text)
{
List<NtripMountpointInfo> result = new();
if (string.IsNullOrWhiteSpace(text))
return result;
string[] lines = text.Replace("\r\n", "\n").Split('\n');
foreach (string raw in lines)
{
string line = raw.Trim();
if (!line.StartsWith("STR;", StringComparison.OrdinalIgnoreCase))
continue;
string[] p = line.Split(';');
// STR;mount;identifier;format;details;carrier;nav;network;country;
// lat;lon;nmea;solution;generator;compression;auth;fee;bitrate;misc
if (p.Length < 18)
continue;
if (!double.TryParse(
p[9],
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double lat) ||
!double.TryParse(
p[10],
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double lon))
{
continue;
}
bool exigeGga =
p.Length > 11 &&
(p[11].Trim() == "1" ||
p[11].Trim().Equals("Y", StringComparison.OrdinalIgnoreCase));
int bitrate = 0;
if (p.Length > 17)
{
int.TryParse(
p[17].Trim(),
NumberStyles.Integer,
CultureInfo.InvariantCulture,
out bitrate);
}
result.Add(new NtripMountpointInfo
{
Mountpoint = p[1].Trim(),
Identificador = p[2].Trim(),
Formato = p[3].Trim(),
SistemasGnss = p.Length > 6 ? p[6].Trim() : string.Empty,
Rede = p.Length > 7 ? p[7].Trim() : string.Empty,
Pais = p.Length > 8 ? p[8].Trim() : string.Empty,
Latitude = lat,
Longitude = lon,
ExigeGgaCliente = exigeGga,
Autenticacao = p.Length > 15 ? p[15].Trim() : string.Empty,
TaxaBits = bitrate,
});
}
return result;
}
private static bool FormatoRtcm3Compativel(string format)
{
if (string.IsNullOrWhiteSpace(format))
return false;
return format.IndexOf(
"RTCM 3",
StringComparison.OrdinalIgnoreCase) >= 0 ||
format.IndexOf(
"RTCM3",
StringComparison.OrdinalIgnoreCase) >= 0;
}
private static bool CoordenadaValida(double lat, double lon)
{
return !double.IsNaN(lat) &&
!double.IsInfinity(lat) &&
!double.IsNaN(lon) &&
!double.IsInfinity(lon) &&
lat >= -90.0 &&
lat <= 90.0 &&
lon >= -180.0 &&
lon <= 180.0 &&
(Math.Abs(lat) > 1e-9 || Math.Abs(lon) > 1e-9);
}
private static double HaversineKm(
double lat1,
double lon1,
double lat2,
double lon2)
{
const double earthRadiusKm = 6371.0088;
double p1 = lat1 * Math.PI / 180.0;
double p2 = lat2 * Math.PI / 180.0;
double dLat = (lat2 - lat1) * Math.PI / 180.0;
double dLon = (lon2 - lon1) * Math.PI / 180.0;
double a =
Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) +
Math.Cos(p1) *
Math.Cos(p2) *
Math.Sin(dLon / 2.0) *
Math.Sin(dLon / 2.0);
return 2.0 * earthRadiusKm * Math.Asin(Math.Sqrt(a));
}
private async Task ExecutarSessaoNtripAsync(
NtripProviderConfig provider,
NtripCandidate candidato,
CancellationToken ct)
{
if (provider == null)
throw new ArgumentNullException(nameof(provider));
if (candidato == null ||
string.IsNullOrWhiteSpace(candidato.Mountpoint))
{
throw new ArgumentException(
"Candidato NTRIP inválido.",
nameof(candidato));
}
if (provider.ExigeUsuario && string.IsNullOrWhiteSpace(provider.Username))
throw new InvalidOperationException(
$"{provider.Nome}: usuário NTRIP não configurado.");
if (provider.ExigeSenha && string.IsNullOrWhiteSpace(provider.Password))
throw new InvalidOperationException(
$"{provider.Nome}: senha NTRIP não configurada.");
TcpClient client = null;
try
{
client = new TcpClient();
lock (_ntripLock)
_ntripClient = client;
await ConnectTcpWithTimeoutAsync(
client,
provider.Host,
provider.Port,
TimeSpan.FromSeconds(8),
ct).ConfigureAwait(false);
using NetworkStream stream = client.GetStream();
string credentials = string.IsNullOrWhiteSpace(provider.Username)
? string.Empty
: Convert.ToBase64String(
Encoding.UTF8.GetBytes(
$"{provider.Username}:{provider.Password ?? string.Empty}"));
string request;
if (provider.UsarNtripV2)
{
request =
$"GET /{candidato.Mountpoint} HTTP/1.1\r\n" +
$"Host: {provider.Host}:{provider.Port}\r\n" +
"Ntrip-Version: Ntrip/2.0\r\n" +
"User-Agent: NTRIP AgroBase/2.1\r\n" +
"Accept: */*\r\n" +
(!string.IsNullOrEmpty(credentials)
? $"Authorization: Basic {credentials}\r\n"
: string.Empty) +
"Connection: close\r\n" +
"\r\n";
}
else
{
request =
$"GET /{candidato.Mountpoint} HTTP/1.0\r\n" +
"User-Agent: NTRIP AgroBase/1.0\r\n" +
"Accept: */*\r\n" +
(!string.IsNullOrEmpty(credentials)
? $"Authorization: Basic {credentials}\r\n"
: string.Empty) +
"Connection: close\r\n" +
"\r\n";
}
byte[] requestBytes = Encoding.ASCII.GetBytes(request);
await stream.WriteAsync(
requestBytes,
0,
requestBytes.Length,
ct).ConfigureAwait(false);
await stream.FlushAsync(ct).ConfigureAwait(false);
(string header, byte[] firstBodyBytes) =
await ReadHttpHeaderAsync(
stream,
16 * 1024,
ct).ConfigureAwait(false);
string firstLine = header
.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None)
.FirstOrDefault() ?? string.Empty;
bool accepted =
firstLine.Contains("200 OK", StringComparison.OrdinalIgnoreCase) ||
firstLine.StartsWith("ICY 200", StringComparison.OrdinalIgnoreCase);
if (!accepted)
throw new IOException(
$"caster recusou conexão: {firstLine}");
if (HeaderTemTransferEncodingChunked(header))
{
throw new NotSupportedException(
"caster respondeu com Transfer-Encoding: chunked. " +
"O dechunking do stream binário NTRIP ainda não está habilitado.");
}
Ntrip_Conectado = true;
NtripProviderAtivo = provider.Nome;
NtripMountpointAtivo = candidato.Mountpoint;
NtripDistanciaMountpointAtivoKm = candidato.DistanciaKm;
string distanciaTexto = candidato.DistanciaKm.HasValue
? $" | distância={candidato.DistanciaKm.Value:F1} km"
: string.Empty;
Models.Variaveis.MostrarLog(
$"[NTRIP][{provider.Nome}] Conexão estabelecida em " +
$"{provider.Host}:{provider.Port}/{candidato.Mountpoint}" +
$"{distanciaTexto}.");
if (provider.Comunitario)
{
Models.Variaveis.MostrarLog(
"[NTRIP][RTK2go][AVISO] Fonte comunitária ativa. " +
"A seleção por distância não valida datum, coordenada publicada " +
"nem qualidade geodésica da estação.");
}
bool recebeuDados = false;
if (firstBodyBytes.Length > 0)
{
await WriteSerialAsync(firstBodyBytes, ct).ConfigureAwait(false);
Interlocked.Add(ref _ntripBytesReceived, firstBodyBytes.Length);
recebeuDados = true;
}
byte[] buffer = new byte[4096];
while (!ct.IsCancellationRequested && CorrecaoRTK_Ntrip)
{
int read;
if (!recebeuDados)
{
read = await ReadNetworkWithTimeoutAsync(
stream,
buffer,
TimeSpan.FromSeconds(
Math.Max(2, NtripPrimeirosDadosTimeoutSegundos)),
ct).ConfigureAwait(false);
if (read <= 0)
{
throw new TimeoutException(
"sessão foi aceita, mas nenhum byte RTCM chegou no prazo.");
}
}
else
{
read = await stream.ReadAsync(
buffer,
0,
buffer.Length,
ct).ConfigureAwait(false);
}
if (read <= 0)
throw new IOException(
"caster encerrou a conexão.");
byte[] serialChunk = new byte[read];
Buffer.BlockCopy(buffer, 0, serialChunk, 0, read);
await WriteSerialAsync(serialChunk, ct).ConfigureAwait(false);
Interlocked.Add(ref _ntripBytesReceived, read);
recebeuDados = true;
}
}
finally
{
Ntrip_Conectado = false;
LimparNtripAtivo();
try { client?.Close(); } catch { }
lock (_ntripLock)
{
if (ReferenceEquals(_ntripClient, client))
_ntripClient = null;
}
}
}
private static async Task<int> ReadNetworkWithTimeoutAsync(
NetworkStream stream,
byte[] buffer,
TimeSpan timeout,
CancellationToken ct)
{
using CancellationTokenSource linked =
CancellationTokenSource.CreateLinkedTokenSource(ct);
linked.CancelAfter(timeout);
try
{
return await stream.ReadAsync(
buffer.AsMemory(0, buffer.Length),
linked.Token).ConfigureAwait(false);
}
catch (OperationCanceledException) when (!ct.IsCancellationRequested)
{
return 0;
}
}
private static bool HeaderTemTransferEncodingChunked(string header)
{
if (string.IsNullOrWhiteSpace(header))
return false;
string[] lines = header.Split(
new[] { "\r\n", "\n" },
StringSplitOptions.RemoveEmptyEntries);
foreach (string line in lines)
{
int idx = line.IndexOf(':');
if (idx <= 0)
continue;
string key = line.Substring(0, idx).Trim();
string value = line.Substring(idx + 1).Trim();
if (key.Equals(
"Transfer-Encoding",
StringComparison.OrdinalIgnoreCase) &&
value.IndexOf(
"chunked",
StringComparison.OrdinalIgnoreCase) >= 0)
{
return true;
}
}
return false;
}
private static byte[] DecodeChunkedHttpBody(byte[] raw)
{
if (raw == null || raw.Length == 0)
return Array.Empty<byte>();
List<byte> output = new();
int pos = 0;
while (pos < raw.Length)
{
int lineEnd = IndexOfCrlf(raw, pos);
if (lineEnd < 0)
throw new InvalidDataException(
"Chunked HTTP inválido: tamanho do chunk incompleto.");
string sizeLine = Encoding.ASCII
.GetString(raw, pos, lineEnd - pos)
.Trim();
int semicolon = sizeLine.IndexOf(';');
if (semicolon >= 0)
sizeLine = sizeLine.Substring(0, semicolon);
if (!int.TryParse(
sizeLine,
NumberStyles.HexNumber,
CultureInfo.InvariantCulture,
out int size))
{
throw new InvalidDataException(
"Chunked HTTP inválido: tamanho de chunk ilegível.");
}
pos = lineEnd + 2;
if (size == 0)
break;
if (size < 0 || pos + size > raw.Length)
throw new InvalidDataException(
"Chunked HTTP inválido: payload de chunk incompleto.");
for (int i = 0; i < size; i++)
output.Add(raw[pos + i]);
pos += size;
if (pos + 1 >= raw.Length ||
raw[pos] != '\r' ||
raw[pos + 1] != '\n')
{
throw new InvalidDataException(
"Chunked HTTP inválido: CRLF ausente após payload.");
}
pos += 2;
}
return output.ToArray();
}
private static int IndexOfCrlf(byte[] data, int start)
{
for (int i = Math.Max(0, start); i + 1 < data.Length; i++)
{
if (data[i] == '\r' && data[i + 1] == '\n')
return i;
}
return -1;
}
private static async Task ConnectTcpWithTimeoutAsync(
TcpClient client,
string host,
int port,
TimeSpan timeout,
CancellationToken ct)
{
Task connectTask = client.ConnectAsync(host, port);
Task timeoutTask = Task.Delay(timeout, ct);
Task completed = await Task.WhenAny(connectTask, timeoutTask).ConfigureAwait(false);
if (completed != connectTask)
{
ct.ThrowIfCancellationRequested();
throw new TimeoutException($"Timeout conectando em {host}:{port}.");
}
await connectTask.ConfigureAwait(false);
}
private static async Task<(string Header, byte[] FirstBodyBytes)> ReadHttpHeaderAsync(
NetworkStream stream,
int maxHeaderBytes,
CancellationToken ct)
{
List<byte> data = new(1024);
byte[] temp = new byte[512];
int headerEnd = -1;
int delimiterLength = 0;
while (headerEnd < 0)
{
int read = await stream.ReadAsync(
temp,
0,
temp.Length,
ct).ConfigureAwait(false);
if (read <= 0)
throw new IOException(
"Conexão encerrada antes do cabeçalho NTRIP.");
for (int i = 0; i < read; i++)
data.Add(temp[i]);
if (data.Count > maxHeaderBytes)
throw new InvalidDataException(
"Cabeçalho NTRIP excedeu o limite.");
(headerEnd, delimiterLength) = FindNtripHeaderEnd(data);
}
byte[] all = data.ToArray();
string header = Encoding.ASCII.GetString(all, 0, headerEnd);
int bodyStart = headerEnd + delimiterLength;
byte[] body = bodyStart < all.Length
? all.Skip(bodyStart).ToArray()
: Array.Empty<byte>();
return (header, body);
}
private static (int HeaderEnd, int DelimiterLength) FindNtripHeaderEnd(
List<byte> data)
{
for (int i = 0; i <= data.Count - 4; i++)
{
if (data[i] == '\r' &&
data[i + 1] == '\n' &&
data[i + 2] == '\r' &&
data[i + 3] == '\n')
{
return (i, 4);
}
}
if (data.Count >= 3 &&
data[0] == (byte)'I' &&
data[1] == (byte)'C' &&
data[2] == (byte)'Y')
{
for (int i = 0; i <= data.Count - 2; i++)
{
if (data[i] == '\r' &&
data[i + 1] == '\n')
{
return (i, 2);
}
}
}
return (-1, 0);
}
// =========================================================
// SNAPSHOTS E UI
// =========================================================
internal long GetGgaSequence() => Interlocked.Read(ref _ggaSequence);
internal bool TryGetGgaSnapshot(long afterSequence, out long sequence, out GgaFix snapshot)
{
sequence = Interlocked.Read(ref _ggaSequence);
snapshot = default;
if (sequence <= afterSequence)
return false;
lock (_modelLock)
{
snapshot = new GgaFix(
tsUtc: UltimaLeitura.DataHora.ToUniversalTime(),
latDeg: UltimaLeitura.Latitude,
lonDeg: UltimaLeitura.Longitude,
altElipsoidalM: UltimaLeitura.AltitudeElipsoidal,
headingDeg: UltimaLeitura.OrientacaoReal,
fixQuality: UltimaLeitura.QualidadeFix);
}
return true;
}
private TiposCorrecaoGPS GetFixQualitySnapshot()
{
lock (_modelLock)
return UltimaLeitura.QualidadeFix;
}
private PositionSnapshot GetPositionSnapshot()
{
lock (_modelLock)
{
return new PositionSnapshot
{
Latitude = UltimaLeitura.Latitude,
Longitude = UltimaLeitura.Longitude,
AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal,
Orientacao = UltimaLeitura.OrientacaoReal,
};
}
}
private void AtualizarCoordenadasGPS()
{
lock (_modelLock)
{
UltimaLeitura.AnguloCarroDefinido = UltimaLeitura.OrientacaoReal;
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
}
if (Interlocked.CompareExchange(ref _uiUpdatePending, 1, 0) != 0)
return;
Application.Current?.Dispatcher?.BeginInvoke(new Action(() =>
{
try
{
Models.Variaveis.Dock?._vm?.AtualizarDadosGnss(UltimaLeitura);
}
catch (Exception ex)
{
Models.Variaveis.MostrarLog($"Erro ao atualizar UI GNSS: {ex.Message}");
}
finally
{
Interlocked.Exchange(ref _uiUpdatePending, 0);
}
}));
}
// =========================================================
// HELPERS
// =========================================================
private static double MonotonicNow() =>
Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
private static double AgeMs(double now, double timestamp)
{
if (timestamp <= 0)
return -1;
return Math.Max(0, (now - timestamp) * 1000.0);
}
private static int ParseInt(string text, int fallback)
{
return int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out int value)
? value
: fallback;
}
private static double ParseDouble(string text, double fallback)
{
return double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out double value)
? value
: fallback;
}
private static double ParseDmm(string raw, string hemisphere, int degreeDigits)
{
if (string.IsNullOrWhiteSpace(raw) || raw.Length <= degreeDigits)
return double.NaN;
if (!double.TryParse(
raw.Substring(0, degreeDigits),
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double degrees) ||
!double.TryParse(
raw.Substring(degreeDigits),
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double minutes))
{
return double.NaN;
}
double value = degrees + minutes / 60.0;
if (hemisphere.Equals("S", StringComparison.OrdinalIgnoreCase) ||
hemisphere.Equals("W", StringComparison.OrdinalIgnoreCase))
{
value = -value;
}
return value;
}
private static bool TryParseNmeaTime(string text, out TimeSpan value)
{
value = default;
if (string.IsNullOrWhiteSpace(text) || text.Length < 6)
return false;
if (!int.TryParse(text.Substring(0, 2), out int hh) ||
!int.TryParse(text.Substring(2, 2), out int mm) ||
!double.TryParse(
text.Substring(4),
NumberStyles.Float,
CultureInfo.InvariantCulture,
out double seconds))
{
return false;
}
int ss = (int)Math.Floor(seconds);
int ms = (int)Math.Round((seconds - ss) * 1000.0);
if (ms >= 1000)
{
ss++;
ms = 0;
}
try
{
value = new TimeSpan(0, hh, mm, ss, ms);
return true;
}
catch
{
return false;
}
}
private static bool TryParseRmcDateTime(
string timeText,
string dateText,
out DateTime utc)
{
utc = default;
if (!TryParseNmeaTime(timeText, out TimeSpan tod) ||
string.IsNullOrWhiteSpace(dateText) ||
dateText.Length != 6)
{
return false;
}
if (!int.TryParse(dateText.Substring(0, 2), out int day) ||
!int.TryParse(dateText.Substring(2, 2), out int month) ||
!int.TryParse(dateText.Substring(4, 2), out int yy))
{
return false;
}
try
{
utc = new DateTime(2000 + yy, month, day, 0, 0, 0, DateTimeKind.Utc).Add(tod);
return true;
}
catch
{
return false;
}
}
public void Dispose()
{
if (Interlocked.Exchange(ref _disposed, 1) != 0)
return;
_tmrCheck.Stop();
_tmrCheck.Dispose();
try { StopNtripAsync().GetAwaiter().GetResult(); } catch { }
_lifetimeCts.Cancel();
try { _rtcmSignal.Release(); } catch { }
try { _rtcmPublisherTask.Wait(TimeSpan.FromSeconds(2)); } catch { }
DisconnectInternal();
_scanGate.Dispose();
_serialWriteGate.Dispose();
_rtcmSignal.Dispose();
_lifetimeCts.Dispose();
}
private enum ParserMode
{
Idle,
Nmea,
Rtcm,
}
private enum GnssExpectedRole
{
PreserveCurrentConfiguration,
RoverTemporary,
BaseFixed,
}
public enum NtripProviderMode
{
IbgeOnly = 0,
Rtk2goOnly = 1,
IbgeThenRtk2go = 2,
}
public enum GnssPapelEstacao
{
Indefinido = 0,
Movel = 1,
BaseFixa = 2
}
public GnssPapelEstacao PapelEstacaoAtual
{
get
{
return _expectedRole switch
{
GnssExpectedRole.RoverTemporary =>
GnssPapelEstacao.Movel,
GnssExpectedRole.BaseFixed =>
GnssPapelEstacao.BaseFixa,
/*
* Quando acabamos de abrir o programa,
* ainda podemos estar em Preserve.
*
* Se o próprio UM982 estiver reportando
* BASE FIX, conseguimos reconhecer que
* ele está funcionando como base.
*/
_ when
GetFixQualitySnapshot() ==
TiposCorrecaoGPS.BaseFix =>
GnssPapelEstacao.BaseFixa,
_ =>
GnssPapelEstacao.Indefinido
};
}
}
private void DefinirPapelEsperado(GnssExpectedRole papel)
{
_expectedRole = papel;
}
private sealed class NtripProviderConfig
{
public string Nome { get; init; }
public string Host { get; init; }
public int Port { get; init; }
public string MountpointManual { get; init; }
public string Username { get; init; }
public string Password { get; init; }
public bool UsarNtripV2 { get; init; }
public bool Comunitario { get; init; }
public bool ExigeUsuario { get; init; }
public bool ExigeSenha { get; init; }
public string FiltroPais { get; init; }
public bool PermitirFallbackManualSeAutoFalhar { get; init; }
}
private sealed class NtripCandidate
{
public string Mountpoint { get; init; }
public double? DistanciaKm { get; init; }
public string Formato { get; init; }
public string SistemasGnss { get; init; }
public bool OrigemAuto { get; init; }
}
private sealed class NtripMountpointInfo
{
public string Mountpoint { get; init; }
public string Identificador { get; init; }
public string Formato { get; init; }
public string SistemasGnss { get; init; }
public string Rede { get; init; }
public string Pais { get; init; }
public double Latitude { get; init; }
public double Longitude { get; init; }
public bool ExigeGgaCliente { get; init; }
public string Autenticacao { get; init; }
public int TaxaBits { get; init; }
public double DistanciaKm { get; set; }
}
private sealed class NtripSourcetableCacheEntry
{
public DateTime ObtidaUtc { get; init; }
public List<NtripMountpointInfo> Streams { get; init; }
}
private sealed class NtripApproxPosition
{
public double Latitude { get; init; }
public double Longitude { get; init; }
}
private sealed class RtcmEnvelope
{
public int Type { get; init; }
public byte[] Data { get; init; }
public double ReceivedMono { get; init; }
public long Sequence { get; init; }
}
private sealed class BaseFixedConfiguration
{
public string PortaUsb { get; init; }
public string PortaSaida { get; init; }
public string BaseId { get; init; }
public double Latitude { get; init; }
public double Longitude { get; init; }
public double AltitudeElipsoidal { get; init; }
}
private sealed class PositionSnapshot
{
public double Latitude { get; init; }
public double Longitude { get; init; }
public double AltitudeElipsoidal { get; init; }
public double Orientacao { get; init; }
}
}
// =============================================================
// BASE FIX SERVICE
// =============================================================
public class BaseFixService
{
public BaseFixService(SerialPort porta, GpsService service)
{
// O SerialPort recebido é mantido apenas por compatibilidade de assinatura.
// As escritas usam sempre o GpsService para não reter uma porta antiga após reconexão.
gpsService = service ?? throw new ArgumentNullException(nameof(service));
}
private readonly GpsService gpsService;
private long _lastGgaSequence;
public List<GgaFix> amostras_pos = new(1000);
public DateTime? inicioProcesso = null;
public DateTime? inicioFix = null;
public DateTime? fimProcesso = null;
private int segundosFixEstavel = 120;
private int maxJanelaSegundos = 120;
private MetodoFixacaoBase metodo = MetodoFixacaoBase.Ntrip;
public double Progresso
{
get
{
if (inicioFix is null)
return 0;
if (fimProcesso != null)
return 100;
return Math.Clamp(
(DateTime.UtcNow - inicioFix.Value).TotalSeconds /
Math.Max(1, segundosFixEstavel) * 100.0,
0,
100);
}
}
public double ProgressoGeral
{
get
{
if (inicioProcesso is null)
return 0;
return Math.Clamp(
(DateTime.UtcNow - inicioProcesso.Value).TotalSeconds /
Math.Max(1, maxJanelaSegundos) * 100.0,
0,
100);
}
}
public string ProgressoStr
{
get
{
if (CorrecaoEmAndamento)
return $"Recebendo correção RTK via {metodo}. Progresso geral: {ProgressoGeral:F2}%, progresso da correção: {Progresso:F2}%";
if (PosicaoBaseFixada && CorrecaoAbsoluta && inicioProcesso.HasValue && fimProcesso.HasValue)
return $"Correção absoluta concluída com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds:F2} segundos";
if (PosicaoBaseFixada && !CorrecaoAbsoluta)
return $"Correção relativa concluída em {segundosFixEstavel} segundos";
if (inicioProcesso.HasValue && fimProcesso.HasValue)
return $"Correção absoluta falhou com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds:F2} segundos";
return "Correção absoluta não realizada";
}
}
public bool PosicaoBaseFixada =>
gpsService.UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.BaseFix;
public bool CorrecaoAbsoluta = false;
public bool CorrecaoEmAndamento = false;
public bool FixLiberado = false;
public double LatitudeFix { get; set; }
public double LongitudeFix { get; set; }
public double AltitudeElpsoidalFix { get; set; }
public double OrientacaoFix { get; set; }
public void DefinirTempos(
int segsFixEstavel,
int segsJanelaSegs,
MetodoFixacaoBase metodo)
{
segundosFixEstavel = Math.Max(1, segsFixEstavel);
maxJanelaSegundos = Math.Max(segundosFixEstavel, segsJanelaSegs);
this.metodo = metodo;
}
public void ReiniciarFix()
{
CorrecaoEmAndamento = false;
CorrecaoAbsoluta = false;
inicioFix = null;
inicioProcesso = null;
fimProcesso = null;
FixLiberado = false;
amostras_pos.Clear();
}
public async Task<bool> FixarBaseViaNtripAsync(
string portaUsb = "com3",
string portaEntrada = "com2",
string portaSaida = "com2",
string baseId = "957",
double madK = 3.5,
Func<Task> startNtrip = null,
Func<Task> stopNtrip = null,
CancellationToken ct = default)
{
if (!APIService.HasInternetNow())
return false;
await ConfigurarComoRoverParadoAsync(portaUsb, portaEntrada, ct)
.ConfigureAwait(false);
if (startNtrip != null)
await startNtrip().ConfigureAwait(false);
try
{
List<GgaFix> samples = await EsperarFixEAmostrarAsync(ct).ConfigureAwait(false);
if (samples.Count < 10)
{
Models.Variaveis.MostrarLog(
"Poucas amostras de RTK FIX coletadas. Verifique NTRIP e sinais GNSS.");
CorrecaoAbsoluta = false;
return false;
}
var (lat, lon, h, heading, count) = FiltrarEAgrupar(samples, madK);
if (count < 5 ||
double.IsNaN(lat) ||
double.IsNaN(lon) ||
double.IsNaN(h))
{
Models.Variaveis.MostrarLog("Filtro da posição da base não produziu amostras suficientes.");
CorrecaoAbsoluta = false;
return false;
}
// Para de injetar RTCM NTRIP e aguarda o socket realmente fechar
// antes de enviar comandos ASCII de configuração ao UM982.
if (stopNtrip != null)
await stopNtrip().ConfigureAwait(false);
await AplicarBaseFixAsync(
portaUsb,
portaSaida,
baseId,
lat,
lon,
h,
ct).ConfigureAwait(false);
Models.Variaveis.MostrarLog(
$"[BASE/FIX] Coordenadas aplicadas (n={count}): lat={lat:0.000000000}, lon={lon:0.000000000}, h={h:0.000}, heading={heading:0.00}");
CorrecaoAbsoluta = true;
return true;
}
finally
{
if (stopNtrip != null)
{
try { await stopNtrip().ConfigureAwait(false); }
catch { }
}
}
}
public async Task ConfigurarComoRoverParadoAsync(
string portaUsb,
string portaEntrada,
CancellationToken ct = default)
{
Models.Variaveis.MostrarLog("Configurando GNSS temporariamente como rover parado...");
string[] commands =
{
$"config {portaUsb} 115200\r\n",
$"config {portaEntrada} 115200\r\n",
"unlog com1\r\n",
"unlog com2\r\n",
"unlog com3\r\n",
"mode rover uav\r\n",
$"gngga {portaUsb} 1\r\n",
$"gpths {portaUsb} 1\r\n",
// Não salva a configuração rover temporária.
};
await gpsService.SendCommandsAsync(commands, 250, ct).ConfigureAwait(false);
}
private async Task<List<GgaFix>> EsperarFixEAmostrarAsync(CancellationToken ct)
{
Models.Variaveis.MostrarLog("Iniciando coleta de dados para fixação da base...");
amostras_pos = new List<GgaFix>(1000);
inicioProcesso = DateTime.UtcNow;
inicioFix = null;
_lastGgaSequence = gpsService.GetGgaSequence();
DateTime noFixDeadline = DateTime.UtcNow.AddMinutes(2);
while (ProgressoGeral < 100 && FixLiberado)
{
ct.ThrowIfCancellationRequested();
if (inicioFix is null && DateTime.UtcNow >= noFixDeadline)
{
Models.Variaveis.MostrarLog("Timeout aguardando RTK FIX via NTRIP.");
break;
}
GgaFix gga = await LerProximoGgaAsync(5000, ct).ConfigureAwait(false);
if (gga is null)
continue;
bool valid = gga.FixQuality == TiposCorrecaoGPS.RTKFixo;
if (!valid &&
inicioProcesso.HasValue &&
(DateTime.UtcNow - inicioProcesso.Value).TotalSeconds > 120)
{
valid = gga.FixQuality == TiposCorrecaoGPS.RTKFlutuante;
}
if (!FixLiberado || !valid)
{
inicioFix = null;
amostras_pos.Clear();
continue;
}
if (inicioFix is null)
{
Models.Variaveis.MostrarLog("RTK válido definido. Iniciando janela de coleta estável...");
inicioFix = DateTime.UtcNow;
amostras_pos.Clear();
}
amostras_pos.Add(gga);
if (Progresso >= 100)
break;
}
return amostras_pos;
}
private async Task<GgaFix> LerProximoGgaAsync(
int timeoutMs = 5000,
CancellationToken ct = default)
{
Stopwatch sw = Stopwatch.StartNew();
while (!ct.IsCancellationRequested && sw.ElapsedMilliseconds < timeoutMs)
{
if (gpsService.TryGetGgaSnapshot(
_lastGgaSequence,
out long sequence,
out GgaFix snapshot))
{
_lastGgaSequence = sequence;
return snapshot;
}
await Task.Delay(50, ct).ConfigureAwait(false);
}
return null;
}
private (double lat, double lon, double h, double hdg, int n) FiltrarEAgrupar(
List<GgaFix> samples,
double madK = 3.5)
{
Models.Variaveis.MostrarLog("Filtrando dados aferidos...");
if (samples == null || samples.Count == 0)
return (double.NaN, double.NaN, double.NaN, double.NaN, 0);
double medLat = Median(samples.Select(x => x.LatDeg));
double medLon = Median(samples.Select(x => x.LonDeg));
double medH = Median(samples.Select(x => x.AltElipsoidalM));
double[] validHeadings = samples
.Select(x => x.HeadingDeg)
.Where(x => !double.IsNaN(x) && !double.IsInfinity(x))
.ToArray();
bool useHeadingFilter = validHeadings.Length >= Math.Max(5, samples.Count / 2);
double medHeading = useHeadingFilter
? CircularMedian(validHeadings)
: double.NaN;
double madLat = Math.Max(1e-12, Median(samples.Select(x => Math.Abs(x.LatDeg - medLat))));
double madLon = Math.Max(1e-12, Median(samples.Select(x => Math.Abs(x.LonDeg - medLon))));
double madH = Math.Max(1e-9, Median(samples.Select(x => Math.Abs(x.AltElipsoidalM - medH))));
double madHeading = useHeadingFilter
? Math.Max(0.01, Median(validHeadings.Select(x => AngularDistanceDegrees(x, medHeading))))
: 1.0;
List<GgaFix> filtered = samples.Where(x =>
Math.Abs(x.LatDeg - medLat) / madLat <= madK &&
Math.Abs(x.LonDeg - medLon) / madLon <= madK &&
Math.Abs(x.AltElipsoidalM - medH) / madH <= madK &&
(!useHeadingFilter ||
AngularDistanceDegrees(x.HeadingDeg, medHeading) / madHeading <= madK))
.ToList();
if (filtered.Count == 0)
return (double.NaN, double.NaN, double.NaN, double.NaN, 0);
double heading = CircularMean(
filtered
.Select(x => x.HeadingDeg)
.Where(x => !double.IsNaN(x) && !double.IsInfinity(x)));
return (
filtered.Average(x => x.LatDeg),
filtered.Average(x => x.LonDeg),
filtered.Average(x => x.AltElipsoidalM),
heading,
filtered.Count);
}
public async Task AplicarBaseFixAsync(
string portaUsb,
string portaSaida,
string baseId,
double latDeg,
double lonDeg,
double hEllipsM,
CancellationToken ct = default)
{
Models.Variaveis.MostrarLog("Aplicando dados de correção da base...");
string lat = latDeg.ToString("0.000000000", CultureInfo.InvariantCulture);
string lon = lonDeg.ToString("0.000000000", CultureInfo.InvariantCulture);
string h = hEllipsM.ToString("0.000", CultureInfo.InvariantCulture);
string[] commands =
{
"unlog com1\r\n",
"unlog com2\r\n",
"unlog com3\r\n",
$"mode base {baseId} {lat} {lon} {h}\r\n",
$"RTCM1006 {portaSaida} 10\r\n",
$"RTCM1033 {portaSaida} 30\r\n",
$"RTCM1074 {portaSaida} 1\r\n",
$"RTCM1084 {portaSaida} 1\r\n",
$"RTCM1094 {portaSaida} 1\r\n",
$"RTCM1124 {portaSaida} 1\r\n",
$"RTCM1230 {portaSaida} 10\r\n",
$"gngga {portaUsb} 1\r\n",
$"gpths {portaUsb} 1\r\n",
"saveconfig\r\n",
};
await gpsService.SendCommandsAsync(commands, 200, ct).ConfigureAwait(false);
}
private static double Median(IEnumerable<double> values)
{
double[] ordered = values
.Where(x => !double.IsNaN(x) && !double.IsInfinity(x))
.OrderBy(x => x)
.ToArray();
if (ordered.Length == 0)
return double.NaN;
int middle = ordered.Length / 2;
return ordered.Length % 2 == 1
? ordered[middle]
: (ordered[middle - 1] + ordered[middle]) / 2.0;
}
private static double CircularMean(IEnumerable<double> headings)
{
double[] valid = headings
.Where(x => !double.IsNaN(x) && !double.IsInfinity(x))
.ToArray();
if (valid.Length == 0)
return double.NaN;
double sin = valid.Sum(x => Math.Sin(x * Math.PI / 180.0));
double cos = valid.Sum(x => Math.Cos(x * Math.PI / 180.0));
double angle = Math.Atan2(sin, cos) * 180.0 / Math.PI;
return (angle + 360.0) % 360.0;
}
private static double CircularMedian(IEnumerable<double> headings)
{
double[] valid = headings
.Where(x => !double.IsNaN(x) && !double.IsInfinity(x))
.Select(x => (x % 360.0 + 360.0) % 360.0)
.ToArray();
if (valid.Length == 0)
return double.NaN;
return valid
.OrderBy(candidate => valid.Sum(x => AngularDistanceDegrees(x, candidate)))
.First();
}
private static double AngularDistanceDegrees(double a, double b)
{
double diff = Math.Abs(((a - b + 540.0) % 360.0) - 180.0);
return diff;
}
public enum MetodoFixacaoBase
{
Ntrip = 0,
SurveyIn = 1,
Manual = 2,
}
}
public sealed class GpsTransportMetrics
{
public bool SerialConnected { get; init; }
public string PortName { get; init; }
public double SerialLastRxAgeMs { get; init; }
public double LastValidNmeaAgeMs { get; init; }
public double LastValidGgaAgeMs { get; init; }
public double LastValidRtcmAgeMs { get; init; }
public double LastForwardedRtcmAgeMs { get; init; }
public long SerialBytesReceived { get; init; }
public long SerialReadErrors { get; init; }
public long NmeaValid { get; init; }
public long NmeaInvalid { get; init; }
public long NmeaChecksumErrors { get; init; }
public long RtcmValid { get; init; }
public long RtcmCrcErrors { get; init; }
public long RtcmInvalidLength { get; init; }
public long RtcmResyncs { get; init; }
public long RtcmQueued { get; init; }
public long RtcmReplaced { get; init; }
public long RtcmDroppedStale { get; init; }
public long RtcmForwarded { get; init; }
public long RtcmPublishErrors { get; init; }
public int RtcmQueueDepth { get; init; }
public double RtcmOldestQueueAgeMs { get; init; }
public long SerialReconnects { get; init; }
public bool NtripConnected { get; init; }
public long NtripBytesReceived { get; init; }
public long NtripReconnects { get; init; }
public long NtripErrors { get; init; }
public string NtripProvider { get; init; }
public string NtripMountpoint { get; init; }
public double? NtripMountpointDistanceKm { get; init; }
public string ExpectedRole { get; init; }
}
}
Reference in New Issue View Git Blame Copy Permalink