6038 lines
190 KiB
C#
6038 lines
190 KiB
C#
using AgroBase.Models;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Concurrent;
|
|
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.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
/// <summary>
|
|
/// Serviço GNSS/RTCM do rover.
|
|
///
|
|
/// Princípios:
|
|
/// - leitura serial nunca executa rede, disco ou tarefas lentas;
|
|
/// - toda escrita no UM982 passa por um único lock;
|
|
/// - RTCM entra em fila limitada e mensagens velhas são descartadas;
|
|
/// - NTRIP e MQTT compartilham o mesmo trilho de escrita;
|
|
/// - intervalos e idades usam relógio monotônico;
|
|
/// - configuração do módulo é exclusiva;
|
|
/// - mantém as assinaturas públicas usadas pelo projeto legado.
|
|
/// </summary>
|
|
public class GPSService
|
|
{
|
|
// ============================================================
|
|
// CONTRATO PÚBLICO LEGADO
|
|
// ============================================================
|
|
|
|
public static SerialPort PortaGPS = null;
|
|
|
|
/// <summary>
|
|
/// Estado puro. Consultar esta propriedade não abre nem altera a porta.
|
|
/// </summary>
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
SerialPort porta = PortaGPS;
|
|
return porta != null && porta.IsOpen;
|
|
}
|
|
}
|
|
|
|
public static GPSModel UltimaLeitura = new GPSModel();
|
|
public static GPSModel PenultimaLeitura = new GPSModel();
|
|
public static List<GPSModel> UltimasLeituras = new List<GPSModel>();
|
|
public static List<string> Logs = new List<string>();
|
|
|
|
public static int TaxaAmostragemHz { get; set; } = 5;
|
|
|
|
private static bool InverterHeading = true;
|
|
private static int rtk_timeout = 60;
|
|
private static int TempoMin_Ntrip = 10;
|
|
|
|
private static volatile bool LoopRTK_Ntrip = false;
|
|
public static volatile bool CorrecaoRTK_Ntrip = false;
|
|
|
|
/// <summary>
|
|
/// Indica que uma sessão NTRIP foi aceita pelo caster e está ativa.
|
|
/// CorrecaoRTK_Ntrip continua representando a solicitação/configuração.
|
|
/// </summary>
|
|
public static volatile bool Ntrip_Conectado = false;
|
|
|
|
/// <summary>
|
|
/// Estratégia de provedores NTRIP do rover.
|
|
/// </summary>
|
|
public enum NtripProviderMode
|
|
{
|
|
IbgeOnly = 0,
|
|
Rtk2goOnly = 1,
|
|
IbgeThenRtk2go = 2
|
|
}
|
|
|
|
/// <summary>
|
|
/// Por padrão tenta primeiro IBGE e, se os candidatos válidos falharem,
|
|
/// tenta RTK2go.
|
|
/// </summary>
|
|
public static NtripProviderMode NtripMode { get; set; } =
|
|
NtripProviderMode.IbgeThenRtk2go;
|
|
|
|
/// <summary>
|
|
/// Quando true, baixa a sourcetable e ordena as estações por distância
|
|
/// da posição GGA atual do rover.
|
|
/// </summary>
|
|
public static bool NtripAutoSelecionarMountpoint { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Quantidade máxima de estações tentadas por provedor em cada rodada.
|
|
/// </summary>
|
|
public static int NtripMaxCandidatosPorProvedor { get; set; } = 5;
|
|
|
|
/// <summary>
|
|
/// Limite conservador para uma estação física de referência.
|
|
/// O rover sempre tenta as mais próximas primeiro.
|
|
/// </summary>
|
|
public static double NtripDistanciaMaximaKm { get; set; } = 120.0;
|
|
|
|
/// <summary>
|
|
/// Tempo máximo aguardando uma posição GGA antes de desistir
|
|
/// da descoberta automática e aplicar o fallback seguro.
|
|
/// </summary>
|
|
public static int NtripEsperaPosicaoAutoSegundos { get; set; } = 15;
|
|
|
|
/// <summary>
|
|
/// Porta interna do UM982 na qual o rover publica GGA para o computador.
|
|
/// Deve acompanhar ConfigurarModuloRover(), cujo padrão é com3.
|
|
/// </summary>
|
|
public static string NtripPortaGgaAutoSelecao { get; set; } = "com3";
|
|
|
|
/// <summary>
|
|
/// Cache local da sourcetable para evitar downloads em cada reconnect.
|
|
/// </summary>
|
|
public static int NtripSourcetableCacheSegundos { get; set; } = 600;
|
|
|
|
/// <summary>
|
|
/// Um HTTP 200 sem nenhum byte de correção dentro desta janela é tratado
|
|
/// como sessão sem dados e o próximo candidato é tentado.
|
|
/// </summary>
|
|
public static int NtripPrimeirosDadosTimeoutSegundos { get; set; } = 8;
|
|
|
|
/// <summary>
|
|
/// Mountpoints que falham ficam temporariamente suspensos para não
|
|
/// martelar repetidamente a mesma estação.
|
|
/// </summary>
|
|
public static int NtripCooldownMountpointSegundos { get; set; } = 60;
|
|
|
|
// IBGE / RBMC-IP. Mountpoint manual é mantido apenas como override/fallback.
|
|
public static string NtripHost { get; set; } = "gps-ntrip.ibge.gov.br";
|
|
public static int NtripPort { get; set; } = 2101;
|
|
public static string NtripMountpoint { get; set; } = "EESC0";
|
|
public static string NtripUsername { get; set; } =
|
|
Environment.GetEnvironmentVariable("AGRO_NTRIP_USERNAME") ?? string.Empty;
|
|
public static string NtripPassword { get; set; } =
|
|
Environment.GetEnvironmentVariable("AGRO_NTRIP_PASSWORD") ?? string.Empty;
|
|
|
|
// RTK2go. O usuário é normalmente um e-mail; a senha padrão do caster é "none".
|
|
public static string NtripRtk2goHost { get; set; } = "rtk2go.com";
|
|
public static int NtripRtk2goPort { get; set; } = 2101;
|
|
public static string NtripRtk2goMountpoint { get; set; } = "SPLI1";
|
|
public static string NtripRtk2goUsername { get; set; } =
|
|
Environment.GetEnvironmentVariable("AGRO_RTK2GO_EMAIL") ?? string.Empty;
|
|
public static string NtripRtk2goPassword { get; set; } = "none";
|
|
|
|
/// <summary>
|
|
/// Diagnóstico do caster efetivamente utilizado.
|
|
/// </summary>
|
|
public static string NtripProviderAtivo { get; private set; } = string.Empty;
|
|
public static string NtripMountpointAtivo { get; private set; } = string.Empty;
|
|
public static double? NtripDistanciaMountpointAtivoKm { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Permite configurar credenciais apenas em memória, sem gravá-las no fonte.
|
|
/// </summary>
|
|
public static 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();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Mantido para compatibilidade e diagnóstico humano.
|
|
/// Watchdogs internos usam Stopwatch.
|
|
/// </summary>
|
|
public static DateTime UltimoEnvioCorrecaoRTK = DateTime.MinValue;
|
|
|
|
public static GeoLeverArm LeverArm = new GeoLeverArm(
|
|
offsetFisicoFrontalCm: VariaveisEquipamento.LeverArmFrontalCm,
|
|
offsetFisicoLateralCm: VariaveisEquipamento.LeverArmLateralCm,
|
|
mirrL: true,
|
|
invF: true,
|
|
invH: false
|
|
);
|
|
|
|
public static Queue<GPSModel> historicoPosicao = new Queue<GPSModel>();
|
|
|
|
// ============================================================
|
|
// SINCRONIZAÇÃO E CICLO DE VIDA
|
|
// ============================================================
|
|
|
|
private static readonly object _stateLock = new object();
|
|
private static readonly object _bufferLock = new object();
|
|
private static readonly object _rtcmQueueLock = new object();
|
|
private static readonly object _workerLock = new object();
|
|
private static readonly object _portLifecycleLock = new object();
|
|
|
|
private static readonly SemaphoreSlim _serialWriteLock = new SemaphoreSlim(1, 1);
|
|
private static readonly SemaphoreSlim _configurationLock = new SemaphoreSlim(1, 1);
|
|
private static readonly SemaphoreSlim _rtcmSignal = new SemaphoreSlim(0, int.MaxValue);
|
|
private static readonly SemaphoreSlim _logSignal = new SemaphoreSlim(0, int.MaxValue);
|
|
private static readonly SemaphoreSlim _localPublishSignal = new SemaphoreSlim(0, int.MaxValue);
|
|
|
|
private static CancellationTokenSource _serviceCts;
|
|
private static Task _nmeaWorkerTask;
|
|
private static Task _rtcmWriterTask;
|
|
private static Task _logWriterTask;
|
|
private static Task _localPublisherTask;
|
|
|
|
private static CancellationTokenSource _configurationCts;
|
|
|
|
private static CancellationTokenSource _ntripCts;
|
|
private static Task _ntripTask;
|
|
private static TcpClient _ntripClient;
|
|
|
|
private static readonly object _ntripSourcetableLock = new object();
|
|
private static readonly Dictionary<string, NtripSourcetableCacheEntry>
|
|
_ntripSourcetableCache =
|
|
new Dictionary<string, NtripSourcetableCacheEntry>(
|
|
StringComparer.OrdinalIgnoreCase);
|
|
|
|
private static readonly object _ntripCooldownLock = new object();
|
|
private static readonly Dictionary<string, DateTime> _ntripCooldownAteUtc =
|
|
new Dictionary<string, DateTime>(
|
|
StringComparer.OrdinalIgnoreCase);
|
|
|
|
private static long _portGeneration;
|
|
|
|
// ============================================================
|
|
// BUFFERS E FILAS
|
|
// ============================================================
|
|
|
|
private static readonly StringBuilder _nmeaBuffer = new StringBuilder(8192);
|
|
|
|
private const int MaxNmeaBufferChars = 64 * 1024;
|
|
|
|
private static readonly object _nmeaQueueLock = new object();
|
|
private static readonly Queue<string> _nmeaQueue = new Queue<string>();
|
|
private static readonly SemaphoreSlim _nmeaSignal = new SemaphoreSlim(0, int.MaxValue);
|
|
private const int NmeaQueueCapacity = 512;
|
|
|
|
private static readonly Queue<RtcmEnvelope> _rtcmQueue = new Queue<RtcmEnvelope>();
|
|
|
|
private const int RtcmQueueCapacity = 48;
|
|
private const int RtcmMaxAgeMs = 2500;
|
|
private const int RtcmMaxPayloadBytes = 64 * 1024;
|
|
|
|
private static long _rtcmSequence;
|
|
|
|
private static readonly ConcurrentQueue<string> _gpsLogQueue = new ConcurrentQueue<string>();
|
|
|
|
private static readonly object _localPublishLock = new object();
|
|
private static readonly Dictionary<string, string> _localLatestPayload = new Dictionary<string, string>(StringComparer.Ordinal);
|
|
|
|
// ============================================================
|
|
// MÉTRICAS
|
|
// ============================================================
|
|
|
|
private static long _lastSerialRxMono;
|
|
private static long _lastValidNmeaMono;
|
|
private static long _lastValidGgaMono;
|
|
private static long _lastValidThsMono;
|
|
private static long _lastRtcmReceivedMono;
|
|
private static long _lastRtcmWrittenMono;
|
|
|
|
private static long _serialBytesReceived;
|
|
private static long _nmeaValid;
|
|
private static long _nmeaInvalid;
|
|
private static long _nmeaChecksumErrors;
|
|
private static long _nmeaBufferResets;
|
|
private static long _nmeaQueued;
|
|
private static long _nmeaDroppedQueue;
|
|
|
|
private static long _rtcmReceived;
|
|
private static long _rtcmQueued;
|
|
private static long _rtcmWritten;
|
|
private static long _rtcmDroppedQueue;
|
|
private static long _rtcmDroppedStale;
|
|
private static long _rtcmDroppedInvalid;
|
|
private static long _rtcmWriteErrors;
|
|
|
|
private static long _configurationRuns;
|
|
private static long _configurationErrors;
|
|
|
|
private static long _ntripReconnects;
|
|
private static long _ntripBytesReceived;
|
|
private static long _ntripErrors;
|
|
|
|
private static string _lastError;
|
|
private static readonly object _metricsTextLock = new object();
|
|
|
|
|
|
|
|
private const int IntervaloTrajetoriaMapaMs = 1_000;
|
|
private const int MaxPontosTrajetoriaMapa = 800;
|
|
|
|
private static int _publicacaoMapaAutomaticaHabilitada = 1;
|
|
private static long _ultimaTrajetoriaMapaMono;
|
|
|
|
/// <summary>
|
|
/// Permite que uma tela seja dona da publicação visual sem interromper o
|
|
/// processamento GNSS, Redis, trajetória ou histórico da operação.
|
|
/// O valor padrão preserva o comportamento legado.
|
|
/// </summary>
|
|
public static bool PublicacaoMapaAutomaticaHabilitada
|
|
{
|
|
get
|
|
{
|
|
return Volatile.Read(
|
|
ref _publicacaoMapaAutomaticaHabilitada
|
|
) == 1;
|
|
}
|
|
set
|
|
{
|
|
Volatile.Write(
|
|
ref _publicacaoMapaAutomaticaHabilitada,
|
|
value ? 1 : 0
|
|
);
|
|
}
|
|
}
|
|
|
|
public static void AtualizarLeituraSimulada(GPSModel leitura, int atrasoPosicaoPassos = 0)
|
|
{
|
|
if (leitura == null)
|
|
throw new ArgumentNullException(nameof(leitura));
|
|
|
|
lock (_stateLock)
|
|
{
|
|
/*
|
|
* A leitura simulada deve obedecer à mesma semântica temporal
|
|
* da leitura GNSS real:
|
|
*
|
|
* PenultimaLeitura = amostra imediatamente anterior
|
|
* UltimaLeitura = nova amostra
|
|
*
|
|
* Sem isso, a posição anterior fica congelada durante toda
|
|
* a simulação e contamina Aproximando, orientação anterior,
|
|
* deslocamento GNSS e recuperação da trajetória.
|
|
*/
|
|
CopiarPosicaoParaPenultima();
|
|
|
|
double orientacaoReal =
|
|
GPSUtils.NormalizarAngulo(leitura.OrientacaoReal);
|
|
|
|
UltimaLeitura.Momento = leitura.Momento;
|
|
UltimaLeitura.DataHora = leitura.DataHora;
|
|
UltimaLeitura.UltimoComandoRespondido =
|
|
leitura.UltimoComandoRespondido;
|
|
|
|
UltimaLeitura.Latitude = leitura.Latitude;
|
|
UltimaLeitura.Longitude = leitura.Longitude;
|
|
UltimaLeitura.LatitudeAnt = leitura.LatitudeAnt;
|
|
UltimaLeitura.LongitudeAnt = leitura.LongitudeAnt;
|
|
|
|
UltimaLeitura.OrientacaoReal = orientacaoReal;
|
|
UltimaLeitura.AnguloCarroDefinido = orientacaoReal;
|
|
UltimaLeitura.TipoOrientacao = "A";
|
|
|
|
UltimaLeitura.OrientacaoMovimento =
|
|
double.IsNaN(leitura.OrientacaoMovimento) ||
|
|
double.IsInfinity(leitura.OrientacaoMovimento)
|
|
? orientacaoReal
|
|
: GPSUtils.NormalizarAngulo(
|
|
leitura.OrientacaoMovimento
|
|
);
|
|
|
|
UltimaLeitura.Distancia = leitura.Distancia;
|
|
UltimaLeitura.Velocidade = leitura.Velocidade;
|
|
|
|
if (leitura.TimestampOri != null)
|
|
UltimaLeitura.TimestampOri =
|
|
leitura.TimestampOri.Clone();
|
|
|
|
if (leitura.TimestampPos != null)
|
|
UltimaLeitura.TimestampPos =
|
|
leitura.TimestampPos.Clone();
|
|
|
|
UltimaLeitura.Heartbeat = leitura.Heartbeat;
|
|
UltimaLeitura.Inicializado = true;
|
|
|
|
AtualizarAtrasoPosicoes(
|
|
Math.Max(0, atrasoPosicaoPassos)
|
|
);
|
|
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
// ============================================================
|
|
// INICIALIZAÇÃO DA PORTA
|
|
// ============================================================
|
|
|
|
public static void AtualizarPortaCOM(SerialPort porta)
|
|
{
|
|
if (porta == null)
|
|
throw new ArgumentNullException(nameof(porta));
|
|
|
|
EnsureBackgroundWorkersStarted();
|
|
|
|
long generation = Interlocked.Increment(ref _portGeneration);
|
|
|
|
/*
|
|
* A troca da porta participa do mesmo trilho de escrita.
|
|
* Assim nenhum RTCM ou comando ASCII escreve enquanto a
|
|
* SerialPort está sendo fechada e substituída.
|
|
*/
|
|
_serialWriteLock.Wait();
|
|
|
|
try
|
|
{
|
|
lock (_portLifecycleLock)
|
|
{
|
|
SerialPort anterior = PortaGPS;
|
|
|
|
if (anterior != null)
|
|
{
|
|
try
|
|
{
|
|
anterior.DataReceived -= PortaGPS_DataReceived;
|
|
}
|
|
catch { }
|
|
|
|
try
|
|
{
|
|
if (anterior.IsOpen)
|
|
anterior.Close();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.AtualizarPortaCOM] Erro ao fechar porta antiga: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
|
|
try
|
|
{
|
|
anterior.Dispose();
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
var novaPorta = new SerialPort
|
|
{
|
|
BaudRate = porta.BaudRate,
|
|
PortName = porta.PortName,
|
|
ReadTimeout = 2000,
|
|
WriteTimeout = 2000,
|
|
Encoding = Encoding.ASCII,
|
|
DtrEnable = false,
|
|
RtsEnable = false
|
|
};
|
|
|
|
novaPorta.DataReceived += PortaGPS_DataReceived;
|
|
|
|
try
|
|
{
|
|
if (porta.IsOpen)
|
|
porta.Close();
|
|
}
|
|
catch { }
|
|
|
|
PortaGPS = novaPorta;
|
|
|
|
try
|
|
{
|
|
novaPorta.Open();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
novaPorta.DataReceived -= PortaGPS_DataReceived;
|
|
PortaGPS = null;
|
|
RecordError(
|
|
"[GPSService.AtualizarPortaCOM] Não foi possível abrir " +
|
|
novaPorta.PortName + ": " + ex.Message
|
|
);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
|
|
DefinirDispositivo();
|
|
|
|
_ = Task.Run(async () =>
|
|
{
|
|
try
|
|
{
|
|
await ConfigurarModulo().ConfigureAwait(false);
|
|
|
|
if (generation != Interlocked.Read(ref _portGeneration))
|
|
return;
|
|
|
|
if (Variaveis.IsAgroBase && CorrecaoRTK_Ntrip)
|
|
await IniciarNtripAsync().ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.AtualizarPortaCOM] Falha pós-conexão: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
});
|
|
}
|
|
|
|
private static void DefinirDispositivo()
|
|
{
|
|
SerialPort porta = PortaGPS;
|
|
|
|
if (porta == null || !porta.IsOpen)
|
|
return;
|
|
|
|
try
|
|
{
|
|
bool jaExiste = SerialService.DispositivosMapeados.Any(
|
|
x => x.Dispositivo == T_Code.Gps &&
|
|
string.Equals(
|
|
x.Endereco,
|
|
porta.PortName,
|
|
StringComparison.OrdinalIgnoreCase
|
|
)
|
|
);
|
|
|
|
if (!jaExiste)
|
|
{
|
|
SerialService.DispositivosMapeados.Add(
|
|
new DispositivoDetalhesModel
|
|
{
|
|
Dispositivo = T_Code.Gps,
|
|
Endereco = porta.PortName,
|
|
Versao = "1"
|
|
}
|
|
);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.DefinirDispositivo] " + ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// CONFIGURAÇÃO DO UM982
|
|
// ============================================================
|
|
|
|
public static async Task ConfigurarModulo()
|
|
{
|
|
EnsureBackgroundWorkersStarted();
|
|
|
|
CancellationTokenSource novaCts = new CancellationTokenSource();
|
|
CancellationTokenSource anterior =
|
|
Interlocked.Exchange(ref _configurationCts, novaCts);
|
|
|
|
if (anterior != null)
|
|
{
|
|
try { anterior.Cancel(); } catch { }
|
|
anterior.Dispose();
|
|
}
|
|
|
|
await ConfigurarModuloRover(
|
|
comprimento_antena: 100,
|
|
cancellationToken: novaCts.Token
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
private static async Task ConfigurarModuloRover(string porta_usb = "com3", string porta_entrada = "com2", int comprimento_antena = 100, int tolerancia_antena = 5, CancellationToken cancellationToken = default(CancellationToken))
|
|
{
|
|
await _configurationLock.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
Interlocked.Increment(ref _configurationRuns);
|
|
|
|
try
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.ConfigurarModuloRover] Iniciando configuração."
|
|
);
|
|
|
|
string freq = (1.0 / Math.Max(1, TaxaAmostragemHz))
|
|
.ToString("0.0", CultureInfo.InvariantCulture);
|
|
|
|
string[] comandos =
|
|
{
|
|
"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 rover uav\r\n",
|
|
|
|
"config rtk timeout " + rtk_timeout + "\r\n",
|
|
"config dgps timeout 60\r\n",
|
|
|
|
"config heading fixlength\r\n",
|
|
"config heading length " +
|
|
comprimento_antena + " " +
|
|
tolerancia_antena + "\r\n",
|
|
|
|
"gngga " + porta_usb + " " + freq + "\r\n",
|
|
"gpths " + porta_usb + " " + freq + "\r\n",
|
|
"gpvtg " + porta_usb + " " + freq + "\r\n",
|
|
|
|
"saveconfig\r\n"
|
|
};
|
|
|
|
await Task.Delay(1500, cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
/*
|
|
* Mantém exclusividade durante a sequência completa.
|
|
* RTCMs que envelhecerem enquanto o módulo é configurado
|
|
* serão descartados pelo worker, em vez de serem injetados
|
|
* entre comandos ASCII.
|
|
*/
|
|
await _serialWriteLock.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
foreach (string comando in comandos)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
WriteSerialUnsafe(
|
|
Encoding.ASCII.GetBytes(comando),
|
|
comando.Length
|
|
);
|
|
|
|
await Task.Delay(350, cancellationToken)
|
|
.ConfigureAwait(false);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Interlocked.Increment(ref _configurationErrors);
|
|
RecordError(
|
|
"[GPSService.ConfigurarModuloRover] " + ex.Message
|
|
);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
_configurationLock.Release();
|
|
}
|
|
}
|
|
|
|
private static async Task ConfigurarModuloBase(string porta_usb = "com3", string porta_saida = "com2", int tempo_fixacao = 60, CancellationToken cancellationToken = default(CancellationToken))
|
|
{
|
|
await _configurationLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
|
|
Interlocked.Increment(ref _configurationRuns);
|
|
|
|
try
|
|
{
|
|
string[] comandos =
|
|
{
|
|
"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 957 time " + tempo_fixacao + " 0\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 _serialWriteLock.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
foreach (string comando in comandos)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
byte[] bytes = Encoding.ASCII.GetBytes(comando);
|
|
WriteSerialUnsafe(bytes, bytes.Length);
|
|
|
|
await Task.Delay(350, cancellationToken)
|
|
.ConfigureAwait(false);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
Interlocked.Increment(ref _configurationErrors);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
_configurationLock.Release();
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// RECEPÇÃO SERIAL / NMEA
|
|
// ============================================================
|
|
|
|
private static void PortaGPS_DataReceived(object sender, SerialDataReceivedEventArgs e)
|
|
{
|
|
SerialPort porta = sender as SerialPort;
|
|
|
|
if (porta == null || !porta.IsOpen)
|
|
return;
|
|
|
|
try
|
|
{
|
|
string recebido = porta.ReadExisting();
|
|
|
|
if (string.IsNullOrEmpty(recebido))
|
|
return;
|
|
|
|
Interlocked.Add(
|
|
ref _serialBytesReceived,
|
|
Encoding.ASCII.GetByteCount(recebido)
|
|
);
|
|
|
|
Interlocked.Exchange(
|
|
ref _lastSerialRxMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
|
|
List<string> sentencas = ExtrairSentencasCompletas(recebido);
|
|
|
|
foreach (string sentenca in sentencas)
|
|
{
|
|
EnfileirarSentencaNmea(sentenca);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.PortaGPS_DataReceived] " + ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
private static void EnfileirarSentencaNmea(string sentenca)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(sentenca))
|
|
return;
|
|
|
|
lock (_nmeaQueueLock)
|
|
{
|
|
while (_nmeaQueue.Count >= NmeaQueueCapacity)
|
|
{
|
|
_nmeaQueue.Dequeue();
|
|
Interlocked.Increment(ref _nmeaDroppedQueue);
|
|
}
|
|
|
|
_nmeaQueue.Enqueue(sentenca);
|
|
Interlocked.Increment(ref _nmeaQueued);
|
|
}
|
|
|
|
try { _nmeaSignal.Release(); }
|
|
catch (SemaphoreFullException) { }
|
|
}
|
|
|
|
private static async Task NmeaWorkerLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await _nmeaSignal.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
|
|
bool processouAlgo = false;
|
|
|
|
while (true)
|
|
{
|
|
string sentenca = null;
|
|
|
|
lock (_nmeaQueueLock)
|
|
{
|
|
if (_nmeaQueue.Count > 0)
|
|
sentenca = _nmeaQueue.Dequeue();
|
|
}
|
|
|
|
if (sentenca == null)
|
|
break;
|
|
|
|
ProcessarSentencaSegura(sentenca);
|
|
processouAlgo = true;
|
|
}
|
|
|
|
if (processouAlgo)
|
|
EnfileirarLogSnapshot();
|
|
}
|
|
}
|
|
|
|
private static List<string> ExtrairSentencasCompletas(string recebido)
|
|
{
|
|
var resultado = new List<string>();
|
|
|
|
lock (_bufferLock)
|
|
{
|
|
_nmeaBuffer.Append(recebido);
|
|
|
|
if (_nmeaBuffer.Length > MaxNmeaBufferChars)
|
|
{
|
|
/*
|
|
* Tenta preservar a última possível sentença.
|
|
* Caso não exista '$', limpa completamente.
|
|
*/
|
|
string acumuladoExcedido = _nmeaBuffer.ToString();
|
|
int ultimoInicio = acumuladoExcedido.LastIndexOf('$');
|
|
|
|
_nmeaBuffer.Clear();
|
|
|
|
if (ultimoInicio >= 0 &&
|
|
acumuladoExcedido.Length - ultimoInicio < 2048)
|
|
{
|
|
_nmeaBuffer.Append(
|
|
acumuladoExcedido.Substring(ultimoInicio)
|
|
);
|
|
}
|
|
|
|
Interlocked.Increment(ref _nmeaBufferResets);
|
|
}
|
|
|
|
while (true)
|
|
{
|
|
string acumulado = _nmeaBuffer.ToString();
|
|
int fim = acumulado.IndexOf('\n');
|
|
|
|
if (fim < 0)
|
|
break;
|
|
|
|
string linha = acumulado.Substring(0, fim)
|
|
.Trim('\r', '\n', ' ', '\t');
|
|
|
|
_nmeaBuffer.Remove(0, fim + 1);
|
|
|
|
if (!string.IsNullOrWhiteSpace(linha))
|
|
resultado.Add(linha);
|
|
}
|
|
}
|
|
|
|
return resultado;
|
|
}
|
|
|
|
private static void ProcessarSentencaSegura(string sentenca)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(sentenca))
|
|
return;
|
|
|
|
try
|
|
{
|
|
if (!ValidarChecksumNmea(sentenca))
|
|
{
|
|
Interlocked.Increment(ref _nmeaChecksumErrors);
|
|
Interlocked.Increment(ref _nmeaInvalid);
|
|
return;
|
|
}
|
|
|
|
lock (_stateLock)
|
|
{
|
|
ProcessarDadosNMEA(sentenca);
|
|
}
|
|
|
|
Interlocked.Increment(ref _nmeaValid);
|
|
Interlocked.Exchange(
|
|
ref _lastValidNmeaMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Interlocked.Increment(ref _nmeaInvalid);
|
|
RecordError(
|
|
"[GPSService.ProcessarSentencaSegura] Sentença '" +
|
|
sentenca + "': " + ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
private static bool ValidarChecksumNmea(string sentenca)
|
|
{
|
|
if (sentenca.StartsWith(
|
|
"$command,",
|
|
StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if (!sentenca.StartsWith("$", StringComparison.Ordinal))
|
|
return false;
|
|
|
|
int asterisco = sentenca.LastIndexOf('*');
|
|
|
|
/*
|
|
* Alguns ACKs e firmwares podem não fornecer checksum.
|
|
* Sentenças de navegação sem '*' são aceitas por compatibilidade,
|
|
* mas as que possuem checksum são verificadas rigorosamente.
|
|
*/
|
|
if (asterisco < 0)
|
|
return true;
|
|
|
|
if (asterisco + 2 >= sentenca.Length)
|
|
return false;
|
|
|
|
byte calculado = 0;
|
|
|
|
for (int i = 1; i < asterisco; i++)
|
|
calculado ^= (byte)sentenca[i];
|
|
|
|
byte informado;
|
|
|
|
return byte.TryParse(
|
|
sentenca.Substring(asterisco + 1, 2),
|
|
NumberStyles.HexNumber,
|
|
CultureInfo.InvariantCulture,
|
|
out informado
|
|
) &&
|
|
calculado == informado;
|
|
}
|
|
|
|
private static void ProcessarDadosNMEA(string sentenca)
|
|
{
|
|
DateTime agora = DateTime.Now;
|
|
|
|
if (sentenca.StartsWith("$GPGGA"))
|
|
{
|
|
ProcessarGPGGA(sentenca);
|
|
AtualizarCoordenadasGPS();
|
|
MarkValidGga();
|
|
}
|
|
else if (sentenca.StartsWith("$GNGGA") ||
|
|
sentenca.StartsWith("$GLGGA"))
|
|
{
|
|
ProcessarGNGGA(sentenca);
|
|
AtualizarCoordenadasGPS();
|
|
MarkValidGga();
|
|
}
|
|
else if (sentenca.StartsWith("$GNRMC"))
|
|
{
|
|
ProcessarGxRMC(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GNVTG") ||
|
|
sentenca.StartsWith("$GPVTG"))
|
|
{
|
|
ProcessarGNVTG(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GPGSV") ||
|
|
sentenca.StartsWith("$GLGSV") ||
|
|
sentenca.StartsWith("$GBGSV") ||
|
|
sentenca.StartsWith("$GAGSV"))
|
|
{
|
|
ProcessarGSV(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GNTHS") ||
|
|
sentenca.StartsWith("$GPTHS") ||
|
|
sentenca.StartsWith("$GATHS"))
|
|
{
|
|
ProcessarGNTHS(sentenca);
|
|
|
|
Interlocked.Exchange(
|
|
ref _lastValidThsMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
}
|
|
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);
|
|
}
|
|
else if (sentenca.StartsWith(
|
|
"$command,",
|
|
StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.ProcessarDadosNMEA] " + sentenca
|
|
);
|
|
}
|
|
|
|
PenultimaLeitura.UltimoComandoRespondido =
|
|
UltimaLeitura.UltimoComandoRespondido;
|
|
|
|
UltimaLeitura.UltimoComandoRespondido = agora;
|
|
}
|
|
|
|
private static void MarkValidGga()
|
|
{
|
|
Interlocked.Exchange(
|
|
ref _lastValidGgaMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
}
|
|
|
|
private static void ProcessarGPGGA(string sentenca)
|
|
{
|
|
string[] parts = sentenca.Split(',');
|
|
|
|
if (parts.Length < 10)
|
|
throw new FormatException("GPGGA incompleta.");
|
|
|
|
CopiarPosicaoParaPenultima();
|
|
|
|
UltimaLeitura.TimestampPos.valor =
|
|
Stopwatch.GetTimestamp() /
|
|
(double)Stopwatch.Frequency;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
double lat;
|
|
double lon;
|
|
|
|
if (TryParseDmm(parts[2], parts[3], 2, out lat) &&
|
|
TryParseDmm(parts[4], parts[5], 3, out lon))
|
|
{
|
|
UltimaLeitura.LatitudeAnt = lat;
|
|
UltimaLeitura.LongitudeAnt = lon;
|
|
AplicarPosicaoCorrigida();
|
|
}
|
|
|
|
int numSat;
|
|
double hdop;
|
|
double alt;
|
|
|
|
UltimaLeitura.NumeroSatelites =
|
|
int.TryParse(parts[7], out numSat) ? numSat : 0;
|
|
|
|
UltimaLeitura.PrecisaoHorizontal =
|
|
double.TryParse(
|
|
parts[8],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out hdop
|
|
) ? hdop : 0;
|
|
|
|
UltimaLeitura.Altitude =
|
|
double.TryParse(
|
|
parts[9],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out alt
|
|
) ? alt : 0;
|
|
}
|
|
|
|
private static void ProcessarGNGGA(string sentenca)
|
|
{
|
|
string[] campos = sentenca.Split(',');
|
|
|
|
if (campos.Length < 10)
|
|
throw new FormatException("GNGGA incompleta.");
|
|
|
|
string horaUtc = GetField(campos, 1);
|
|
string latRaw = GetField(campos, 2);
|
|
string latHem = GetField(campos, 3);
|
|
string lonRaw = GetField(campos, 4);
|
|
string lonHem = GetField(campos, 5);
|
|
string qualidadeRaw = GetField(campos, 6, "0");
|
|
string satRaw = GetField(campos, 7, "0");
|
|
string hdopRaw = GetField(campos, 8, "99.9");
|
|
string altRaw = GetField(campos, 9, "0");
|
|
string geoidRaw = GetField(campos, 11, "0");
|
|
string idadeRaw = GetField(campos, 13);
|
|
string baseId = StripChecksum(GetField(campos, 14));
|
|
|
|
double latitude = 0;
|
|
double longitude = 0;
|
|
bool possuiLat = TryParseDmm(
|
|
latRaw,
|
|
latHem,
|
|
2,
|
|
out latitude
|
|
);
|
|
bool possuiLon = TryParseDmm(
|
|
lonRaw,
|
|
lonHem,
|
|
3,
|
|
out longitude
|
|
);
|
|
|
|
double altMsl;
|
|
double geoid;
|
|
double hdop;
|
|
int sats;
|
|
int fixCode;
|
|
|
|
double.TryParse(
|
|
altRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out altMsl
|
|
);
|
|
|
|
double.TryParse(
|
|
geoidRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out geoid
|
|
);
|
|
|
|
double.TryParse(
|
|
hdopRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out hdop
|
|
);
|
|
|
|
int.TryParse(satRaw, out sats);
|
|
int.TryParse(qualidadeRaw, out fixCode);
|
|
|
|
double idadeCorrecao = -1;
|
|
double idadeValida;
|
|
|
|
if (!string.IsNullOrWhiteSpace(idadeRaw) &&
|
|
double.TryParse(
|
|
idadeRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out idadeValida))
|
|
{
|
|
idadeCorrecao = idadeValida;
|
|
}
|
|
|
|
CopiarPosicaoParaPenultima();
|
|
|
|
UltimaLeitura.TimestampPos.valor =
|
|
Stopwatch.GetTimestamp() /
|
|
(double)Stopwatch.Frequency;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
if (possuiLat && possuiLon)
|
|
{
|
|
UltimaLeitura.LatitudeAnt = latitude;
|
|
UltimaLeitura.LongitudeAnt = longitude;
|
|
}
|
|
|
|
UltimaLeitura.Altitude = altMsl;
|
|
UltimaLeitura.AltitudeElipsoidal = altMsl + geoid;
|
|
UltimaLeitura.PrecisaoHorizontal = hdop;
|
|
UltimaLeitura.NumeroSatelites = sats;
|
|
UltimaLeitura.QualidadeFix = (TiposCorrecaoGPS)fixCode;
|
|
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
|
|
UltimaLeitura.BaseID = baseId;
|
|
|
|
if (possuiLat && possuiLon)
|
|
AplicarPosicaoCorrigida();
|
|
|
|
DateTime dataHora;
|
|
|
|
if (TryParseNmeaTime(horaUtc, out dataHora))
|
|
UltimaLeitura.DataHora = dataHora;
|
|
|
|
if (!UltimaLeitura.EnuOriginSet &&
|
|
UltimaLeitura.QualidadeFix ==
|
|
TiposCorrecaoGPS.RTKFixo &&
|
|
possuiLat &&
|
|
possuiLon)
|
|
{
|
|
UltimaLeitura.Lat0 = latitude;
|
|
UltimaLeitura.Lon0 = longitude;
|
|
UltimaLeitura.EnuOriginSet = true;
|
|
}
|
|
}
|
|
|
|
private static void CopiarPosicaoParaPenultima()
|
|
{
|
|
PenultimaLeitura.TimestampPos =
|
|
UltimaLeitura.TimestampPos.Clone();
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
|
|
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
|
|
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
|
PenultimaLeitura.AltitudeElipsoidal =
|
|
UltimaLeitura.AltitudeElipsoidal;
|
|
|
|
PenultimaLeitura.PrecisaoHorizontal =
|
|
UltimaLeitura.PrecisaoHorizontal;
|
|
|
|
PenultimaLeitura.NumeroSatelites =
|
|
UltimaLeitura.NumeroSatelites;
|
|
|
|
PenultimaLeitura.QualidadeFix =
|
|
UltimaLeitura.QualidadeFix;
|
|
|
|
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
|
PenultimaLeitura.IdadeCorrecao =
|
|
UltimaLeitura.IdadeCorrecao;
|
|
|
|
PenultimaLeitura.BaseID = UltimaLeitura.BaseID;
|
|
}
|
|
|
|
private static void ProcessarGNVTG(string sentenca)
|
|
{
|
|
string[] campos = sentenca.Split(',');
|
|
|
|
if (campos.Length < 8)
|
|
throw new FormatException("VTG incompleta.");
|
|
|
|
double curso;
|
|
double velocidadeKmh;
|
|
|
|
double.TryParse(
|
|
campos[1],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out curso
|
|
);
|
|
|
|
double.TryParse(
|
|
campos[7],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out velocidadeKmh
|
|
);
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.CursoVerdadeiro =
|
|
UltimaLeitura.CursoVerdadeiro;
|
|
|
|
PenultimaLeitura.Velocidade =
|
|
UltimaLeitura.Velocidade;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
UltimaLeitura.CursoVerdadeiro = curso;
|
|
UltimaLeitura.Velocidade = velocidadeKmh;
|
|
}
|
|
|
|
private static void ProcessarGSV(string sentenca)
|
|
{
|
|
string[] campos = sentenca.Split(',');
|
|
|
|
if (campos.Length < 4 || sentenca.Length < 3)
|
|
throw new FormatException("GSV incompleta.");
|
|
|
|
string tipoSistema = sentenca.Substring(1, 2);
|
|
|
|
int sentAtual;
|
|
int sentTotal;
|
|
int visiveis;
|
|
|
|
int.TryParse(campos[2], out sentAtual);
|
|
int.TryParse(campos[1], out sentTotal);
|
|
int.TryParse(campos[3], out visiveis);
|
|
|
|
if (UltimaLeitura.SatelitesEmVista == null)
|
|
{
|
|
UltimaLeitura.SatelitesEmVista =
|
|
new List<GPSSatelitesEmVistaModel>();
|
|
}
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.SatelitesEmVista =
|
|
new List<GPSSatelitesEmVistaModel>(
|
|
UltimaLeitura.SatelitesEmVista
|
|
);
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
GPSSatelitesEmVistaModel leitura =
|
|
UltimaLeitura.SatelitesEmVista
|
|
.FirstOrDefault(x => x.TipoSistema == tipoSistema);
|
|
|
|
if (leitura == null)
|
|
{
|
|
leitura = new GPSSatelitesEmVistaModel
|
|
{
|
|
TipoSistema = tipoSistema,
|
|
Sentencas =
|
|
new List<GPSSatelitesEmVistaSentencaModel>()
|
|
};
|
|
|
|
UltimaLeitura.SatelitesEmVista.Add(leitura);
|
|
}
|
|
|
|
GPSSatelitesEmVistaSentencaModel parte =
|
|
leitura.Sentencas.FirstOrDefault(
|
|
x => x.SentencaAtual == sentAtual
|
|
);
|
|
|
|
if (parte == null)
|
|
{
|
|
parte = new GPSSatelitesEmVistaSentencaModel
|
|
{
|
|
SentencaAtual = sentAtual,
|
|
SentencasTotal = sentTotal,
|
|
QuantidadeSatelites = visiveis,
|
|
Dados =
|
|
new List<GPSSatelitesEmVistaDadosModel>()
|
|
};
|
|
|
|
leitura.Sentencas.Add(parte);
|
|
}
|
|
|
|
parte.Dados =
|
|
new List<GPSSatelitesEmVistaDadosModel>();
|
|
|
|
for (int i = 4; i + 3 < campos.Length; i += 4)
|
|
{
|
|
double elevacao;
|
|
double azimute;
|
|
double snr;
|
|
|
|
double.TryParse(
|
|
campos[i + 1],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out elevacao
|
|
);
|
|
|
|
double.TryParse(
|
|
campos[i + 2],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out azimute
|
|
);
|
|
|
|
double.TryParse(
|
|
StripChecksum(campos[i + 3]),
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out snr
|
|
);
|
|
|
|
parte.Dados.Add(
|
|
new GPSSatelitesEmVistaDadosModel
|
|
{
|
|
PRN = campos[i],
|
|
Elevacao = elevacao,
|
|
Azimute = azimute,
|
|
QualidadeSinal = snr
|
|
}
|
|
);
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNTHS(string sentenca)
|
|
{
|
|
string conteudo = sentenca.Trim();
|
|
|
|
if (conteudo.StartsWith("$"))
|
|
conteudo = conteudo.Substring(1);
|
|
|
|
string[] partesChecksum = conteudo.Split(
|
|
new[] { '*' },
|
|
2
|
|
);
|
|
|
|
string[] campos = partesChecksum[0].Split(',');
|
|
|
|
if (campos.Length < 3)
|
|
throw new FormatException("THS incompleta.");
|
|
|
|
string headingTexto =
|
|
(campos[1] ?? string.Empty).Trim();
|
|
|
|
string status =
|
|
(campos[2] ?? string.Empty)
|
|
.Trim()
|
|
.ToUpperInvariant();
|
|
|
|
double headingTrue;
|
|
|
|
bool numerico = double.TryParse(
|
|
headingTexto,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out headingTrue
|
|
);
|
|
|
|
bool valido =
|
|
status == "A" &&
|
|
numerico &&
|
|
!double.IsNaN(headingTrue) &&
|
|
!double.IsInfinity(headingTrue);
|
|
|
|
PenultimaLeitura.TimestampOri.valor =
|
|
UltimaLeitura.TimestampOri.valor;
|
|
|
|
PenultimaLeitura.Momento =
|
|
UltimaLeitura.Momento;
|
|
|
|
PenultimaLeitura.OrientacaoReal =
|
|
UltimaLeitura.OrientacaoReal;
|
|
|
|
PenultimaLeitura.TipoOrientacao =
|
|
UltimaLeitura.TipoOrientacao;
|
|
|
|
UltimaLeitura.TimestampOri.valor =
|
|
Stopwatch.GetTimestamp() /
|
|
(double)Stopwatch.Frequency;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
if (!valido)
|
|
{
|
|
UltimaLeitura.OrientacaoReal = 9999;
|
|
UltimaLeitura.TipoOrientacao = "V";
|
|
return;
|
|
}
|
|
|
|
headingTrue = GPSUtils.NormalizarAngulo(headingTrue);
|
|
|
|
UltimaLeitura.OrientacaoReal =
|
|
InverterHeading
|
|
? GPSUtils.NormalizarAngulo(
|
|
headingTrue - 180.0
|
|
)
|
|
: headingTrue;
|
|
|
|
UltimaLeitura.TipoOrientacao = "A";
|
|
|
|
AplicarPosicaoCorrigida();
|
|
DefinirAnguloCarro();
|
|
}
|
|
|
|
private static void ProcessarGxGSA(string sentenca)
|
|
{
|
|
string[] campos = sentenca.Split(',');
|
|
|
|
if (campos.Length < 17)
|
|
return;
|
|
|
|
TiposDimensaoCorrecaoGPS modoSolucao =
|
|
(TiposDimensaoCorrecaoGPS)
|
|
GPSUtils.ParseInt(campos[2]);
|
|
|
|
int satsUsados = 0;
|
|
|
|
for (int i = 3; i <= 14 && i < campos.Length; i++)
|
|
{
|
|
if (!string.IsNullOrWhiteSpace(campos[i]))
|
|
satsUsados++;
|
|
}
|
|
|
|
double pdop = GPSUtils.ParseDouble(campos[15]);
|
|
double hdop = GPSUtils.ParseDouble(campos[16]);
|
|
|
|
double vdop = campos.Length > 17
|
|
? GPSUtils.ParseDouble(
|
|
StripChecksum(campos[17])
|
|
)
|
|
: double.NaN;
|
|
|
|
if (!double.IsInfinity(hdop) &&
|
|
!double.IsNaN(hdop))
|
|
{
|
|
UltimaLeitura.PrecisaoHorizontal = hdop;
|
|
}
|
|
|
|
if (satsUsados > 0)
|
|
{
|
|
UltimaLeitura.NumeroSatelites =
|
|
Math.Max(
|
|
UltimaLeitura.NumeroSatelites,
|
|
satsUsados
|
|
);
|
|
}
|
|
|
|
if (!double.IsInfinity(pdop) &&
|
|
!double.IsNaN(pdop))
|
|
{
|
|
UltimaLeitura.PDOP = pdop;
|
|
}
|
|
|
|
if (!double.IsInfinity(vdop) &&
|
|
!double.IsNaN(vdop))
|
|
{
|
|
UltimaLeitura.VDOP = vdop;
|
|
}
|
|
|
|
UltimaLeitura.FixDimensao = modoSolucao;
|
|
}
|
|
|
|
private static void ProcessarGxRMC(string sentenca)
|
|
{
|
|
string[] campos = sentenca.Split(',');
|
|
|
|
if (campos.Length < 10)
|
|
throw new FormatException("RMC incompleta.");
|
|
|
|
string timeUtc = GetField(campos, 1);
|
|
string status = GetField(campos, 2);
|
|
string latRaw = GetField(campos, 3);
|
|
string latHem = GetField(campos, 4);
|
|
string lonRaw = GetField(campos, 5);
|
|
string lonHem = GetField(campos, 6);
|
|
string spdKnotsRaw = GetField(campos, 7);
|
|
string cogRaw = GetField(campos, 8);
|
|
string dateRaw = GetField(campos, 9);
|
|
string magRaw = GetField(campos, 10);
|
|
string magHem = StripChecksum(GetField(campos, 11));
|
|
|
|
string mode = campos.Length > 12
|
|
? StripChecksum(campos[12]).Trim()
|
|
: string.Empty;
|
|
|
|
bool valido =
|
|
string.Equals(
|
|
status,
|
|
"A",
|
|
StringComparison.OrdinalIgnoreCase
|
|
);
|
|
|
|
double lat;
|
|
double lon;
|
|
|
|
if (valido &&
|
|
TryParseDmm(latRaw, latHem, 2, out lat) &&
|
|
TryParseDmm(lonRaw, lonHem, 3, out lon))
|
|
{
|
|
PenultimaLeitura.Latitude =
|
|
UltimaLeitura.Latitude;
|
|
|
|
PenultimaLeitura.Longitude =
|
|
UltimaLeitura.Longitude;
|
|
|
|
PenultimaLeitura.LatitudeAnt =
|
|
UltimaLeitura.LatitudeAnt;
|
|
|
|
PenultimaLeitura.LongitudeAnt =
|
|
UltimaLeitura.LongitudeAnt;
|
|
|
|
UltimaLeitura.LatitudeAnt = lat;
|
|
UltimaLeitura.LongitudeAnt = lon;
|
|
|
|
AplicarPosicaoCorrigida();
|
|
}
|
|
|
|
double spdKnots;
|
|
|
|
if (double.TryParse(
|
|
spdKnotsRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out spdKnots))
|
|
{
|
|
/*
|
|
* Mantém a semântica histórica do projeto:
|
|
* o campo recebe o valor da sentença em knots.
|
|
*/
|
|
UltimaLeitura.Velocidade = spdKnots;
|
|
}
|
|
|
|
double cog;
|
|
|
|
if (double.TryParse(
|
|
cogRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out cog))
|
|
{
|
|
UltimaLeitura.CursoVerdadeiro = cog;
|
|
}
|
|
|
|
DateTime dataHora;
|
|
|
|
if (TryParseRmcDateTime(
|
|
dateRaw,
|
|
timeUtc,
|
|
out dataHora))
|
|
{
|
|
UltimaLeitura.DataHora =
|
|
dataHora.ToLocalTime();
|
|
}
|
|
|
|
double mag;
|
|
|
|
if (double.TryParse(
|
|
magRaw,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out mag))
|
|
{
|
|
if (string.Equals(
|
|
magHem,
|
|
"W",
|
|
StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
mag = -mag;
|
|
}
|
|
|
|
UltimaLeitura.VariacaoMagnetica = mag;
|
|
}
|
|
|
|
AplicarModoRmc(mode);
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
|
|
private static void AplicarModoRmc(string mode)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(mode))
|
|
return;
|
|
|
|
switch (mode.Trim().ToUpperInvariant())
|
|
{
|
|
case "R":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.RTKFixo;
|
|
break;
|
|
|
|
case "F":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.RTKFlutuante;
|
|
break;
|
|
|
|
case "D":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.DGPS;
|
|
break;
|
|
|
|
case "E":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.DeadReckoing;
|
|
break;
|
|
|
|
case "A":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.Autonomo;
|
|
break;
|
|
|
|
case "N":
|
|
UltimaLeitura.QualidadeFix =
|
|
TiposCorrecaoGPS.SemCorrecao;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// RTCM VIA MQTT / FILA DE ESCRITA
|
|
// ============================================================
|
|
|
|
public static void AplicarCorrecaoRTK_Mqtt(byte[] correcao, int bytesRead)
|
|
{
|
|
/*
|
|
* Uma solução RTK não pode receber correções simultâneas de duas
|
|
* bases/referências diferentes. Se NTRIP estiver efetivamente
|
|
* conectado, ele é a fonte ativa e RTCM via MQTT é ignorado.
|
|
*
|
|
* Se o NTRIP cair, Ntrip_Conectado volta a false e o MQTT pode
|
|
* assumir novamente sem exigir reinicialização do serviço.
|
|
*/
|
|
if (Ntrip_Conectado)
|
|
return;
|
|
|
|
if (correcao == null || bytesRead <= 0 || bytesRead > correcao.Length || bytesRead > RtcmMaxPayloadBytes)
|
|
{
|
|
Interlocked.Increment(ref _rtcmDroppedInvalid);
|
|
return;
|
|
}
|
|
|
|
byte[] copia = new byte[bytesRead];
|
|
Buffer.BlockCopy(correcao, 0, copia, 0, bytesRead);
|
|
|
|
EnfileirarRtcm(copia, RtcmSource.Mqtt);
|
|
}
|
|
|
|
private static void EnfileirarRtcm(byte[] bytes, RtcmSource source)
|
|
{
|
|
EnsureBackgroundWorkersStarted();
|
|
|
|
long agora = Stopwatch.GetTimestamp();
|
|
|
|
Interlocked.Increment(ref _rtcmReceived);
|
|
Interlocked.Exchange(ref _lastRtcmReceivedMono, agora);
|
|
|
|
var envelope = new RtcmEnvelope
|
|
{
|
|
Bytes = bytes,
|
|
ReceivedMonotonic = agora,
|
|
ReceivedUtc = DateTime.UtcNow,
|
|
Sequence = Interlocked.Increment(
|
|
ref _rtcmSequence
|
|
),
|
|
Source = source,
|
|
MessageType = TryGetRtcmMessageType(bytes)
|
|
};
|
|
|
|
lock (_rtcmQueueLock)
|
|
{
|
|
while (_rtcmQueue.Count >= RtcmQueueCapacity)
|
|
{
|
|
_rtcmQueue.Dequeue();
|
|
Interlocked.Increment(
|
|
ref _rtcmDroppedQueue
|
|
);
|
|
}
|
|
|
|
_rtcmQueue.Enqueue(envelope);
|
|
Interlocked.Increment(ref _rtcmQueued);
|
|
}
|
|
|
|
try { _rtcmSignal.Release(); }
|
|
catch (SemaphoreFullException) { }
|
|
}
|
|
|
|
private static async Task RtcmWriterLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await _rtcmSignal.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
|
|
while (true)
|
|
{
|
|
RtcmEnvelope envelope = null;
|
|
|
|
lock (_rtcmQueueLock)
|
|
{
|
|
if (_rtcmQueue.Count > 0)
|
|
envelope = _rtcmQueue.Dequeue();
|
|
}
|
|
|
|
if (envelope == null)
|
|
break;
|
|
|
|
double idadeMs = GetAgeMs(
|
|
envelope.ReceivedMonotonic
|
|
);
|
|
|
|
if (idadeMs > RtcmMaxAgeMs)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _rtcmDroppedStale
|
|
);
|
|
continue;
|
|
}
|
|
|
|
if (!Iniciado)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _rtcmDroppedQueue
|
|
);
|
|
continue;
|
|
}
|
|
|
|
try
|
|
{
|
|
await _serialWriteLock
|
|
.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
/*
|
|
* Verifica novamente depois de esperar o lock.
|
|
* A configuração pode ter ocupado a serial.
|
|
*/
|
|
idadeMs = GetAgeMs(
|
|
envelope.ReceivedMonotonic
|
|
);
|
|
|
|
if (idadeMs > RtcmMaxAgeMs)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _rtcmDroppedStale
|
|
);
|
|
continue;
|
|
}
|
|
|
|
WriteSerialUnsafe(
|
|
envelope.Bytes,
|
|
envelope.Bytes.Length
|
|
);
|
|
|
|
UltimoEnvioCorrecaoRTK =
|
|
DateTime.Now;
|
|
|
|
Interlocked.Exchange(
|
|
ref _lastRtcmWrittenMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
|
|
Interlocked.Increment(
|
|
ref _rtcmWritten
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _rtcmWriteErrors
|
|
);
|
|
|
|
RecordError(
|
|
"[GPSService.RtcmWriter] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private static int TryGetRtcmMessageType(byte[] bytes)
|
|
{
|
|
/*
|
|
* RTCM3:
|
|
* D3 | 6 bits reservados + 10 bits length | payload
|
|
* Os 12 primeiros bits do payload formam o message type.
|
|
*/
|
|
if (bytes == null ||
|
|
bytes.Length < 5 ||
|
|
bytes[0] != 0xD3)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
return ((bytes[3] << 4) | (bytes[4] >> 4))
|
|
& 0x0FFF;
|
|
}
|
|
|
|
// ============================================================
|
|
// NTRIP: FAILOVER + AUTOSELEÇÃO DE MOUNTPOINT
|
|
// ============================================================
|
|
|
|
public static async Task IniciarNtripAsync()
|
|
{
|
|
if (!Variaveis.IsAgroBase || !CorrecaoRTK_Ntrip)
|
|
return;
|
|
|
|
lock (_workerLock)
|
|
{
|
|
if (_ntripTask != null &&
|
|
!_ntripTask.IsCompleted)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_ntripCts = new CancellationTokenSource();
|
|
|
|
_ntripTask = Task.Run(
|
|
() => AplicarCorrecaoRTK_Ntrip(
|
|
_ntripCts.Token
|
|
),
|
|
_ntripCts.Token
|
|
);
|
|
}
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
public static async Task PararNtripAsync()
|
|
{
|
|
CancellationTokenSource cts;
|
|
Task task;
|
|
TcpClient client;
|
|
|
|
lock (_workerLock)
|
|
{
|
|
cts = _ntripCts;
|
|
task = _ntripTask;
|
|
client = _ntripClient;
|
|
|
|
_ntripCts = null;
|
|
_ntripTask = null;
|
|
_ntripClient = null;
|
|
}
|
|
|
|
CorrecaoRTK_Ntrip = false;
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
|
|
if (cts != null)
|
|
{
|
|
try { cts.Cancel(); }
|
|
catch { }
|
|
}
|
|
|
|
if (client != null)
|
|
{
|
|
try { client.Close(); }
|
|
catch { }
|
|
|
|
try { client.Dispose(); }
|
|
catch { }
|
|
}
|
|
|
|
if (task != null)
|
|
{
|
|
try
|
|
{
|
|
await task.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.PararNtripAsync] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
if (cts != null)
|
|
cts.Dispose();
|
|
}
|
|
|
|
private static void LimparNtripAtivo()
|
|
{
|
|
NtripProviderAtivo = string.Empty;
|
|
NtripMountpointAtivo = string.Empty;
|
|
NtripDistanciaMountpointAtivoKm = null;
|
|
}
|
|
|
|
private static List<NtripProviderConfig>
|
|
ObterSequenciaProvedoresNtrip()
|
|
{
|
|
var ibge = new NtripProviderConfig
|
|
{
|
|
Nome = "IBGE",
|
|
Host = NtripHost,
|
|
Port = NtripPort,
|
|
MountpointManual = NtripMountpoint,
|
|
Username = NtripUsername,
|
|
Password = NtripPassword,
|
|
UsarNtripV2 = true,
|
|
Comunitario = false,
|
|
ExigeUsuario = true,
|
|
ExigeSenha = true,
|
|
FiltroPais = "BRA",
|
|
PermitirFallbackManualSeAutoFalhar = true
|
|
};
|
|
|
|
var rtk2go = new NtripProviderConfig
|
|
{
|
|
Nome = "RTK2go",
|
|
Host = NtripRtk2goHost,
|
|
Port = NtripRtk2goPort,
|
|
MountpointManual = NtripRtk2goMountpoint,
|
|
Username = NtripRtk2goUsername,
|
|
Password = NtripRtk2goPassword,
|
|
UsarNtripV2 = true,
|
|
Comunitario = true,
|
|
ExigeUsuario = true,
|
|
ExigeSenha = false,
|
|
FiltroPais = "BRA",
|
|
|
|
// Em modo automático nunca usamos SPLI1 silenciosamente
|
|
// caso a descoberta do RTK2go falhe.
|
|
PermitirFallbackManualSeAutoFalhar = false
|
|
};
|
|
|
|
List<NtripProviderConfig> providers;
|
|
|
|
switch (NtripMode)
|
|
{
|
|
case NtripProviderMode.Rtk2goOnly:
|
|
providers =
|
|
new List<NtripProviderConfig> { rtk2go };
|
|
break;
|
|
|
|
case NtripProviderMode.IbgeThenRtk2go:
|
|
providers =
|
|
new List<NtripProviderConfig>
|
|
{
|
|
ibge,
|
|
rtk2go
|
|
};
|
|
break;
|
|
|
|
default:
|
|
providers =
|
|
new List<NtripProviderConfig> { ibge };
|
|
break;
|
|
}
|
|
|
|
var validos = new List<NtripProviderConfig>();
|
|
|
|
foreach (NtripProviderConfig provider in providers)
|
|
{
|
|
if (!ConfiguracaoNtripEstruturalValida(provider))
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
(provider != null ? 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."
|
|
: "Defina AGRO_NTRIP_USERNAME.";
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"] Usuário não configurado. " +
|
|
dica
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
if (provider.ExigeSenha &&
|
|
string.IsNullOrWhiteSpace(provider.Password))
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"] Senha não configurada. " +
|
|
"Defina AGRO_NTRIP_PASSWORD."
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
validos.Add(provider);
|
|
}
|
|
|
|
return validos;
|
|
}
|
|
|
|
private static bool ConfiguracaoNtripEstruturalValida(
|
|
NtripProviderConfig provider)
|
|
{
|
|
return
|
|
provider != null &&
|
|
!string.IsNullOrWhiteSpace(provider.Host) &&
|
|
provider.Port > 0;
|
|
}
|
|
|
|
private static async Task AplicarCorrecaoRTK_Ntrip(
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (!Iniciado || LoopRTK_Ntrip)
|
|
return;
|
|
|
|
LoopRTK_Ntrip = true;
|
|
|
|
double backoffSeconds = 1.0;
|
|
|
|
try
|
|
{
|
|
while (
|
|
!cancellationToken.IsCancellationRequested &&
|
|
CorrecaoRTK_Ntrip)
|
|
{
|
|
/*
|
|
* Diferente da versão antiga, não encerramos o worker
|
|
* apenas porque a internet caiu momentaneamente.
|
|
* Assim o rover se recupera sozinho quando o link volta.
|
|
*/
|
|
if (!APIService.HasInternet)
|
|
{
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
|
|
await Task.Delay(
|
|
TimeSpan.FromSeconds(2),
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
continue;
|
|
}
|
|
|
|
List<NtripProviderConfig> providers =
|
|
ObterSequenciaProvedoresNtrip();
|
|
|
|
if (providers.Count == 0)
|
|
{
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP] " +
|
|
"Nenhum provedor possui configuração válida."
|
|
);
|
|
|
|
await Task.Delay(
|
|
TimeSpan.FromSeconds(5),
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
continue;
|
|
}
|
|
|
|
foreach (NtripProviderConfig provider in providers)
|
|
{
|
|
if (
|
|
cancellationToken.IsCancellationRequested ||
|
|
!CorrecaoRTK_Ntrip)
|
|
{
|
|
break;
|
|
}
|
|
|
|
List<NtripCandidate> candidatos;
|
|
|
|
try
|
|
{
|
|
candidatos =
|
|
await ObterCandidatosNtripAsync(
|
|
provider,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
when (cancellationToken.IsCancellationRequested)
|
|
{
|
|
return;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _ntripErrors
|
|
);
|
|
|
|
RecordError(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Falha preparando candidatos: " +
|
|
ex.Message
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
if (candidatos.Count == 0)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"] Nenhum mountpoint elegível."
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
for (int i = 0;
|
|
i < candidatos.Count;
|
|
i++)
|
|
{
|
|
if (
|
|
cancellationToken.IsCancellationRequested ||
|
|
!CorrecaoRTK_Ntrip)
|
|
{
|
|
break;
|
|
}
|
|
|
|
NtripCandidate candidato =
|
|
candidatos[i];
|
|
|
|
TimeSpan cooldownRestante;
|
|
|
|
if (TryObterCooldownRestante(
|
|
provider.Nome,
|
|
candidato.Mountpoint,
|
|
out cooldownRestante))
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][COOLDOWN] " +
|
|
provider.Nome +
|
|
"/" +
|
|
candidato.Mountpoint +
|
|
" ignorado por mais " +
|
|
Math.Ceiling(
|
|
cooldownRestante.TotalSeconds
|
|
).ToString("F0",
|
|
CultureInfo.InvariantCulture) +
|
|
"s."
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
try
|
|
{
|
|
string distanciaTexto =
|
|
candidato.DistanciaKm.HasValue
|
|
? " | " +
|
|
candidato
|
|
.DistanciaKm
|
|
.Value
|
|
.ToString(
|
|
"F1",
|
|
CultureInfo
|
|
.InvariantCulture
|
|
) +
|
|
" km"
|
|
: string.Empty;
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"] Tentando candidato " +
|
|
(i + 1) +
|
|
"/" +
|
|
candidatos.Count +
|
|
": " +
|
|
candidato.Mountpoint +
|
|
distanciaTexto +
|
|
"..."
|
|
);
|
|
|
|
await ExecutarSessaoNtripAsync(
|
|
provider,
|
|
candidato,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
/*
|
|
* Retorno normal só ocorre quando a sessão foi
|
|
* encerrada por cancelamento/desativação.
|
|
*/
|
|
}
|
|
catch (OperationCanceledException)
|
|
when (cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
return;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
|
|
Interlocked.Increment(
|
|
ref _ntripErrors
|
|
);
|
|
|
|
RecordError(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"/" +
|
|
candidato.Mountpoint +
|
|
"] Falha: " +
|
|
ex.Message
|
|
);
|
|
|
|
MarcarMountpointEmCooldown(
|
|
provider.Nome,
|
|
candidato.Mountpoint,
|
|
ex
|
|
);
|
|
|
|
if (i + 1 < candidatos.Count)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][FALLBACK] " +
|
|
"Tentando próximo mountpoint de " +
|
|
provider.Nome +
|
|
": " +
|
|
candidatos[i + 1].Mountpoint +
|
|
"."
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (
|
|
!Ntrip_Conectado &&
|
|
!cancellationToken.IsCancellationRequested &&
|
|
CorrecaoRTK_Ntrip)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][FALLBACK] " +
|
|
"Candidatos de " +
|
|
provider.Nome +
|
|
" esgotados."
|
|
);
|
|
}
|
|
}
|
|
|
|
if (
|
|
cancellationToken.IsCancellationRequested ||
|
|
!CorrecaoRTK_Ntrip)
|
|
{
|
|
break;
|
|
}
|
|
|
|
Interlocked.Increment(
|
|
ref _ntripReconnects
|
|
);
|
|
|
|
double jitter =
|
|
((uint)Environment.TickCount % 500) /
|
|
1000.0;
|
|
|
|
await Task.Delay(
|
|
TimeSpan.FromSeconds(
|
|
backoffSeconds + jitter
|
|
),
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
backoffSeconds =
|
|
Math.Min(
|
|
20.0,
|
|
backoffSeconds * 1.8
|
|
);
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
when (cancellationToken.IsCancellationRequested)
|
|
{
|
|
}
|
|
finally
|
|
{
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
LoopRTK_Ntrip = false;
|
|
}
|
|
}
|
|
|
|
private static async Task<List<NtripCandidate>>
|
|
ObterCandidatosNtripAsync(
|
|
NtripProviderConfig provider,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (!NtripAutoSelecionarMountpoint)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(
|
|
provider.MountpointManual))
|
|
{
|
|
return new List<NtripCandidate>();
|
|
}
|
|
|
|
return new List<NtripCandidate>
|
|
{
|
|
new NtripCandidate
|
|
{
|
|
Mountpoint =
|
|
provider.MountpointManual.Trim(),
|
|
DistanciaKm = null,
|
|
Formato = string.Empty,
|
|
SistemasGnss = string.Empty,
|
|
OrigemAuto = false
|
|
}
|
|
};
|
|
}
|
|
|
|
NtripApproxPosition pos =
|
|
await AguardarPosicaoParaSelecaoNtripAsync(
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
if (pos == null)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Sem posição GGA recente " +
|
|
"para ordenar estações."
|
|
);
|
|
|
|
return CriarFallbackManualSePermitido(
|
|
provider
|
|
);
|
|
}
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Posição aproximada: lat=" +
|
|
pos.Latitude.ToString(
|
|
"F6",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
", lon=" +
|
|
pos.Longitude.ToString(
|
|
"F6",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
"."
|
|
);
|
|
|
|
List<NtripMountpointInfo> streams;
|
|
|
|
try
|
|
{
|
|
streams =
|
|
await ObterSourcetableAsync(
|
|
provider,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Sourcetable indisponível: " +
|
|
ex.Message
|
|
);
|
|
|
|
return CriarFallbackManualSePermitido(
|
|
provider
|
|
);
|
|
}
|
|
|
|
double maxKm =
|
|
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
|
|
)
|
|
)
|
|
/*
|
|
* IBGE e os mountpoints físicos do RTK2go usados
|
|
* atualmente não exigem GGA para o caster.
|
|
* Streams VRS/Network que exijam GGA ficam de fora
|
|
* até implementarmos o envio periódico ao caster.
|
|
*/
|
|
.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)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.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)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Estação mais próxima é " +
|
|
maisProxima.Mountpoint +
|
|
" a " +
|
|
maisProxima.DistanciaKm.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
" km, acima do limite de " +
|
|
maxKm.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
" km."
|
|
);
|
|
|
|
/*
|
|
* Em auto-seleção não furamos o limite com um
|
|
* mountpoint manual silencioso.
|
|
*/
|
|
return new List<NtripCandidate>();
|
|
}
|
|
|
|
int maxCandidates =
|
|
Math.Max(
|
|
1,
|
|
NtripMaxCandidatosPorProvedor
|
|
);
|
|
|
|
List<NtripMountpointInfo> dentroDoLimite =
|
|
ranqueados
|
|
.Where(
|
|
x => x.DistanciaKm <= maxKm
|
|
)
|
|
.ToList();
|
|
|
|
var elegiveis =
|
|
new List<NtripMountpointInfo>(
|
|
maxCandidates
|
|
);
|
|
|
|
foreach (
|
|
NtripMountpointInfo item
|
|
in dentroDoLimite)
|
|
{
|
|
TimeSpan restante;
|
|
|
|
if (TryObterCooldownRestante(
|
|
provider.Nome,
|
|
item.Mountpoint,
|
|
out restante))
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO][COOLDOWN] " +
|
|
item.Mountpoint +
|
|
" (" +
|
|
item.DistanciaKm.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
" km) ignorado por mais " +
|
|
Math.Ceiling(
|
|
restante.TotalSeconds
|
|
).ToString(
|
|
"F0",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
"s."
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
elegiveis.Add(item);
|
|
|
|
if (elegiveis.Count >= maxCandidates)
|
|
break;
|
|
}
|
|
|
|
if (elegiveis.Count == 0)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Todas as estações dentro de " +
|
|
maxKm.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
" km estão temporariamente em cooldown."
|
|
);
|
|
|
|
return new List<NtripCandidate>();
|
|
}
|
|
|
|
string ranking =
|
|
string.Join(
|
|
" | ",
|
|
elegiveis.Select(
|
|
(x, i) =>
|
|
(i + 1) +
|
|
"." +
|
|
x.Mountpoint +
|
|
" " +
|
|
x.DistanciaKm.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
"km"
|
|
)
|
|
);
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.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 ?? string.Empty).Trim() +
|
|
"::" +
|
|
(mountpoint ?? string.Empty).Trim();
|
|
}
|
|
|
|
private static 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)
|
|
{
|
|
DateTime ateUtc;
|
|
|
|
if (!_ntripCooldownAteUtc.TryGetValue(
|
|
chave,
|
|
out ateUtc))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
DateTime agora =
|
|
DateTime.UtcNow;
|
|
|
|
if (ateUtc <= agora)
|
|
{
|
|
_ntripCooldownAteUtc.Remove(
|
|
chave
|
|
);
|
|
|
|
return false;
|
|
}
|
|
|
|
restante =
|
|
ateUtc - agora;
|
|
|
|
return true;
|
|
}
|
|
}
|
|
|
|
private static 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;
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][COOLDOWN] " +
|
|
providerNome +
|
|
"/" +
|
|
mountpoint +
|
|
" suspenso por " +
|
|
segundos +
|
|
"s" +
|
|
motivoTexto +
|
|
"."
|
|
);
|
|
}
|
|
|
|
private static List<NtripCandidate>
|
|
CriarFallbackManualSePermitido(
|
|
NtripProviderConfig provider)
|
|
{
|
|
if (
|
|
!provider
|
|
.PermitirFallbackManualSeAutoFalhar ||
|
|
string.IsNullOrWhiteSpace(
|
|
provider.MountpointManual))
|
|
{
|
|
return new List<NtripCandidate>();
|
|
}
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"][AUTO] Usando mountpoint manual " +
|
|
provider.MountpointManual +
|
|
" apenas como fallback de descoberta."
|
|
);
|
|
|
|
return new List<NtripCandidate>
|
|
{
|
|
new NtripCandidate
|
|
{
|
|
Mountpoint =
|
|
provider.MountpointManual.Trim(),
|
|
DistanciaKm = null,
|
|
Formato = string.Empty,
|
|
SistemasGnss = string.Empty,
|
|
OrigemAuto = false
|
|
}
|
|
};
|
|
}
|
|
|
|
private static async Task<NtripApproxPosition>
|
|
AguardarPosicaoParaSelecaoNtripAsync(
|
|
CancellationToken cancellationToken)
|
|
{
|
|
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;
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][AUTO] " +
|
|
"Aguardando posição GGA válida por até " +
|
|
esperaSegundos +
|
|
"s antes de selecionar mountpoints."
|
|
);
|
|
|
|
while (
|
|
!cancellationToken.IsCancellationRequested &&
|
|
DateTime.UtcNow <= deadlineUtc)
|
|
{
|
|
NtripApproxPosition pos =
|
|
TryGetApproxPositionForNtrip();
|
|
|
|
if (pos != null)
|
|
{
|
|
double decorrido =
|
|
Math.Max(
|
|
0,
|
|
(
|
|
DateTime.UtcNow -
|
|
inicioUtc
|
|
).TotalSeconds
|
|
);
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][AUTO] " +
|
|
"Posição GGA disponível após " +
|
|
decorrido.ToString(
|
|
"F1",
|
|
CultureInfo.InvariantCulture
|
|
) +
|
|
"s."
|
|
);
|
|
|
|
return pos;
|
|
}
|
|
|
|
if (DateTime.UtcNow >= proximoReforcoUtc)
|
|
{
|
|
string porta =
|
|
string.IsNullOrWhiteSpace(
|
|
NtripPortaGgaAutoSelecao
|
|
)
|
|
? "com3"
|
|
: NtripPortaGgaAutoSelecao.Trim();
|
|
|
|
/*
|
|
* Preserva a frequência configurada no rover.
|
|
* Não reduzimos o GGA de 5 Hz para 1 Hz só para
|
|
* realizar a descoberta NTRIP.
|
|
*/
|
|
string freq =
|
|
(
|
|
1.0 /
|
|
Math.Max(
|
|
1,
|
|
TaxaAmostragemHz
|
|
)
|
|
)
|
|
.ToString(
|
|
"0.0",
|
|
CultureInfo.InvariantCulture
|
|
);
|
|
|
|
try
|
|
{
|
|
await EnviarComandoSerialNtripAsync(
|
|
"gngga " +
|
|
porta +
|
|
" " +
|
|
freq +
|
|
"\r\n",
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
when (cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][AUTO] " +
|
|
"Não foi possível reforçar GGA em " +
|
|
porta +
|
|
": " +
|
|
ex.Message
|
|
);
|
|
}
|
|
|
|
proximoReforcoUtc =
|
|
DateTime.UtcNow.AddSeconds(3);
|
|
}
|
|
|
|
await Task.Delay(
|
|
200,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][AUTO] " +
|
|
"Nenhuma posição GGA válida recebida em " +
|
|
esperaSegundos +
|
|
"s. A descoberta automática usará o fallback seguro."
|
|
);
|
|
|
|
return null;
|
|
}
|
|
|
|
private static NtripApproxPosition
|
|
TryGetApproxPositionForNtrip()
|
|
{
|
|
double ggaAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastValidGgaMono
|
|
)
|
|
);
|
|
|
|
/*
|
|
* Para ordenar bases basta uma posição autônoma recente.
|
|
* Trinta segundos ainda é amplamente suficiente para
|
|
* diferenciar estações a quilômetros de distância.
|
|
*/
|
|
if (
|
|
double.IsInfinity(ggaAgeMs) ||
|
|
ggaAgeMs < 0 ||
|
|
ggaAgeMs > 30_000)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
lock (_stateLock)
|
|
{
|
|
double lat =
|
|
UltimaLeitura.Latitude;
|
|
|
|
double lon =
|
|
UltimaLeitura.Longitude;
|
|
|
|
if (!CoordenadaValida(lat, lon))
|
|
{
|
|
lat =
|
|
UltimaLeitura.LatitudeAnt;
|
|
|
|
lon =
|
|
UltimaLeitura.LongitudeAnt;
|
|
}
|
|
|
|
if (!CoordenadaValida(lat, lon))
|
|
return null;
|
|
|
|
return new NtripApproxPosition
|
|
{
|
|
Latitude = lat,
|
|
Longitude = lon
|
|
};
|
|
}
|
|
}
|
|
|
|
private static async Task
|
|
EnviarComandoSerialNtripAsync(
|
|
string comando,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (string.IsNullOrEmpty(comando))
|
|
return;
|
|
|
|
byte[] bytes =
|
|
Encoding.ASCII.GetBytes(
|
|
comando
|
|
);
|
|
|
|
await _serialWriteLock
|
|
.WaitAsync(
|
|
cancellationToken
|
|
)
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
WriteSerialUnsafe(
|
|
bytes,
|
|
bytes.Length
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
}
|
|
|
|
private static async Task<List<NtripMountpointInfo>>
|
|
ObterSourcetableAsync(
|
|
NtripProviderConfig provider,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
string cacheKey =
|
|
provider.Host +
|
|
":" +
|
|
provider.Port;
|
|
|
|
int cacheSeconds =
|
|
Math.Max(
|
|
30,
|
|
NtripSourcetableCacheSegundos
|
|
);
|
|
|
|
lock (_ntripSourcetableLock)
|
|
{
|
|
NtripSourcetableCacheEntry cached;
|
|
|
|
if (
|
|
_ntripSourcetableCache.TryGetValue(
|
|
cacheKey,
|
|
out cached
|
|
) &&
|
|
(
|
|
DateTime.UtcNow -
|
|
cached.ObtidaUtc
|
|
).TotalSeconds <= cacheSeconds)
|
|
{
|
|
return
|
|
cached
|
|
.Streams
|
|
.Select(
|
|
CloneMountpointInfo
|
|
)
|
|
.ToList();
|
|
}
|
|
}
|
|
|
|
List<NtripMountpointInfo> streams =
|
|
await BaixarSourcetableAsync(
|
|
provider,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
lock (_ntripSourcetableLock)
|
|
{
|
|
_ntripSourcetableCache[cacheKey] =
|
|
new NtripSourcetableCacheEntry
|
|
{
|
|
ObtidaUtc =
|
|
DateTime.UtcNow,
|
|
|
|
Streams =
|
|
streams
|
|
.Select(
|
|
CloneMountpointInfo
|
|
)
|
|
.ToList()
|
|
};
|
|
}
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.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 static async Task<List<NtripMountpointInfo>>
|
|
BaixarSourcetableAsync(
|
|
NtripProviderConfig provider,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
using (var client = new TcpClient())
|
|
{
|
|
await ConnectTcpWithTimeoutAsync(
|
|
client,
|
|
provider.Host,
|
|
provider.Port,
|
|
TimeSpan.FromSeconds(8),
|
|
cancellationToken
|
|
).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 AgroRover/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,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
await stream.FlushAsync(
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
Tuple<string, byte[]> response =
|
|
await ReadHttpHeaderAsync(
|
|
stream,
|
|
32 * 1024,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
string header =
|
|
response.Item1;
|
|
|
|
byte[] firstBodyBytes =
|
|
response.Item2;
|
|
|
|
string firstLine =
|
|
header
|
|
.Split(
|
|
new[]
|
|
{
|
|
"\r\n",
|
|
"\n"
|
|
},
|
|
StringSplitOptions.None
|
|
)
|
|
.FirstOrDefault() ??
|
|
string.Empty;
|
|
|
|
bool accepted =
|
|
firstLine.IndexOf(
|
|
"200 OK",
|
|
StringComparison
|
|
.OrdinalIgnoreCase
|
|
) >= 0 ||
|
|
firstLine.StartsWith(
|
|
"SOURCETABLE 200",
|
|
StringComparison
|
|
.OrdinalIgnoreCase
|
|
);
|
|
|
|
if (!accepted)
|
|
{
|
|
throw new IOException(
|
|
"sourcetable recusada: " +
|
|
firstLine
|
|
);
|
|
}
|
|
|
|
const int maxBodyBytes =
|
|
2 * 1024 * 1024;
|
|
|
|
var body =
|
|
new List<byte>(
|
|
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),
|
|
cancellationToken
|
|
).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]
|
|
);
|
|
}
|
|
|
|
byte[] bodyArray =
|
|
body.ToArray();
|
|
|
|
int tailSize =
|
|
Math.Min(
|
|
bodyArray.Length,
|
|
256
|
|
);
|
|
|
|
string tail =
|
|
Encoding.ASCII
|
|
.GetString(
|
|
bodyArray,
|
|
bodyArray.Length -
|
|
tailSize,
|
|
tailSize
|
|
);
|
|
|
|
if (tail.IndexOf(
|
|
"ENDSOURCETABLE",
|
|
StringComparison
|
|
.OrdinalIgnoreCase
|
|
) >= 0)
|
|
{
|
|
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)
|
|
{
|
|
var result =
|
|
new List<NtripMountpointInfo>();
|
|
|
|
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;
|
|
|
|
double lat;
|
|
double lon;
|
|
|
|
if (
|
|
!double.TryParse(
|
|
p[9],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out lat
|
|
) ||
|
|
!double.TryParse(
|
|
p[10],
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out 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 static async Task ExecutarSessaoNtripAsync(
|
|
NtripProviderConfig provider,
|
|
NtripCandidate candidato,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (provider == null)
|
|
throw new ArgumentNullException(
|
|
nameof(provider)
|
|
);
|
|
|
|
if (
|
|
candidato == null ||
|
|
string.IsNullOrWhiteSpace(
|
|
candidato.Mountpoint))
|
|
{
|
|
throw new ArgumentException(
|
|
"Candidato NTRIP inválido.",
|
|
nameof(candidato)
|
|
);
|
|
}
|
|
|
|
TcpClient client = null;
|
|
|
|
try
|
|
{
|
|
client =
|
|
new TcpClient();
|
|
|
|
lock (_workerLock)
|
|
{
|
|
_ntripClient =
|
|
client;
|
|
}
|
|
|
|
await ConnectTcpWithTimeoutAsync(
|
|
client,
|
|
provider.Host,
|
|
provider.Port,
|
|
TimeSpan.FromSeconds(8),
|
|
cancellationToken
|
|
).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 AgroRover/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 AgroRover/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,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
await stream.FlushAsync(
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
Tuple<string, byte[]> response =
|
|
await ReadHttpHeaderAsync(
|
|
stream,
|
|
16 * 1024,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
string header =
|
|
response.Item1;
|
|
|
|
byte[] firstBodyBytes =
|
|
response.Item2;
|
|
|
|
string firstLine =
|
|
header
|
|
.Split(
|
|
new[]
|
|
{
|
|
"\r\n",
|
|
"\n"
|
|
},
|
|
StringSplitOptions.None
|
|
)
|
|
.FirstOrDefault() ??
|
|
string.Empty;
|
|
|
|
bool accepted =
|
|
firstLine.IndexOf(
|
|
"200 OK",
|
|
StringComparison.OrdinalIgnoreCase
|
|
) >= 0 ||
|
|
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 stream binário não pode ser encaminhado ao UM982 " +
|
|
"sem dechunking contínuo."
|
|
);
|
|
}
|
|
|
|
/*
|
|
* Antes de assumir a nova referência NTRIP, removemos
|
|
* correções ainda pendentes da fonte anterior. Isso evita
|
|
* que alguns frames MQTT antigos sejam escritos no UM982
|
|
* logo antes dos primeiros frames do novo caster.
|
|
*/
|
|
lock (_rtcmQueueLock)
|
|
{
|
|
_rtcmQueue.Clear();
|
|
}
|
|
|
|
Ntrip_Conectado = true;
|
|
|
|
NtripProviderAtivo =
|
|
provider.Nome;
|
|
|
|
NtripMountpointAtivo =
|
|
candidato.Mountpoint;
|
|
|
|
NtripDistanciaMountpointAtivoKm =
|
|
candidato.DistanciaKm;
|
|
|
|
string distanciaTexto =
|
|
candidato.DistanciaKm.HasValue
|
|
? " | distância=" +
|
|
candidato
|
|
.DistanciaKm
|
|
.Value
|
|
.ToString(
|
|
"F1",
|
|
CultureInfo
|
|
.InvariantCulture
|
|
) +
|
|
" km"
|
|
: string.Empty;
|
|
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][" +
|
|
provider.Nome +
|
|
"] Conexão estabelecida em " +
|
|
provider.Host +
|
|
":" +
|
|
provider.Port +
|
|
"/" +
|
|
candidato.Mountpoint +
|
|
distanciaTexto +
|
|
"."
|
|
);
|
|
|
|
if (provider.Comunitario)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"[GPSService.NTRIP][RTK2go][AVISO] " +
|
|
"Fonte comunitária ativa. Distância não valida " +
|
|
"datum, coordenada publicada nem qualidade geodésica."
|
|
);
|
|
}
|
|
|
|
bool recebeuDados = false;
|
|
|
|
if (
|
|
firstBodyBytes != null &&
|
|
firstBodyBytes.Length > 0)
|
|
{
|
|
Interlocked.Add(
|
|
ref _ntripBytesReceived,
|
|
firstBodyBytes.Length
|
|
);
|
|
|
|
EnfileirarRtcm(
|
|
firstBodyBytes,
|
|
RtcmSource.Ntrip
|
|
);
|
|
|
|
recebeuDados = true;
|
|
}
|
|
|
|
byte[] buffer =
|
|
new byte[4096];
|
|
|
|
while (
|
|
!cancellationToken.IsCancellationRequested &&
|
|
CorrecaoRTK_Ntrip)
|
|
{
|
|
int bytesRead;
|
|
|
|
if (!recebeuDados)
|
|
{
|
|
bytesRead =
|
|
await ReadNetworkWithTimeoutAsync(
|
|
stream,
|
|
buffer,
|
|
TimeSpan.FromSeconds(
|
|
Math.Max(
|
|
2,
|
|
NtripPrimeirosDadosTimeoutSegundos
|
|
)
|
|
),
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
|
|
if (bytesRead <= 0)
|
|
{
|
|
throw new TimeoutException(
|
|
"sessão foi aceita, mas nenhum byte RTCM " +
|
|
"chegou no prazo."
|
|
);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
bytesRead =
|
|
await stream.ReadAsync(
|
|
buffer,
|
|
0,
|
|
buffer.Length,
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
if (bytesRead <= 0)
|
|
{
|
|
throw new IOException(
|
|
"caster encerrou a conexão."
|
|
);
|
|
}
|
|
|
|
byte[] bloco =
|
|
new byte[bytesRead];
|
|
|
|
Buffer.BlockCopy(
|
|
buffer,
|
|
0,
|
|
bloco,
|
|
0,
|
|
bytesRead
|
|
);
|
|
|
|
Interlocked.Add(
|
|
ref _ntripBytesReceived,
|
|
bytesRead
|
|
);
|
|
|
|
/*
|
|
* Preserva a arquitetura do rover:
|
|
* rede nunca escreve diretamente no UM982.
|
|
* NTRIP e MQTT passam pela mesma fila limitada
|
|
* e pelo mesmo writer serial.
|
|
*/
|
|
EnfileirarRtcm(
|
|
bloco,
|
|
RtcmSource.Ntrip
|
|
);
|
|
|
|
recebeuDados = true;
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Ntrip_Conectado = false;
|
|
LimparNtripAtivo();
|
|
|
|
if (client != null)
|
|
{
|
|
try { client.Close(); }
|
|
catch { }
|
|
|
|
try { client.Dispose(); }
|
|
catch { }
|
|
}
|
|
|
|
lock (_workerLock)
|
|
{
|
|
if (ReferenceEquals(
|
|
_ntripClient,
|
|
client))
|
|
{
|
|
_ntripClient = null;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private static async Task<int>
|
|
ReadNetworkWithTimeoutAsync(
|
|
NetworkStream stream,
|
|
byte[] buffer,
|
|
TimeSpan timeout,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
using (
|
|
CancellationTokenSource linked =
|
|
CancellationTokenSource
|
|
.CreateLinkedTokenSource(
|
|
cancellationToken
|
|
))
|
|
{
|
|
linked.CancelAfter(
|
|
timeout
|
|
);
|
|
|
|
try
|
|
{
|
|
return await stream.ReadAsync(
|
|
buffer,
|
|
0,
|
|
buffer.Length,
|
|
linked.Token
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
when (!cancellationToken
|
|
.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 new byte[0];
|
|
|
|
var output =
|
|
new List<byte>();
|
|
|
|
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
|
|
);
|
|
}
|
|
|
|
int size;
|
|
|
|
if (!int.TryParse(
|
|
sizeLine,
|
|
NumberStyles.HexNumber,
|
|
CultureInfo.InvariantCulture,
|
|
out 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 cancellationToken)
|
|
{
|
|
Task connectTask =
|
|
client.ConnectAsync(
|
|
host,
|
|
port
|
|
);
|
|
|
|
Task timeoutTask =
|
|
Task.Delay(
|
|
timeout,
|
|
cancellationToken
|
|
);
|
|
|
|
Task completed =
|
|
await Task.WhenAny(
|
|
connectTask,
|
|
timeoutTask
|
|
).ConfigureAwait(false);
|
|
|
|
if (completed != connectTask)
|
|
{
|
|
cancellationToken
|
|
.ThrowIfCancellationRequested();
|
|
|
|
throw new TimeoutException(
|
|
"Timeout conectando em " +
|
|
host +
|
|
":" +
|
|
port +
|
|
"."
|
|
);
|
|
}
|
|
|
|
await connectTask
|
|
.ConfigureAwait(false);
|
|
}
|
|
|
|
private static async Task<Tuple<string, byte[]>>
|
|
ReadHttpHeaderAsync(
|
|
NetworkStream stream,
|
|
int maxHeaderBytes,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
var data =
|
|
new List<byte>(1024);
|
|
|
|
byte[] temp =
|
|
new byte[512];
|
|
|
|
int headerEnd = -1;
|
|
int delimiterLength = 0;
|
|
|
|
while (headerEnd < 0)
|
|
{
|
|
int read =
|
|
await stream.ReadAsync(
|
|
temp,
|
|
0,
|
|
temp.Length,
|
|
cancellationToken
|
|
).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."
|
|
);
|
|
}
|
|
|
|
Tuple<int, int> headerInfo =
|
|
FindNtripHeaderEnd(
|
|
data
|
|
);
|
|
|
|
headerEnd =
|
|
headerInfo.Item1;
|
|
|
|
delimiterLength =
|
|
headerInfo.Item2;
|
|
}
|
|
|
|
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()
|
|
: new byte[0];
|
|
|
|
return Tuple.Create(
|
|
header,
|
|
body
|
|
);
|
|
}
|
|
|
|
private static Tuple<int, int>
|
|
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 Tuple.Create(
|
|
i,
|
|
4
|
|
);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* NTRIP Rev1 / SNIP pode responder apenas:
|
|
* ICY 200 OK\r\n
|
|
* seguido imediatamente do RTCM.
|
|
*/
|
|
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 Tuple.Create(
|
|
i,
|
|
2
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
return Tuple.Create(
|
|
-1,
|
|
0
|
|
);
|
|
}
|
|
|
|
// ============================================================
|
|
// SNAPSHOT E ESTADO GNSS
|
|
// ============================================================
|
|
|
|
public static GPSModel GetSnapshot()
|
|
{
|
|
lock (_stateLock)
|
|
return UltimaLeitura.Clone();
|
|
}
|
|
|
|
public static GPSModel GetPreviousSnapshot()
|
|
{
|
|
lock (_stateLock)
|
|
return PenultimaLeitura.Clone();
|
|
}
|
|
|
|
public static (GPSModel atual, GPSModel anterior) GetSnapshotPair()
|
|
{
|
|
lock (_stateLock)
|
|
{
|
|
return (
|
|
UltimaLeitura.Clone(),
|
|
PenultimaLeitura.Clone()
|
|
);
|
|
}
|
|
}
|
|
|
|
private static bool PossuiHeadingValido(GPSModel leitura)
|
|
{
|
|
if (leitura == null)
|
|
return false;
|
|
|
|
string status =
|
|
(leitura.TipoOrientacao ?? string.Empty)
|
|
.Trim()
|
|
.ToUpperInvariant();
|
|
|
|
double heading = leitura.OrientacaoReal;
|
|
|
|
return
|
|
status == "A" &&
|
|
leitura.TimestampOri.frequencia >= 1.0 &&
|
|
!double.IsNaN(heading) &&
|
|
!double.IsInfinity(heading) &&
|
|
heading >= 0.0 &&
|
|
heading < 360.0;
|
|
}
|
|
|
|
private static void AplicarPosicaoCorrigida()
|
|
{
|
|
if (!PossuiHeadingValido(UltimaLeitura))
|
|
{
|
|
UltimaLeitura.Latitude =
|
|
UltimaLeitura.LatitudeAnt;
|
|
|
|
UltimaLeitura.Longitude =
|
|
UltimaLeitura.LongitudeAnt;
|
|
|
|
return;
|
|
}
|
|
|
|
var corrigida =
|
|
LeverArm.FixLeverArmLatLon_Fast(
|
|
UltimaLeitura.LatitudeAnt,
|
|
UltimaLeitura.LongitudeAnt,
|
|
UltimaLeitura.OrientacaoReal,
|
|
UltimaLeitura.TimestampOri.frequencia
|
|
);
|
|
|
|
UltimaLeitura.Latitude = corrigida.lat;
|
|
UltimaLeitura.Longitude = corrigida.lon;
|
|
}
|
|
|
|
public static void AtualizarCoordenadasGPS()
|
|
{
|
|
if (!Variaveis.IsAgroBase)
|
|
return;
|
|
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
|
|
PenultimaLeitura.Ntrip_ativado = UltimaLeitura.Ntrip_ativado;
|
|
PenultimaLeitura.Heartbeat = UltimaLeitura.Heartbeat;
|
|
PenultimaLeitura.LeverArmFrontal = UltimaLeitura.LeverArmFrontal;
|
|
PenultimaLeitura.LeverArmLateral = UltimaLeitura.LeverArmLateral;
|
|
PenultimaLeitura.LeverArmFrontalOp = UltimaLeitura.LeverArmFrontalOp;
|
|
PenultimaLeitura.LeverArmLateralOp = UltimaLeitura.LeverArmLateralOp;
|
|
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
|
|
|
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
|
UltimaLeitura.LeverArmFrontal = LeverArm != null ? LeverArm.FrontalTotalCm : 0;
|
|
UltimaLeitura.LeverArmLateral = LeverArm != null ? LeverArm.LateralTotalCm : 0;
|
|
UltimaLeitura.LeverArmFrontalOp = LeverArm != null ? LeverArm.FrontalCampoCm : 0;
|
|
UltimaLeitura.LeverArmLateralOp = LeverArm != null ? LeverArm.LateralCampoCm : 0;
|
|
|
|
UltimasLeituras.Add(UltimaLeitura.Clone());
|
|
|
|
while (UltimasLeituras.Count > Math.Max(1, TaxaAmostragemHz))
|
|
{
|
|
UltimasLeituras.RemoveAt(0);
|
|
}
|
|
|
|
DefinirOrientacaoMovimento();
|
|
AtualizaDadosRedis();
|
|
|
|
if (op.Sensoriamento.Operacao.OperacaoIniciada)
|
|
{
|
|
var trajetoria = op.Trajetoria;
|
|
var gpsTrajetoria = op.GPSTrajetoria;
|
|
GPSModel novaLeitura = UltimaLeitura.Clone();
|
|
|
|
gpsTrajetoria.Add(novaLeitura);
|
|
|
|
if (gpsTrajetoria.Count > 1)
|
|
{
|
|
int ultimo = gpsTrajetoria.Count - 1;
|
|
int penultimo = ultimo - 1;
|
|
double distancia = GPSUtils.DistanciaEntrePontos(gpsTrajetoria[ultimo], gpsTrajetoria[penultimo]);
|
|
|
|
if (trajetoria != null)
|
|
trajetoria.AtualizarDistanciaPercorrida(distancia);
|
|
}
|
|
|
|
var bicos = op.DispAtu?.Dados?.BicosPulverizadores;
|
|
|
|
if (bicos != null)
|
|
{
|
|
foreach (var bico in bicos)
|
|
{
|
|
if (!bico.Inicializado || !bico.Comandar || !bico.ComandoAtuar)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
bico.AdicionarPontoTrechoAtivo(UltimaLeitura.Latitude, UltimaLeitura.Longitude);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (op.Trajetoria != null)
|
|
{
|
|
op.Trajetoria.LoopAtualizaDados();
|
|
}
|
|
|
|
if (PublicacaoMapaAutomaticaHabilitada)
|
|
{
|
|
AtualizarTrajetoriaDinamica();
|
|
|
|
EnviarCoordenadasParaMapa(
|
|
UltimaLeitura.Latitude,
|
|
UltimaLeitura.Longitude,
|
|
UltimaLeitura.AnguloCarroDefinido,
|
|
op.Sensoriamento.Operacao.OperacaoIniciada,
|
|
rover: true
|
|
);
|
|
|
|
GPSModel posicaoBase = VariaveisOperacao.ObterPosicaoBaseSnapshot();
|
|
|
|
if ((posicaoBase?.UltimoComandoRespondido ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60))
|
|
{
|
|
EnviarCoordenadasParaMapa(
|
|
posicaoBase.Latitude,
|
|
posicaoBase.Longitude,
|
|
posicaoBase.OrientacaoReal,
|
|
false,
|
|
rover: false
|
|
);
|
|
}
|
|
}
|
|
|
|
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
|
UltimaLeitura.Inicializado = Iniciado;
|
|
|
|
if (CorrecaoRTK_Ntrip && GetAgeMs(Interlocked.Read(ref _lastRtcmWrittenMono)) > TempoMin_Ntrip * 1000.0)
|
|
{
|
|
_ = IniciarNtripAsync();
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// PUBLICAÇÃO LOCAL LATEST-ONLY
|
|
// ============================================================
|
|
|
|
public static void EnviarCoordenadasParaMapa(double latitude, double longitude, double orientacao, bool emFoco, bool rover)
|
|
{
|
|
MapasModel mapa = Variaveis.OperacaoEmAndamento?.Mapa;
|
|
Panel painel = mapa?.pnlMapa;
|
|
|
|
// Produto headless: retorna antes de tocar no WebView.
|
|
if (painel == null || painel.IsDisposed || !painel.IsHandleCreated)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int id = rover
|
|
? MapasVariaveisModel.IDMarcadorRover
|
|
: MapasVariaveisModel.IDMarcadorBase;
|
|
|
|
ExecutarMapaNaUi(
|
|
mapa,
|
|
delegate
|
|
{
|
|
Task tarefa = mapa.AtualizarPosicaoMapaAsync(
|
|
id,
|
|
latitude,
|
|
longitude,
|
|
orientacao,
|
|
emFoco
|
|
);
|
|
|
|
ObservarFalhaMapa(
|
|
tarefa,
|
|
"GPSService.EnviarCoordenadasParaMapa"
|
|
);
|
|
}
|
|
);
|
|
}
|
|
|
|
public static void AtualizarTrajetoriaDinamica()
|
|
{
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
var trajetoria = op?.Trajetoria;
|
|
MapasModel mapa = op?.Mapa;
|
|
Panel painel = mapa?.pnlMapa;
|
|
|
|
// No produto sem UI, não cria cópia nem serializa a trajetória.
|
|
if (trajetoria == null || painel == null || painel.IsDisposed || !painel.IsHandleCreated)
|
|
{
|
|
return;
|
|
}
|
|
|
|
long anterior = Interlocked.Read(ref _ultimaTrajetoriaMapaMono);
|
|
|
|
if (GetAgeMs(anterior) < IntervaloTrajetoriaMapaMs)
|
|
return;
|
|
|
|
long agora = Stopwatch.GetTimestamp();
|
|
|
|
// Caso duas leituras cheguem juntas, somente uma publica.
|
|
if (Interlocked.CompareExchange(ref _ultimaTrajetoriaMapaMono, agora, anterior) != anterior)
|
|
{
|
|
return;
|
|
}
|
|
|
|
List<GPSModel> pontos = CriarTrajetoriaMapaVisual(trajetoria._TrajetoriaDinamica, MaxPontosTrajetoriaMapa);
|
|
|
|
if (pontos.Count == 0)
|
|
return;
|
|
|
|
ExecutarMapaNaUi(
|
|
mapa,
|
|
delegate
|
|
{
|
|
Task tarefa = mapa.DefinirTrajetoriaPlanejadaAsync(pontos);
|
|
|
|
ObservarFalhaMapa(
|
|
tarefa,
|
|
"GPSService.AtualizarTrajetoriaDinamica"
|
|
);
|
|
}
|
|
);
|
|
}
|
|
|
|
private static List<GPSModel> CriarTrajetoriaMapaVisual(IList<PontoTrajetoriaModel> origem, int limite)
|
|
{
|
|
var resultado = new List<GPSModel>();
|
|
|
|
if (origem == null || origem.Count == 0)
|
|
return resultado;
|
|
|
|
int count = origem.Count;
|
|
int passo = Math.Max(1, (int)Math.Ceiling(count / (double)Math.Max(2, limite)));
|
|
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
PontoTrajetoriaModel ponto = origem[i];
|
|
|
|
if (ponto?.Posicao == null)
|
|
continue;
|
|
|
|
bool estrutural =
|
|
ponto.PontoBorda ||
|
|
ponto.PontoLigacao ||
|
|
ponto.Tipo == TipoPontoRua.BordaEntrada ||
|
|
ponto.Tipo == TipoPontoRua.BordaSaida ||
|
|
ponto.Tipo == TipoPontoRua.LigacaoEntrada ||
|
|
ponto.Tipo == TipoPontoRua.LigacaoSaida;
|
|
|
|
if (i != 0 && i != count - 1 && !estrutural && i % passo != 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
resultado.Add(new GPSModel()
|
|
{
|
|
Latitude = ponto.Posicao.Latitude,
|
|
Longitude = ponto.Posicao.Longitude
|
|
});
|
|
}
|
|
|
|
return resultado;
|
|
}
|
|
|
|
private static void ExecutarMapaNaUi(MapasModel mapa, Action acao)
|
|
{
|
|
Panel painel = mapa?.pnlMapa;
|
|
|
|
if (acao == null || painel == null || painel.IsDisposed || !painel.IsHandleCreated)
|
|
{
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
if (painel.InvokeRequired)
|
|
{
|
|
painel.BeginInvoke(acao);
|
|
return;
|
|
}
|
|
|
|
acao();
|
|
}
|
|
catch (ObjectDisposedException)
|
|
{
|
|
}
|
|
catch (InvalidOperationException)
|
|
{
|
|
// A janela pode estar fechando entre as validações.
|
|
}
|
|
}
|
|
|
|
private static void ObservarFalhaMapa(Task tarefa, string origem)
|
|
{
|
|
if (tarefa == null)
|
|
return;
|
|
|
|
_ = tarefa.ContinueWith(
|
|
t =>
|
|
{
|
|
Exception erro = t.Exception?.GetBaseException();
|
|
RecordError(
|
|
"[" + origem + "] " +
|
|
(erro?.Message ?? "Falha desconhecida no mapa.")
|
|
);
|
|
},
|
|
CancellationToken.None,
|
|
TaskContinuationOptions.OnlyOnFaulted,
|
|
TaskScheduler.Default
|
|
);
|
|
}
|
|
|
|
private static async Task LocalPublisherLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await _localPublishSignal
|
|
.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
|
|
Dictionary<string, string> lote;
|
|
|
|
lock (_localPublishLock)
|
|
{
|
|
lote = new Dictionary<string, string>(
|
|
_localLatestPayload,
|
|
StringComparer.Ordinal
|
|
);
|
|
|
|
_localLatestPayload.Clear();
|
|
}
|
|
|
|
MqttService mqtt =
|
|
Variaveis.MqttServiceLocal;
|
|
|
|
if (mqtt == null ||
|
|
!mqtt.StatusConexao())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
foreach (var item in lote)
|
|
{
|
|
if (cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
return;
|
|
}
|
|
|
|
MqttService.MqttTopicosModel topico =
|
|
mqtt.Topicos.FirstOrDefault(
|
|
x => x.Topico == item.Key
|
|
);
|
|
|
|
if (topico == null)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
await mqtt.PublishAsync(
|
|
topico,
|
|
item.Value,
|
|
true
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.LocalPublisher] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// ORIENTAÇÃO, REDIS E HISTÓRICO
|
|
// ============================================================
|
|
|
|
private static void DefinirOrientacaoMovimento()
|
|
{
|
|
if (UltimasLeituras.Count < 2)
|
|
{
|
|
double ang = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
|
|
double d = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
|
|
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
|
|
|
UltimaLeitura.OrientacaoMovimento = ang;
|
|
UltimaLeitura.Distancia = d;
|
|
}
|
|
else
|
|
{
|
|
double sumX = 0;
|
|
double sumY = 0;
|
|
double distAcum = 0;
|
|
|
|
for (int i = 0; i < UltimasLeituras.Count - 1; i++)
|
|
{
|
|
GPSModel a = UltimasLeituras[i];
|
|
GPSModel b = UltimasLeituras[i + 1];
|
|
|
|
double angSeg = GPSUtils.CalcularOrientacao(a, b);
|
|
|
|
double dSeg = GPSUtils.DistanciaEntrePontos(a, b);
|
|
|
|
if (dSeg <= 0)
|
|
continue;
|
|
|
|
double rad = angSeg * Math.PI / 180.0;
|
|
|
|
sumX += Math.Cos(rad) * dSeg;
|
|
sumY += Math.Sin(rad) * dSeg;
|
|
distAcum += dSeg;
|
|
}
|
|
|
|
double anguloMovimento;
|
|
|
|
if (distAcum <= 0)
|
|
{
|
|
anguloMovimento = UltimaLeitura.OrientacaoReal;
|
|
}
|
|
else
|
|
{
|
|
anguloMovimento = Math.Atan2(sumY, sumX) * 180.0 / Math.PI;
|
|
anguloMovimento = GPSUtils.NormalizarAngulo(anguloMovimento);
|
|
}
|
|
|
|
GPSModel first = UltimasLeituras[0];
|
|
GPSModel last = UltimasLeituras[UltimasLeituras.Count - 1];
|
|
|
|
double distLinear = GPSUtils.DistanciaEntrePontos(first, last);
|
|
|
|
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
|
UltimaLeitura.OrientacaoMovimento = anguloMovimento;
|
|
UltimaLeitura.Distancia = distLinear;
|
|
}
|
|
|
|
DefinirAnguloCarro();
|
|
}
|
|
|
|
public static void DefinirAnguloCarro()
|
|
{
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
|
|
double anguloFinal;
|
|
|
|
bool imuIniciado = op.Sensoriamento?.IMU?.Iniciado ?? false;
|
|
|
|
bool gpsIniciado = Iniciado;
|
|
|
|
bool headingValido = PossuiHeadingValido(UltimaLeitura);
|
|
|
|
if (op.Simulando)
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoReal;
|
|
}
|
|
else if (gpsIniciado && headingValido)
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoReal;
|
|
}
|
|
else if (gpsIniciado && UltimaLeitura.Distancia > 0.25)
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoMovimento;
|
|
}
|
|
else if (imuIniciado)
|
|
{
|
|
anguloFinal = op.Sensoriamento.IMU.YawSeguro;
|
|
}
|
|
else
|
|
{
|
|
anguloFinal = UltimaLeitura.AnguloCarroDefinido;
|
|
}
|
|
|
|
UltimaLeitura.AnguloCarroDefinido = anguloFinal;
|
|
}
|
|
|
|
public static double FundirHeadingComMovimento(double headingDeg, double angMovDeg, double distLinearJanela, double velPercent = 0.0, bool rtkFix = true, double angFundidoAnterior = double.NaN, double alfaLowPass = 0.25)
|
|
{
|
|
double distMin = 0.03;
|
|
double distFull = 0.25;
|
|
double pesoMinHeading = 0.30;
|
|
double fatorConfMovSemFix = 0.80;
|
|
|
|
double fd = Smoothstep(Norm(distLinearJanela, distMin, distFull));
|
|
|
|
double fv =
|
|
FuncoesMatematicas.Clamp(
|
|
velPercent / 100.0,
|
|
0.0,
|
|
1.0
|
|
);
|
|
|
|
double confMov = Math.Max(fd, fv);
|
|
|
|
if (!rtkFix)
|
|
confMov *= fatorConfMovSemFix;
|
|
|
|
double pesoHeading = 1.0 - confMov;
|
|
double minHeading =
|
|
pesoMinHeading * confMov;
|
|
|
|
if (pesoHeading < minHeading)
|
|
pesoHeading = minHeading;
|
|
|
|
if (pesoHeading > 1.0)
|
|
pesoHeading = 1.0;
|
|
|
|
double angMisto =
|
|
MisturarAngulosCircular(
|
|
headingDeg,
|
|
angMovDeg,
|
|
pesoHeading
|
|
);
|
|
|
|
if (!double.IsNaN(angFundidoAnterior) &&
|
|
alfaLowPass > 0)
|
|
{
|
|
angMisto = LowPassAngle0to360(
|
|
angFundidoAnterior,
|
|
angMisto,
|
|
alfaLowPass
|
|
);
|
|
}
|
|
|
|
return Normalize0To360(angMisto);
|
|
}
|
|
|
|
private static double Norm(double value, double min, double max)
|
|
{
|
|
if (max <= min)
|
|
return value >= max ? 1.0 : 0.0;
|
|
|
|
return FuncoesMatematicas.Clamp(
|
|
(value - min) / (max - min),
|
|
0.0,
|
|
1.0
|
|
);
|
|
}
|
|
|
|
private static double Smoothstep(double value)
|
|
{
|
|
double x =
|
|
FuncoesMatematicas.Clamp(
|
|
value,
|
|
0.0,
|
|
1.0
|
|
);
|
|
|
|
return x * x * (3.0 - 2.0 * x);
|
|
}
|
|
|
|
private static double MisturarAngulosCircular(double aDeg, double bDeg, double pesoA)
|
|
{
|
|
double a = aDeg * Math.PI / 180.0;
|
|
double b = bDeg * Math.PI / 180.0;
|
|
|
|
double x =
|
|
Math.Cos(a) * pesoA +
|
|
Math.Cos(b) * (1.0 - pesoA);
|
|
|
|
double y =
|
|
Math.Sin(a) * pesoA +
|
|
Math.Sin(b) * (1.0 - pesoA);
|
|
|
|
return Math.Atan2(y, x) *
|
|
180.0 / Math.PI;
|
|
}
|
|
|
|
private static double LowPassAngle0to360(double atualDeg, double novoDeg, double alfa)
|
|
{
|
|
double diff = NormalizeSigned180(novoDeg - atualDeg);
|
|
|
|
return Normalize0To360(atualDeg + alfa * diff);
|
|
}
|
|
|
|
private static double NormalizeSigned180(double angDeg)
|
|
{
|
|
angDeg = (angDeg + 180.0) % 360.0;
|
|
|
|
if (angDeg < 0)
|
|
angDeg += 360.0;
|
|
|
|
return angDeg - 180.0;
|
|
}
|
|
|
|
private static double Normalize0To360(double angDeg)
|
|
{
|
|
angDeg %= 360.0;
|
|
|
|
if (angDeg < 0)
|
|
angDeg += 360.0;
|
|
|
|
return angDeg;
|
|
}
|
|
|
|
public static void AtualizaDadosRedis()
|
|
{
|
|
try
|
|
{
|
|
GPSModel posicaoAtual;
|
|
|
|
if (Variaveis.OperacaoEmAndamento?.Simulando == true)
|
|
{
|
|
lock (_stateLock)
|
|
{
|
|
posicaoAtual = historicoPosicao.Count > 0 ? historicoPosicao.Peek().Clone() : UltimaLeitura.Clone();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
posicaoAtual = GetSnapshot();
|
|
}
|
|
|
|
bool rtkValido = posicaoAtual.IdadeCorrecao >= 0 && posicaoAtual.IdadeCorrecao < rtk_timeout &&
|
|
(
|
|
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFixo ||
|
|
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFlutuante ||
|
|
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.DGPS
|
|
);
|
|
|
|
string statusOrientacao = (posicaoAtual.TipoOrientacao ?? string.Empty).Trim().ToUpperInvariant();
|
|
double valorOrientacao = posicaoAtual.OrientacaoReal;
|
|
bool orientacaoValida = statusOrientacao == "A" && !double.IsNaN(valorOrientacao) && !double.IsInfinity(valorOrientacao) && valorOrientacao >= 0.0 && valorOrientacao < 360.0;
|
|
|
|
GpsTransportMetrics metrics = GetTransportMetrics();
|
|
|
|
RedisService.AtualizarCampos(
|
|
RedisService.ModKey(T_Code.Gps),
|
|
|
|
("conectado", Iniciado),
|
|
("freq_base", TaxaAmostragemHz),
|
|
|
|
("lat", posicaoAtual.Latitude),
|
|
("lon", posicaoAtual.Longitude),
|
|
("theta", posicaoAtual.AnguloCarroDefinido),
|
|
|
|
("fix", (int)posicaoAtual.QualidadeFix),
|
|
("rtk", rtkValido),
|
|
("hAcc", posicaoAtual.PrecisaoCm),
|
|
("nSatelites", posicaoAtual.NumeroSatelites),
|
|
("age", posicaoAtual.IdadeCorrecao),
|
|
|
|
("freq.posicao", posicaoAtual.TimestampPos.frequencia),
|
|
("freq.orientacao", posicaoAtual.TimestampOri.frequencia),
|
|
|
|
("latency.posicao", posicaoAtual.TimestampPos.dt),
|
|
("latency.orientacao", posicaoAtual.TimestampOri.dt),
|
|
|
|
("timestamp.posicao", posicaoAtual.TimestampPos.valor),
|
|
("timestamp.orientacao", posicaoAtual.TimestampOri.valor),
|
|
|
|
("orientacao.status", statusOrientacao),
|
|
("orientacao.valida", orientacaoValida),
|
|
("orientacao.valor", valorOrientacao),
|
|
|
|
("heartbeat", posicaoAtual.Heartbeat),
|
|
|
|
("rtcm.received", metrics.RtcmReceived),
|
|
("rtcm.written", metrics.RtcmWritten),
|
|
("rtcm.queue_depth", metrics.RtcmQueueDepth),
|
|
("rtcm.last_rx_age_ms", metrics.LastRtcmReceivedAgeMs),
|
|
("rtcm.last_write_age_ms", metrics.LastRtcmWrittenAgeMs),
|
|
("rtcm.drop_stale", metrics.RtcmDroppedStale),
|
|
("rtcm.write_errors", metrics.RtcmWriteErrors)
|
|
);
|
|
|
|
CorrecaoRTK_Ntrip = RedisService.GetField<bool>(RedisService.ModKey(T_Code.Gps), "correcao_ntrip", false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError("[GPSService.AtualizaDadosRedis] " + ex.Message);
|
|
|
|
try
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(
|
|
T_Code.Gps,
|
|
StatusModulo.Falha,
|
|
0,
|
|
"Erro ao salvar dados do GPS no Redis: " + ex.Message
|
|
);
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
|
|
public static void AtualizarAtrasoPosicoes(int errosConsiderar = 0)
|
|
{
|
|
lock (_stateLock)
|
|
{
|
|
historicoPosicao.Enqueue(UltimaLeitura.Clone());
|
|
|
|
while (historicoPosicao.Count > 1 &&
|
|
(
|
|
historicoPosicao.Count >
|
|
errosConsiderar ||
|
|
errosConsiderar == 0
|
|
))
|
|
{
|
|
historicoPosicao.Dequeue();
|
|
}
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// LOG EM WORKER
|
|
// ============================================================
|
|
|
|
private static void EnfileirarLogSnapshot()
|
|
{
|
|
try
|
|
{
|
|
string json;
|
|
|
|
lock (_stateLock)
|
|
{
|
|
GPSModel obj = UltimaLeitura.Clone();
|
|
obj.Momento = DateTime.Now;
|
|
json = JsonConvert.SerializeObject(obj);
|
|
}
|
|
|
|
_gpsLogQueue.Enqueue(json);
|
|
|
|
try { _logSignal.Release(); }
|
|
catch (SemaphoreFullException) { }
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.EnfileirarLogSnapshot] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
private static async Task LogWriterLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
var lote = new List<string>(300);
|
|
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await _logSignal.WaitAsync(
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
|
|
string item;
|
|
|
|
while (lote.Count < 300 &&
|
|
_gpsLogQueue.TryDequeue(
|
|
out item))
|
|
{
|
|
lote.Add(item);
|
|
}
|
|
|
|
if (lote.Count < 300)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
/*
|
|
* RegistrarLogDispositivo limpa a lista recebida.
|
|
* Passamos uma lista exclusiva do worker.
|
|
*/
|
|
VariaveisOperacao
|
|
.RegistrarLogDispositivo(
|
|
lote,
|
|
T_Code.Gps,
|
|
".json",
|
|
300
|
|
);
|
|
|
|
lote = new List<string>(300);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.LogWriter] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
while (_gpsLogQueue.TryDequeue(out var pendente))
|
|
lote.Add(pendente);
|
|
|
|
if (lote.Count > 0)
|
|
{
|
|
try
|
|
{
|
|
VariaveisOperacao
|
|
.RegistrarLogDispositivo(
|
|
lote,
|
|
T_Code.Gps,
|
|
".json",
|
|
1
|
|
);
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// WORKERS E SHUTDOWN
|
|
// ============================================================
|
|
|
|
private static void EnsureBackgroundWorkersStarted()
|
|
{
|
|
lock (_workerLock)
|
|
{
|
|
if (_serviceCts != null &&
|
|
!_serviceCts.IsCancellationRequested)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_serviceCts =
|
|
new CancellationTokenSource();
|
|
|
|
CancellationToken token =
|
|
_serviceCts.Token;
|
|
|
|
_nmeaWorkerTask = Task.Run(
|
|
() => NmeaWorkerLoopAsync(token),
|
|
token
|
|
);
|
|
|
|
_rtcmWriterTask = Task.Run(
|
|
() => RtcmWriterLoopAsync(token),
|
|
token
|
|
);
|
|
|
|
_logWriterTask = Task.Run(
|
|
() => LogWriterLoopAsync(token),
|
|
token
|
|
);
|
|
|
|
_localPublisherTask = Task.Run(
|
|
() => LocalPublisherLoopAsync(token),
|
|
token
|
|
);
|
|
}
|
|
}
|
|
|
|
public static async Task EncerrarAsync()
|
|
{
|
|
await PararNtripAsync().ConfigureAwait(false);
|
|
|
|
CancellationTokenSource cts;
|
|
Task[] tasks;
|
|
|
|
lock (_workerLock)
|
|
{
|
|
cts = _serviceCts;
|
|
|
|
tasks = new[]
|
|
{
|
|
_nmeaWorkerTask,
|
|
_rtcmWriterTask,
|
|
_logWriterTask,
|
|
_localPublisherTask
|
|
}
|
|
.Where(x => x != null)
|
|
.ToArray();
|
|
|
|
_serviceCts = null;
|
|
_nmeaWorkerTask = null;
|
|
_rtcmWriterTask = null;
|
|
_logWriterTask = null;
|
|
_localPublisherTask = null;
|
|
}
|
|
|
|
if (cts != null)
|
|
{
|
|
try { cts.Cancel(); } catch { }
|
|
}
|
|
|
|
try
|
|
{
|
|
await Task.WhenAll(tasks)
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException) { }
|
|
catch (Exception ex)
|
|
{
|
|
RecordError(
|
|
"[GPSService.EncerrarAsync] " +
|
|
ex.Message
|
|
);
|
|
}
|
|
|
|
if (cts != null)
|
|
cts.Dispose();
|
|
|
|
CancellationTokenSource configCts =
|
|
Interlocked.Exchange(
|
|
ref _configurationCts,
|
|
null
|
|
);
|
|
|
|
if (configCts != null)
|
|
{
|
|
try { configCts.Cancel(); } catch { }
|
|
}
|
|
|
|
/*
|
|
* Aguarda uma configuração em andamento abandonar os locks.
|
|
* O token acima interrompe os delays cooperativos.
|
|
*/
|
|
await _configurationLock.WaitAsync()
|
|
.ConfigureAwait(false);
|
|
_configurationLock.Release();
|
|
|
|
await _serialWriteLock.WaitAsync()
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
lock (_portLifecycleLock)
|
|
{
|
|
SerialPort porta = PortaGPS;
|
|
PortaGPS = null;
|
|
|
|
if (porta != null)
|
|
{
|
|
try
|
|
{
|
|
porta.DataReceived -=
|
|
PortaGPS_DataReceived;
|
|
}
|
|
catch { }
|
|
|
|
try
|
|
{
|
|
if (porta.IsOpen)
|
|
porta.Close();
|
|
}
|
|
catch { }
|
|
|
|
try { porta.Dispose(); }
|
|
catch { }
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_serialWriteLock.Release();
|
|
}
|
|
|
|
if (configCts != null)
|
|
configCts.Dispose();
|
|
}
|
|
|
|
// ============================================================
|
|
// MÉTRICAS
|
|
// ============================================================
|
|
|
|
public static GpsTransportMetrics GetTransportMetrics()
|
|
{
|
|
int queueDepth;
|
|
int nmeaQueueDepth;
|
|
|
|
lock (_rtcmQueueLock)
|
|
queueDepth = _rtcmQueue.Count;
|
|
|
|
lock (_nmeaQueueLock)
|
|
nmeaQueueDepth = _nmeaQueue.Count;
|
|
|
|
string lastError;
|
|
|
|
lock (_metricsTextLock)
|
|
lastError = _lastError;
|
|
|
|
return new GpsTransportMetrics
|
|
{
|
|
SerialConnected = Iniciado,
|
|
SerialPortName =
|
|
PortaGPS != null
|
|
? PortaGPS.PortName
|
|
: null,
|
|
|
|
LastSerialRxAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastSerialRxMono
|
|
)
|
|
),
|
|
|
|
LastValidNmeaAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastValidNmeaMono
|
|
)
|
|
),
|
|
|
|
LastValidGgaAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastValidGgaMono
|
|
)
|
|
),
|
|
|
|
LastValidThsAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastValidThsMono
|
|
)
|
|
),
|
|
|
|
SerialBytesReceived =
|
|
Interlocked.Read(
|
|
ref _serialBytesReceived
|
|
),
|
|
|
|
NmeaValid =
|
|
Interlocked.Read(ref _nmeaValid),
|
|
|
|
NmeaInvalid =
|
|
Interlocked.Read(ref _nmeaInvalid),
|
|
|
|
NmeaChecksumErrors =
|
|
Interlocked.Read(
|
|
ref _nmeaChecksumErrors
|
|
),
|
|
|
|
NmeaBufferResets =
|
|
Interlocked.Read(
|
|
ref _nmeaBufferResets
|
|
),
|
|
|
|
NmeaQueued =
|
|
Interlocked.Read(ref _nmeaQueued),
|
|
|
|
NmeaDroppedQueue =
|
|
Interlocked.Read(
|
|
ref _nmeaDroppedQueue
|
|
),
|
|
|
|
NmeaQueueDepth = nmeaQueueDepth,
|
|
|
|
RtcmReceived =
|
|
Interlocked.Read(ref _rtcmReceived),
|
|
|
|
RtcmQueued =
|
|
Interlocked.Read(ref _rtcmQueued),
|
|
|
|
RtcmWritten =
|
|
Interlocked.Read(ref _rtcmWritten),
|
|
|
|
RtcmDroppedQueue =
|
|
Interlocked.Read(
|
|
ref _rtcmDroppedQueue
|
|
),
|
|
|
|
RtcmDroppedStale =
|
|
Interlocked.Read(
|
|
ref _rtcmDroppedStale
|
|
),
|
|
|
|
RtcmDroppedInvalid =
|
|
Interlocked.Read(
|
|
ref _rtcmDroppedInvalid
|
|
),
|
|
|
|
RtcmWriteErrors =
|
|
Interlocked.Read(
|
|
ref _rtcmWriteErrors
|
|
),
|
|
|
|
RtcmQueueDepth = queueDepth,
|
|
|
|
LastRtcmReceivedAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastRtcmReceivedMono
|
|
)
|
|
),
|
|
|
|
LastRtcmWrittenAgeMs =
|
|
GetAgeMs(
|
|
Interlocked.Read(
|
|
ref _lastRtcmWrittenMono
|
|
)
|
|
),
|
|
|
|
ConfigurationRuns =
|
|
Interlocked.Read(
|
|
ref _configurationRuns
|
|
),
|
|
|
|
ConfigurationErrors =
|
|
Interlocked.Read(
|
|
ref _configurationErrors
|
|
),
|
|
|
|
NtripRunning = LoopRTK_Ntrip,
|
|
NtripConnected = Ntrip_Conectado,
|
|
NtripProvider = NtripProviderAtivo,
|
|
NtripMountpoint = NtripMountpointAtivo,
|
|
NtripMountpointDistanceKm =
|
|
NtripDistanciaMountpointAtivoKm,
|
|
|
|
NtripReconnects =
|
|
Interlocked.Read(
|
|
ref _ntripReconnects
|
|
),
|
|
|
|
NtripBytesReceived =
|
|
Interlocked.Read(
|
|
ref _ntripBytesReceived
|
|
),
|
|
|
|
NtripErrors =
|
|
Interlocked.Read(
|
|
ref _ntripErrors
|
|
),
|
|
|
|
LastError = lastError
|
|
};
|
|
}
|
|
|
|
private static void RecordError(string error)
|
|
{
|
|
lock (_metricsTextLock)
|
|
_lastError = error;
|
|
|
|
Variaveis.MostrarLog(error);
|
|
}
|
|
|
|
// ============================================================
|
|
// HELPERS
|
|
// ============================================================
|
|
|
|
private static void WriteSerialUnsafe(byte[] bytes, int count)
|
|
{
|
|
SerialPort porta = PortaGPS;
|
|
|
|
if (porta == null || !porta.IsOpen)
|
|
throw new IOException(
|
|
"Porta GPS não está aberta."
|
|
);
|
|
|
|
porta.Write(bytes, 0, count);
|
|
}
|
|
|
|
private static double GetAgeMs(long timestamp)
|
|
{
|
|
if (timestamp <= 0)
|
|
return double.PositiveInfinity;
|
|
|
|
long delta =
|
|
Stopwatch.GetTimestamp() - timestamp;
|
|
|
|
if (delta <= 0)
|
|
return 0;
|
|
|
|
return delta * 1000.0 /
|
|
Stopwatch.Frequency;
|
|
}
|
|
|
|
private static string GetField(string[] fields, int index, string fallback = "")
|
|
{
|
|
return fields != null &&
|
|
index >= 0 &&
|
|
index < fields.Length
|
|
? fields[index] ?? fallback
|
|
: fallback;
|
|
}
|
|
|
|
private static string StripChecksum(string value)
|
|
{
|
|
if (string.IsNullOrEmpty(value))
|
|
return string.Empty;
|
|
|
|
int index = value.IndexOf('*');
|
|
|
|
return index >= 0
|
|
? value.Substring(0, index)
|
|
: value;
|
|
}
|
|
|
|
private static bool TryParseDmm(string raw, string hemisphere, int degreeDigits, out double result)
|
|
{
|
|
result = 0;
|
|
|
|
if (string.IsNullOrWhiteSpace(raw) || raw.Length <= degreeDigits)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double degrees;
|
|
double minutes;
|
|
|
|
if (!double.TryParse(
|
|
raw.Substring(0, degreeDigits),
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out degrees) ||
|
|
!double.TryParse(
|
|
raw.Substring(degreeDigits),
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out minutes))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
result = degrees + minutes / 60.0;
|
|
|
|
if (string.Equals(
|
|
hemisphere,
|
|
"S",
|
|
StringComparison.OrdinalIgnoreCase) ||
|
|
string.Equals(
|
|
hemisphere,
|
|
"W",
|
|
StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
result = -result;
|
|
}
|
|
|
|
return !double.IsNaN(result) &&
|
|
!double.IsInfinity(result);
|
|
}
|
|
|
|
private static bool TryParseNmeaTime(string raw, out DateTime result)
|
|
{
|
|
result = default(DateTime);
|
|
|
|
if (string.IsNullOrWhiteSpace(raw) || raw.Length < 6)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
int hh;
|
|
int mm;
|
|
double ss;
|
|
|
|
if (!int.TryParse(
|
|
raw.Substring(0, 2),
|
|
out hh) ||
|
|
!int.TryParse(
|
|
raw.Substring(2, 2),
|
|
out mm) ||
|
|
!double.TryParse(
|
|
raw.Substring(4),
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out ss))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
int seconds = (int)Math.Floor(ss);
|
|
int milliseconds = (int)Math.Round(
|
|
(ss - seconds) * 1000.0
|
|
);
|
|
|
|
if (milliseconds >= 1000)
|
|
{
|
|
seconds++;
|
|
milliseconds = 0;
|
|
}
|
|
|
|
try
|
|
{
|
|
result = DateTime.UtcNow.Date
|
|
.AddHours(hh)
|
|
.AddMinutes(mm)
|
|
.AddSeconds(seconds)
|
|
.AddMilliseconds(milliseconds)
|
|
.ToLocalTime();
|
|
|
|
return true;
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private static bool TryParseRmcDateTime(string dateRaw, string timeRaw, out DateTime result)
|
|
{
|
|
result = default(DateTime);
|
|
|
|
if (string.IsNullOrWhiteSpace(dateRaw) || dateRaw.Length != 6 || string.IsNullOrWhiteSpace(timeRaw) || timeRaw.Length < 6)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
int day;
|
|
int month;
|
|
int yearShort;
|
|
int hour;
|
|
int minute;
|
|
int second;
|
|
|
|
if (!int.TryParse(
|
|
dateRaw.Substring(0, 2),
|
|
out day) ||
|
|
!int.TryParse(
|
|
dateRaw.Substring(2, 2),
|
|
out month) ||
|
|
!int.TryParse(
|
|
dateRaw.Substring(4, 2),
|
|
out yearShort) ||
|
|
!int.TryParse(
|
|
timeRaw.Substring(0, 2),
|
|
out hour) ||
|
|
!int.TryParse(
|
|
timeRaw.Substring(2, 2),
|
|
out minute) ||
|
|
!int.TryParse(
|
|
timeRaw.Substring(4, 2),
|
|
out second))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
result = new DateTime(
|
|
2000 + yearShort,
|
|
month,
|
|
day,
|
|
hour,
|
|
minute,
|
|
second,
|
|
DateTimeKind.Utc
|
|
);
|
|
|
|
return true;
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private sealed class NtripProviderConfig
|
|
{
|
|
public string Nome { get; set; }
|
|
public string Host { get; set; }
|
|
public int Port { get; set; }
|
|
public string MountpointManual { get; set; }
|
|
public string Username { get; set; }
|
|
public string Password { get; set; }
|
|
public bool UsarNtripV2 { get; set; }
|
|
public bool Comunitario { get; set; }
|
|
public bool ExigeUsuario { get; set; }
|
|
public bool ExigeSenha { get; set; }
|
|
public string FiltroPais { get; set; }
|
|
public bool PermitirFallbackManualSeAutoFalhar { get; set; }
|
|
}
|
|
|
|
private sealed class NtripCandidate
|
|
{
|
|
public string Mountpoint { get; set; }
|
|
public double? DistanciaKm { get; set; }
|
|
public string Formato { get; set; }
|
|
public string SistemasGnss { get; set; }
|
|
public bool OrigemAuto { get; set; }
|
|
}
|
|
|
|
private sealed class NtripMountpointInfo
|
|
{
|
|
public string Mountpoint { get; set; }
|
|
public string Identificador { get; set; }
|
|
public string Formato { get; set; }
|
|
public string SistemasGnss { get; set; }
|
|
public string Rede { get; set; }
|
|
public string Pais { get; set; }
|
|
public double Latitude { get; set; }
|
|
public double Longitude { get; set; }
|
|
public bool ExigeGgaCliente { get; set; }
|
|
public string Autenticacao { get; set; }
|
|
public int TaxaBits { get; set; }
|
|
public double DistanciaKm { get; set; }
|
|
}
|
|
|
|
private sealed class NtripSourcetableCacheEntry
|
|
{
|
|
public DateTime ObtidaUtc { get; set; }
|
|
public List<NtripMountpointInfo> Streams { get; set; }
|
|
}
|
|
|
|
private sealed class NtripApproxPosition
|
|
{
|
|
public double Latitude { get; set; }
|
|
public double Longitude { get; set; }
|
|
}
|
|
|
|
private sealed class RtcmEnvelope
|
|
{
|
|
public byte[] Bytes { get; set; }
|
|
public long ReceivedMonotonic { get; set; }
|
|
public DateTime ReceivedUtc { get; set; }
|
|
public long Sequence { get; set; }
|
|
public int MessageType { get; set; }
|
|
public RtcmSource Source { get; set; }
|
|
}
|
|
|
|
private enum RtcmSource
|
|
{
|
|
Mqtt,
|
|
Ntrip
|
|
}
|
|
|
|
private sealed class NtripHeaderResult
|
|
{
|
|
public bool Success { get; set; }
|
|
public string StatusLine { get; set; }
|
|
public byte[] RemainingBytes { get; set; }
|
|
}
|
|
}
|
|
|
|
public sealed class GpsTransportMetrics
|
|
{
|
|
public bool SerialConnected { get; set; }
|
|
public string SerialPortName { get; set; }
|
|
|
|
public double LastSerialRxAgeMs { get; set; }
|
|
public double LastValidNmeaAgeMs { get; set; }
|
|
public double LastValidGgaAgeMs { get; set; }
|
|
public double LastValidThsAgeMs { get; set; }
|
|
|
|
public long SerialBytesReceived { get; set; }
|
|
|
|
public long NmeaValid { get; set; }
|
|
public long NmeaInvalid { get; set; }
|
|
public long NmeaChecksumErrors { get; set; }
|
|
public long NmeaBufferResets { get; set; }
|
|
public long NmeaQueued { get; set; }
|
|
public long NmeaDroppedQueue { get; set; }
|
|
public int NmeaQueueDepth { get; set; }
|
|
|
|
public long RtcmReceived { get; set; }
|
|
public long RtcmQueued { get; set; }
|
|
public long RtcmWritten { get; set; }
|
|
public long RtcmDroppedQueue { get; set; }
|
|
public long RtcmDroppedStale { get; set; }
|
|
public long RtcmDroppedInvalid { get; set; }
|
|
public long RtcmWriteErrors { get; set; }
|
|
public int RtcmQueueDepth { get; set; }
|
|
public double LastRtcmReceivedAgeMs { get; set; }
|
|
public double LastRtcmWrittenAgeMs { get; set; }
|
|
|
|
public long ConfigurationRuns { get; set; }
|
|
public long ConfigurationErrors { get; set; }
|
|
|
|
public bool NtripRunning { get; set; }
|
|
public bool NtripConnected { get; set; }
|
|
public string NtripProvider { get; set; }
|
|
public string NtripMountpoint { get; set; }
|
|
public double? NtripMountpointDistanceKm { get; set; }
|
|
public long NtripReconnects { get; set; }
|
|
public long NtripBytesReceived { get; set; }
|
|
public long NtripErrors { get; set; }
|
|
|
|
public string LastError { get; set; }
|
|
}
|
|
|
|
public class GgaFix
|
|
{
|
|
public DateTime TsUtc { get; private set; }
|
|
public double LatDeg { get; private set; }
|
|
public double LonDeg { get; private set; }
|
|
public double AltElipsoidalM { get; private set; }
|
|
public double HeadingDeg { get; private set; }
|
|
public TiposCorrecaoGPS FixQuality { get; private set; }
|
|
|
|
public GgaFix(
|
|
DateTime tsUtc,
|
|
double latDeg,
|
|
double lonDeg,
|
|
double altElipsoidalM,
|
|
double headingDeg,
|
|
TiposCorrecaoGPS fixQuality)
|
|
{
|
|
TsUtc = tsUtc;
|
|
LatDeg = latDeg;
|
|
LonDeg = lonDeg;
|
|
AltElipsoidalM = altElipsoidalM;
|
|
HeadingDeg = headingDeg;
|
|
FixQuality = fixQuality;
|
|
}
|
|
}
|
|
|
|
public class GeoLeverArm
|
|
{
|
|
private readonly double xFisico;
|
|
private readonly double yFisico;
|
|
private readonly double xCampo;
|
|
private readonly double yCampo;
|
|
|
|
private readonly bool invertHeading;
|
|
private readonly bool invertFrontal;
|
|
private readonly bool mirrorLateral;
|
|
|
|
public GeoLeverArm(
|
|
double offsetFisicoFrontalCm = 0,
|
|
double offsetFisicoLateralCm = 0,
|
|
double offsetCampoFrontalCm = 0,
|
|
double offsetCampoLateralCm = 0,
|
|
bool invH = false,
|
|
bool invF = false,
|
|
bool mirrL = false
|
|
)
|
|
{
|
|
xFisico = offsetFisicoFrontalCm / 100.0;
|
|
yFisico = offsetFisicoLateralCm / 100.0;
|
|
xCampo = offsetCampoFrontalCm / 100.0;
|
|
yCampo = offsetCampoLateralCm / 100.0;
|
|
|
|
invertHeading = invH;
|
|
invertFrontal = invF;
|
|
mirrorLateral = mirrL;
|
|
}
|
|
|
|
private static double ToRad(double deg)
|
|
{
|
|
return deg * Math.PI / 180.0;
|
|
}
|
|
|
|
public double FrontalTotalCm
|
|
{
|
|
get { return (xFisico + xCampo) * 100.0; }
|
|
}
|
|
|
|
public double LateralTotalCm
|
|
{
|
|
get { return (yFisico + yCampo) * 100.0; }
|
|
}
|
|
|
|
public double FrontalCampoCm
|
|
{
|
|
get { return (xCampo) * 100.0; }
|
|
}
|
|
|
|
public double LateralCampoCm
|
|
{
|
|
get { return (yCampo) * 100.0; }
|
|
}
|
|
|
|
public (double lat, double lon) FixLeverArmLatLon_Fast(double latAnt_deg, double lonAnt_deg, double headingDeg, double headingFreq)
|
|
{
|
|
if (headingFreq < 1)
|
|
return (latAnt_deg, lonAnt_deg);
|
|
|
|
double xTotal = xFisico + xCampo;
|
|
double yTotal = yFisico + yCampo;
|
|
|
|
if (invertFrontal)
|
|
xTotal = -xTotal;
|
|
|
|
if (mirrorLateral)
|
|
yTotal = -yTotal;
|
|
|
|
double th = ToRad(headingDeg);
|
|
|
|
if (invertHeading)
|
|
th = -th;
|
|
|
|
double fE = Math.Sin(th);
|
|
double fN = Math.Cos(th);
|
|
|
|
double rE = fN;
|
|
double rN = -fE;
|
|
|
|
double dE =
|
|
xTotal * fE +
|
|
yTotal * rE;
|
|
|
|
double dN =
|
|
xTotal * fN +
|
|
yTotal * rN;
|
|
|
|
double latRad = ToRad(latAnt_deg);
|
|
|
|
double dLatDeg =
|
|
dN / GPSUtils.RaioDaTerra *
|
|
180.0 / Math.PI;
|
|
|
|
double cosLat = Math.Cos(latRad);
|
|
|
|
if (Math.Abs(cosLat) < 1e-12)
|
|
return (latAnt_deg, lonAnt_deg);
|
|
|
|
double dLonDeg =
|
|
dE /
|
|
(GPSUtils.RaioDaTerra * cosLat) *
|
|
180.0 / Math.PI;
|
|
|
|
return (
|
|
latAnt_deg - dLatDeg,
|
|
lonAnt_deg - dLonDeg
|
|
);
|
|
}
|
|
|
|
public (double x, double y) GeodeticToENU(double lat, double lon, double lat0, double lon0)
|
|
{
|
|
double latR = ToRad(lat);
|
|
double lonR = ToRad(lon);
|
|
double lat0R = ToRad(lat0);
|
|
double lon0R = ToRad(lon0);
|
|
|
|
double dLat = latR - lat0R;
|
|
double dLon = lonR - lon0R;
|
|
|
|
double xEast =
|
|
dLon *
|
|
Math.Cos(lat0R) *
|
|
GPSUtils.RaioDaTerra;
|
|
|
|
double yNorth =
|
|
dLat * GPSUtils.RaioDaTerra;
|
|
|
|
return (xEast, yNorth);
|
|
}
|
|
}
|
|
}
|