1641 lines
68 KiB
C#
1641 lines
68 KiB
C#
using AgroBase.Models;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.IO.Ports;
|
|
using System.Linq;
|
|
using System.Net.Sockets;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using static AgroBase.Models.Enums;
|
|
using static AgroBase.Services.GPSService;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class GPSService
|
|
{
|
|
public static SerialPort PortaGPS = null;
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
if (PortaGPS != null && !PortaGPS.IsOpen)
|
|
{
|
|
try
|
|
{
|
|
PortaGPS.Open();
|
|
}
|
|
catch
|
|
{
|
|
PortaGPS = null;
|
|
}
|
|
}
|
|
return PortaGPS != null && PortaGPS.IsOpen;
|
|
}
|
|
}
|
|
|
|
public static GPSModel UltimaLeitura = new GPSModel();
|
|
public static GPSModel PenultimaLeitura = new GPSModel();
|
|
public static List<string> Logs = new List<string>();
|
|
|
|
public static int TaxaAmostragemHz { get; set; } = 5;
|
|
|
|
private static int rtk_timeout = 60;
|
|
private static int TempoMin_Ntrip = 10;
|
|
private static bool LoopRTK_Ntrip = false;
|
|
public static bool CorrecaoRTK_Ntrip = false;
|
|
public static DateTime UltimoEnvioCorrecaoRTK = DateTime.MinValue;
|
|
|
|
public static void AtualizarPortaCOM(SerialPort Porta)
|
|
{
|
|
if (PortaGPS == null)
|
|
{
|
|
PortaGPS = new SerialPort();
|
|
}
|
|
else if (Iniciado)
|
|
{
|
|
PortaGPS.Close();
|
|
}
|
|
|
|
PortaGPS.BaudRate = Porta.BaudRate;
|
|
PortaGPS.PortName = Porta.PortName;
|
|
PortaGPS.ReadTimeout = 2000; // Timeout de 2 segundos
|
|
PortaGPS.WriteTimeout = 2000; // Timeout de 2 segundos
|
|
PortaGPS.DataReceived -= PortaGPS_DataReceived;
|
|
PortaGPS.DataReceived += PortaGPS_DataReceived;
|
|
|
|
Porta.Close();
|
|
|
|
if (Iniciado)
|
|
{
|
|
DefinirDispositivo();
|
|
Task.Run(async () => await ConfigurarModulo());
|
|
if (!Variaveis.IsAgroMonitor && CorrecaoRTK_Ntrip)
|
|
{
|
|
Task.Run(async () => await AplicarCorrecaoRTK_Ntrip());
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void DefinirDispositivo()
|
|
{
|
|
if (Iniciado)
|
|
{
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Dispositivo = T_Code.Gps,
|
|
Endereco = PortaGPS.PortName,
|
|
Versao = "1",
|
|
});
|
|
}
|
|
}
|
|
|
|
private static async Task ConfigurarModulo()
|
|
{
|
|
Console.WriteLine("Iniciando configuração do módulo GPS...");
|
|
if (Variaveis.IsAgroMonitor)
|
|
{
|
|
bool sucesso = await BaseFixService.FixarBaseViaNtripAsync(
|
|
startNtrip: async () =>
|
|
{
|
|
Console.WriteLine("Iniciando correção NTRIP...");
|
|
CorrecaoRTK_Ntrip = true;
|
|
Task.Run(async () => await AplicarCorrecaoRTK_Ntrip());
|
|
},
|
|
stopNtrip: async () =>
|
|
{
|
|
Console.WriteLine("Parando correção NTRIP...");
|
|
CorrecaoRTK_Ntrip = false;
|
|
}
|
|
);
|
|
if (!sucesso)
|
|
{
|
|
await ConfigurarModuloBase(tempo_fixacao: 600);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
await ConfigurarModuloRover(comprimento_antena: 130);
|
|
}
|
|
}
|
|
|
|
private static async Task ConfigurarModuloRover(string porta_usb = "com3", string porta_entrada = "com2", int comprimento_antena = 100, int tolerancia_antena = 5)
|
|
{
|
|
string freq = (1.0 / TaxaAmostragemHz).ToString("0.0").Replace(",", ".");
|
|
string[] comandos = {
|
|
// Bauds
|
|
$"config com1 115200\r\n",
|
|
$"config {porta_usb} 115200\r\n",
|
|
$"config {porta_entrada} 115200\r\n",
|
|
|
|
// limpa logs das portas
|
|
$"unlog com1\r\n",
|
|
$"unlog com2\r\n",
|
|
$"unlog com3\r\n",
|
|
|
|
// modo rover + RTK
|
|
$"mode rover uav\r\n",
|
|
|
|
// timeouts de correção
|
|
$"config rtk timeout {rtk_timeout}\r\n",
|
|
$"config dgps timeout 60\r\n", // ou 0 para desabilitar DGPS fallback
|
|
|
|
// heading 2 antenas
|
|
$"config heading fixlength\r\n",
|
|
$"config heading tractor\r\n",
|
|
$"config heading length {comprimento_antena} {tolerancia_antena}\r\n",
|
|
// (se precisar, existe 'config heading offset <azim_off> <pitch_off>')
|
|
|
|
// NMEA só na USB (COM1)
|
|
$"gngga {porta_usb} {freq}\r\n",
|
|
$"gpths {porta_usb} {freq}\r\n",
|
|
|
|
$"saveconfig\r\n"
|
|
};
|
|
|
|
await Task.Delay(2000);
|
|
|
|
foreach (string comando in comandos)
|
|
{
|
|
byte[] bytesComando = Encoding.ASCII.GetBytes(comando);
|
|
PortaGPS.Write(bytesComando, 0, bytesComando.Length);
|
|
await Task.Delay(1000); // Pequeno delay para evitar sobrecarga na comunicação
|
|
}
|
|
}
|
|
|
|
private static async Task ConfigurarModuloBase(string porta_usb = "com3", string porta_saida = "com2", int tempo_fixacao = 60)
|
|
{
|
|
string base_id = "957";
|
|
string distancia_min = "0";
|
|
string[] comandos = {
|
|
// Bauds
|
|
$"config {porta_usb} 115200\r\n",
|
|
$"config {porta_saida} 115200\r\n",
|
|
|
|
// limpa logs das portas
|
|
$"unlog com1\r\n",
|
|
$"unlog com2\r\n",
|
|
$"unlog com3\r\n",
|
|
|
|
// Base com Survey-In (tempo + acurácia)
|
|
$"mode base {base_id} time {tempo_fixacao} {distancia_min}\r\n",
|
|
|
|
// RTCM perfil (comece leve; ative mais constelações se o LoRa aguentar)
|
|
$"RTCM1006 {porta_saida} 10\r\n",
|
|
$"RTCM1033 {porta_saida} 30\r\n",
|
|
$"RTCM1074 {porta_saida} 1\r\n", // GPS MSM4
|
|
$"RTCM1124 {porta_saida} 1\r\n", // BeiDou MSM4
|
|
|
|
// (Opcional) ativar mais constelações:
|
|
$"RTCM1094 {porta_saida} 1\r\n", // Galileo MSM4
|
|
$"RTCM1084 {porta_saida} 1\r\n", // GLONASS MSM4
|
|
//$"RTCM1230 {porta_saida} 10\r\n",// Bias GLONASS (OBRIGATÓRIO se 1084 estiver ativo)
|
|
|
|
// NMEA mínimo para debug na USB
|
|
$"gngga {porta_usb} 1\r\n",
|
|
|
|
$"saveconfig\r\n",
|
|
};
|
|
|
|
await Task.Delay(2000);
|
|
|
|
foreach (string cmd in comandos)
|
|
{
|
|
byte[] bytes = Encoding.ASCII.GetBytes(cmd);
|
|
PortaGPS.Write(bytes, 0, bytes.Length);
|
|
await Task.Delay(500);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private static StringBuilder _buffer = new StringBuilder();
|
|
|
|
private static void PortaGPS_DataReceived(object sender, SerialDataReceivedEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
if (!PortaGPS.IsOpen)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Lê os dados disponíveis na porta
|
|
string recebido = PortaGPS.ReadExisting();
|
|
|
|
//Console.WriteLine(recebido);
|
|
|
|
// Adiciona ao buffer
|
|
_buffer.Append(recebido);
|
|
|
|
// Processa mensagens completas (separadas por "\n")
|
|
string[] mensagens = _buffer.ToString().Split('\n');
|
|
|
|
// Processa todas as mensagens completas
|
|
for (int i = 0; i < mensagens.Length - 1; i++)
|
|
{
|
|
string mensagem = mensagens[i].Trim();
|
|
if (mensagem.Length > 0)
|
|
{
|
|
ProcessarDadosNMEA(mensagem);
|
|
}
|
|
}
|
|
|
|
if (mensagens.Length > 0)
|
|
{
|
|
// Mantém o que sobrou no buffer (última mensagem incompleta)
|
|
_buffer.Clear();
|
|
_buffer.Append(mensagens[mensagens.Length - 1]);
|
|
}
|
|
|
|
var obj = UltimaLeitura.Clone();
|
|
obj.Momento = DateTime.Now;
|
|
|
|
Logs.Add(JsonConvert.SerializeObject(obj));
|
|
VariaveisOperacao.RegistrarLogDispositivo(Logs, T_Code.Gps, ".json", 300);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Erro ao processar dados da porta serial: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private static void ProcessarDadosNMEA(string nmeaData)
|
|
{
|
|
DateTime Agora = DateTime.Now;
|
|
var linhas = nmeaData.Split('\n');
|
|
foreach (var linha in linhas)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(linha)) continue;
|
|
|
|
var sentenca = linha.Trim();
|
|
//Console.WriteLine(sentenca);
|
|
|
|
// Identifica o tipo de sentença
|
|
|
|
// GPS Antigo
|
|
if (sentenca.StartsWith("$GPGGA"))
|
|
{
|
|
ProcessarGPGGA(sentenca);
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
// Coordenadas
|
|
else if (sentenca.StartsWith("$GNGGA") || sentenca.StartsWith("$GLGGA"))
|
|
{
|
|
ProcessarGNGGA(sentenca);
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
// Variação magnética
|
|
else if (sentenca.StartsWith("$GNRMC"))
|
|
{
|
|
ProcessarGNRMC(sentenca);
|
|
}
|
|
// Curso verdadeiro
|
|
else if (sentenca.StartsWith("$GNVTG") || sentenca.StartsWith("$GPVTG"))
|
|
{
|
|
ProcessarGNVTG(sentenca);
|
|
}
|
|
// Satélites em vista
|
|
else if (sentenca.StartsWith("$GPGSV") || sentenca.StartsWith("$GLGSV") || sentenca.StartsWith("$GBGSV") || sentenca.StartsWith("$GAGSV"))
|
|
{
|
|
ProcessarGSV(sentenca);
|
|
}
|
|
// Orientação real
|
|
else if (sentenca.StartsWith("$GNTHS") || sentenca.StartsWith("$GPTHS") || sentenca.StartsWith("$GATHS"))
|
|
{
|
|
ProcessarGNTHS(sentenca);
|
|
//AtualizarCoordenadasGPS();
|
|
}
|
|
// GLL (GP/GN/GL/GA/BD)
|
|
else if (sentenca.Length > 6 && sentenca[3] == 'G' && sentenca[4] == 'L' && sentenca[5] == 'L')
|
|
{
|
|
//ProcessarGNGLL(sentenca);
|
|
}
|
|
// GSA (GP/GN/GL/GA/BD)
|
|
else if (sentenca.Length > 6 && sentenca[3] == 'G' && sentenca[4] == 'S' && sentenca[5] == 'A')
|
|
{
|
|
ProcessarGxGSA(sentenca);
|
|
}
|
|
else if (sentenca.Length > 6 && sentenca[3] == 'R' && sentenca[4] == 'M' && sentenca[5] == 'C') // RMC
|
|
{
|
|
ProcessarGxRMC(sentenca);
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"Sentença desconhecida: {sentenca}");
|
|
}
|
|
}
|
|
|
|
PenultimaLeitura.UltimoComandoRespondido = UltimaLeitura.UltimoComandoRespondido;
|
|
UltimaLeitura.UltimoComandoRespondido = Agora;
|
|
}
|
|
|
|
private static void ProcessarGPGGA(string sentenca)
|
|
{
|
|
string[] parts = sentenca.Split(',');
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
|
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
|
|
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
|
PenultimaLeitura.NumeroSatelites = UltimaLeitura.NumeroSatelites;
|
|
|
|
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
if (parts[2] != "" && parts[3] != "")
|
|
{
|
|
UltimaLeitura.Latitude = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2);
|
|
}
|
|
if (parts[4] != "" && parts[5] != "")
|
|
{
|
|
UltimaLeitura.Longitude = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3);
|
|
}
|
|
UltimaLeitura.NumeroSatelites = int.TryParse(parts[7], out int numSat) ? numSat : 0;
|
|
UltimaLeitura.PrecisaoHorizontal = double.TryParse(parts[8], NumberStyles.Float, CultureInfo.InvariantCulture, out double hdop) ? hdop : 0;
|
|
UltimaLeitura.Altitude = double.TryParse(parts[9], NumberStyles.Float, CultureInfo.InvariantCulture, out double alt) ? alt : 0;
|
|
}
|
|
|
|
private static void ProcessarGNGGA(string sentenca)
|
|
{
|
|
var ci = CultureInfo.InvariantCulture;
|
|
var campos = sentenca.Split(',');
|
|
|
|
string horaUTC = campos.Length > 1 ? campos[1] : "";
|
|
string latitudeRaw = campos.Length > 2 ? campos[2] : "";
|
|
string hemisferioLat = campos.Length > 3 ? campos[3] : "";
|
|
string longitudeRaw = campos.Length > 4 ? campos[4] : "";
|
|
string hemisferioLon = campos.Length > 5 ? campos[5] : "";
|
|
string qualidade = campos.Length > 6 ? campos[6] : "0";
|
|
string satelitesUsados = campos.Length > 7 ? campos[7] : "0";
|
|
string hdop = campos.Length > 8 ? campos[8] : "99.9";
|
|
string altitudeRaw = campos.Length > 9 ? campos[9] : "0";
|
|
string geoidSepRaw = campos.Length > 11 ? campos[11] : "0";
|
|
string idadeCorrecaoRaw = campos.Length > 13 ? campos[13] : "";
|
|
string base_id = campos.Length > 14 ? campos[14] : "";
|
|
|
|
// Conversão de Latitude (ddmm.mmmm)
|
|
double latitude = 0;
|
|
if (!string.IsNullOrEmpty(latitudeRaw))
|
|
{
|
|
// lat tem 2 dígitos de graus
|
|
var deg = double.Parse(latitudeRaw.Substring(0, 2), ci);
|
|
var min = double.Parse(latitudeRaw.Substring(2), ci);
|
|
latitude = deg + (min / 60.0);
|
|
if (hemisferioLat.Equals("S", StringComparison.OrdinalIgnoreCase)) latitude *= -1;
|
|
}
|
|
|
|
|
|
// Conversão de Longitude (dddmm.mmmm)
|
|
double longitude = 0;
|
|
if (!string.IsNullOrEmpty(longitudeRaw))
|
|
{
|
|
// lon tem 3 dígitos de graus
|
|
var deg = double.Parse(longitudeRaw.Substring(0, 3), ci);
|
|
var min = double.Parse(longitudeRaw.Substring(3), ci);
|
|
longitude = deg + (min / 60.0);
|
|
if (hemisferioLon.Equals("W", StringComparison.OrdinalIgnoreCase)) longitude *= -1;
|
|
}
|
|
|
|
// Altitude MSL (campo 9)
|
|
double altMSL = 0;
|
|
double.TryParse(altitudeRaw, NumberStyles.Float, ci, out altMSL);
|
|
|
|
// Geoid separation (campo 11)
|
|
double geoidSep = 0;
|
|
double.TryParse(geoidSepRaw, NumberStyles.Float, ci, out geoidSep);
|
|
|
|
// Altura elipsoidal = MSL + geoid separation
|
|
double altElipsoidal = altMSL + geoidSep;
|
|
|
|
// HDOP (adimensional)
|
|
double.TryParse(hdop, NumberStyles.Float, ci, out double hdopVal);
|
|
|
|
int.TryParse(satelitesUsados, out int nsatelites);
|
|
int.TryParse(qualidade, out int fixCode);
|
|
double idadeCorrecao = -1;
|
|
if (!string.IsNullOrWhiteSpace(idadeCorrecaoRaw))
|
|
double.TryParse(idadeCorrecaoRaw, NumberStyles.Float, ci, out idadeCorrecao);
|
|
|
|
|
|
//Console.WriteLine($"GNGGA: Hora={horaUTC}, Latitude={latitude}, Longitude={longitude}, Qualidade={qualidade}, Satélites={satelitesUsados}, HDOP={hdop}, Altitude={altitude}");
|
|
|
|
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
|
PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal;
|
|
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
|
|
PenultimaLeitura.NumeroSatelites = UltimaLeitura.NumeroSatelites;
|
|
PenultimaLeitura.QualidadeFix = UltimaLeitura.QualidadeFix;
|
|
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
|
PenultimaLeitura.IdadeCorrecao = UltimaLeitura.IdadeCorrecao;
|
|
PenultimaLeitura.BaseID = UltimaLeitura.BaseID;
|
|
|
|
// Armazenar os valores na última leitura
|
|
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
UltimaLeitura.Latitude = latitude;
|
|
UltimaLeitura.Longitude = longitude;
|
|
UltimaLeitura.Altitude = altMSL;
|
|
UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
|
|
UltimaLeitura.PrecisaoHorizontal = hdopVal;
|
|
UltimaLeitura.NumeroSatelites = nsatelites;
|
|
UltimaLeitura.QualidadeFix = (TiposCorrecaoGPS)fixCode;
|
|
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
|
|
UltimaLeitura.BaseID = base_id;
|
|
|
|
// Hora UTC no formato HHmmss.ss (fração opcional)
|
|
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
|
|
{
|
|
// Pega HHmmss e, se houver, fração:
|
|
var hh = int.Parse(horaUTC.Substring(0, 2), ci);
|
|
var mm = int.Parse(horaUTC.Substring(2, 2), ci);
|
|
var ssStr = horaUTC.Substring(4); // "ss" ou "ss.ss"
|
|
double ss = double.Parse(ssStr, ci);
|
|
var ts = new TimeSpan(0, hh, mm, (int)Math.Floor(ss), (int)Math.Round((ss - Math.Floor(ss)) * 1000.0));
|
|
var currentDateUtc = DateTime.UtcNow.Date;
|
|
UltimaLeitura.DataHora = currentDateUtc.Add(ts).ToLocalTime();
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNRMC(string sentenca)
|
|
{
|
|
var campos = sentenca.Split(',');
|
|
|
|
string horaUTC = campos[1].Replace(".", ",");
|
|
string status = campos[2].Replace(".", ",");
|
|
string latitudeRaw = campos[3].Replace(".", ",");
|
|
string hemisferioLat = campos[4].Replace(".", ",");
|
|
string longitudeRaw = campos[5].Replace(".", ",");
|
|
string hemisferioLon = campos[6].Replace(".", ",");
|
|
string velocidadeSobreSolo = campos[7].Replace(".", ",");
|
|
string curso = campos[8].Replace(".", ",");
|
|
string data = campos[9].Replace(".", ",");
|
|
string variaçãoMagnetica = campos[10].Replace(".", ",");
|
|
|
|
double.TryParse(variaçãoMagnetica, out double variacaoMag);
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.VariacaoMagnetica = UltimaLeitura.VariacaoMagnetica;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
UltimaLeitura.VariacaoMagnetica = variacaoMag;
|
|
|
|
//Console.WriteLine($"GNRMC: Hora={horaUTC}, Status={status}, Latitude={latitudeRaw}{hemisferioLat}, Longitude={longitudeRaw}{hemisferioLon}, Velocidade={velocidadeSobreSolo}, Curso={curso}, Data={data}, Variação Magnética={variaçãoMagnetica}");
|
|
}
|
|
|
|
private static void ProcessarGNVTG(string sentenca)
|
|
{
|
|
var campos = sentenca.Split(',');
|
|
|
|
string cursoVerdadeiro = campos[1].Replace(".", ",");
|
|
string referenciaCurso = campos[2].Replace(".", ","); // T = Verdadeiro, M = Magnético
|
|
string velocidadeSobreSoloKnots = campos[5].Replace(".", ",");
|
|
string velocidadeSobreSoloKmh = campos[7].Replace(".", ",");
|
|
|
|
//Console.WriteLine($"GNVTG: Curso Verdadeiro={cursoVerdadeiro}{referenciaCurso}, Velocidade (nós)={velocidadeSobreSoloKnots}, Velocidade (km/h)={velocidadeSobreSoloKmh}");
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.CursoVerdadeiro = UltimaLeitura.CursoVerdadeiro;
|
|
PenultimaLeitura.Velocidade = UltimaLeitura.Velocidade;
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
double.TryParse(cursoVerdadeiro, out double curso);
|
|
UltimaLeitura.CursoVerdadeiro = curso;
|
|
|
|
double.TryParse(velocidadeSobreSoloKmh, out double velocidade);
|
|
UltimaLeitura.Velocidade = velocidade;
|
|
}
|
|
|
|
private static void ProcessarGSV(string sentenca)
|
|
{
|
|
var campos = sentenca.Split(',');
|
|
|
|
string tipoSistema = sentenca.Substring(1, 2); // GP = GPS, GL = GLONASS, etc.
|
|
string totalSentencas = campos[1];
|
|
string sentencaAtual = campos[2];
|
|
string satelitesVisiveis = campos[3];
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.SatelitesEmVista = new List<GPSSatelitesEmVistaModel>(UltimaLeitura.SatelitesEmVista);
|
|
|
|
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
|
|
int.TryParse(sentencaAtual, out int sentAtual);
|
|
int.TryParse(totalSentencas, out int sentTotal);
|
|
int.TryParse(satelitesVisiveis, out int visiveis);
|
|
|
|
if (!UltimaLeitura.SatelitesEmVista.Any(x => x.TipoSistema == tipoSistema))
|
|
{
|
|
UltimaLeitura.SatelitesEmVista.Add(new GPSSatelitesEmVistaModel()
|
|
{
|
|
TipoSistema = tipoSistema,
|
|
Sentencas = new List<GPSSatelitesEmVistaSentencaModel>()
|
|
});
|
|
}
|
|
|
|
var leitura = UltimaLeitura.SatelitesEmVista.First(x => x.TipoSistema == tipoSistema);
|
|
|
|
//Console.WriteLine($"GSV: Sistema={tipoSistema}, Sentença {sentencaAtual}/{totalSentencas}, Satélites Visíveis={satelitesVisiveis}");
|
|
|
|
if (!leitura.Sentencas.Any(x => x.SentencaAtual == sentAtual))
|
|
{
|
|
leitura.Sentencas.Add(new GPSSatelitesEmVistaSentencaModel()
|
|
{
|
|
SentencaAtual = sentAtual,
|
|
SentencasTotal = sentTotal,
|
|
QuantidadeSatelites = visiveis,
|
|
Dados = new List<GPSSatelitesEmVistaDadosModel>()
|
|
});
|
|
}
|
|
|
|
var _sentenca = leitura.Sentencas.First(x => x.SentencaAtual == sentAtual);
|
|
|
|
_sentenca.Dados = new List<GPSSatelitesEmVistaDadosModel>();
|
|
|
|
for (int i = 4; i < campos.Length; i += 4)
|
|
{
|
|
if (i + 3 < campos.Length)
|
|
{
|
|
string prn = campos[i].Replace(".", ",");
|
|
string elevacaoRaw = campos[i + 1].Replace(".", ",");
|
|
string azimuteRaw = campos[i + 2].Replace(".", ",");
|
|
string snrRaw = campos[i + 3].Replace(".", ",");
|
|
|
|
//Console.WriteLine($" Satélite PRN={prn}, Elevação={elevacaoRaw}, Azimute={azimuteRaw}, SNR={snrRaw}");
|
|
|
|
double.TryParse(elevacaoRaw, out double elevacao);
|
|
double.TryParse(azimuteRaw, out double azimute);
|
|
double.TryParse(snrRaw, out double snr);
|
|
|
|
_sentenca.Dados.Add(new GPSSatelitesEmVistaDadosModel()
|
|
{
|
|
PRN = prn,
|
|
Elevacao = elevacao,
|
|
Azimute = azimute,
|
|
QualidadeSinal = snr
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNTHS(string sentenca)
|
|
{
|
|
//Console.WriteLine(sentenca);
|
|
// Remove o caractere de início '$' e divide os campos
|
|
var campos = sentenca.TrimStart('$').Split(',');
|
|
|
|
if (campos.Length < 2)
|
|
{
|
|
Console.WriteLine("Sentença incompleta.");
|
|
return;
|
|
}
|
|
|
|
if (campos.Length > 2 && string.IsNullOrEmpty(campos[1]))
|
|
{
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
// Parsing dos campos
|
|
double headingTrue = double.Parse(campos[1].Replace(".", ",")); // Campo <Heading>
|
|
string status = campos[2].Split('*')[0]; // Campo <Status>
|
|
string checksum = campos[2].Split('*')[1]; // Campo <Checksum>
|
|
|
|
PenultimaLeitura.TimestampOri = UltimaLeitura.TimestampOri.Clone();
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.OrientacaoReal = UltimaLeitura.OrientacaoReal;
|
|
PenultimaLeitura.TipoOrientacao = UltimaLeitura.TipoOrientacao;
|
|
|
|
UltimaLeitura.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
UltimaLeitura.OrientacaoReal = headingTrue;
|
|
UltimaLeitura.TipoOrientacao = status;
|
|
|
|
// ✅ Atualiza o ângulo unificado no GPSService
|
|
DefinirAnguloCarro();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"Erro ao processar a sentença: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNGLL(string sentenca)
|
|
{
|
|
// Aceita GP/GL/GN… qualquer “G?GLL”
|
|
var campos = sentenca.Split(',');
|
|
if (campos.Length < 7) return;
|
|
|
|
// lat/lon
|
|
string latRaw = campos[1];
|
|
string latHem = campos[2];
|
|
string lonRaw = campos[3];
|
|
string lonHem = campos[4];
|
|
string horaUTC = campos[5]; // hhmmss.ss
|
|
string status = campos[6]; // A/V
|
|
string mode = campos.Length > 7 ? campos[7].Split('*')[0] : ""; // pode não existir
|
|
|
|
bool valido = status == "A" && mode != "N";
|
|
|
|
if (valido && !string.IsNullOrWhiteSpace(latRaw) && !string.IsNullOrWhiteSpace(lonRaw))
|
|
{
|
|
double lat = GPSUtils.DmmToDecimal(latRaw, 2);
|
|
double lon = GPSUtils.DmmToDecimal(lonRaw, 3);
|
|
if (!double.IsInfinity(lat) && !double.IsNaN(lat) && !double.IsInfinity(lon) && !double.IsNaN(lon))
|
|
{
|
|
if (latHem == "S") lat = -lat;
|
|
if (lonHem == "W") lon = -lon;
|
|
|
|
// mantém histórico
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
|
|
UltimaLeitura.Latitude = lat;
|
|
UltimaLeitura.Longitude = lon;
|
|
}
|
|
}
|
|
|
|
// atualiza hora se veio no frame
|
|
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
|
|
{
|
|
// hhmmss(.ss)
|
|
var hh = horaUTC.Substring(0, 2);
|
|
var mm = horaUTC.Substring(2, 2);
|
|
var ss = horaUTC.Substring(4);
|
|
if (TimeSpan.TryParseExact($"{hh}:{mm}:{ss}", @"hh\:mm\:ss\.ff",
|
|
CultureInfo.InvariantCulture, out var tod)
|
|
|| TimeSpan.TryParseExact($"{hh}:{mm}:{ss}", @"hh\:mm\:ss",
|
|
CultureInfo.InvariantCulture, out tod))
|
|
{
|
|
UltimaLeitura.DataHora = DateTime.UtcNow.Date.Add(tod).ToLocalTime();
|
|
}
|
|
}
|
|
|
|
// mapeia “mode” (se existir) para sua enum, sem sobrepor GGA melhor
|
|
// Ex.: A=Autonomous(1), D=DGPS(2), R=RTK Fix(4), F=RTK Float(5)
|
|
if (!string.IsNullOrEmpty(mode))
|
|
{
|
|
if (mode == "R") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFixo;
|
|
else if (mode == "F") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFlutuante;
|
|
else if (mode == "D") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DGPS;
|
|
else if (mode == "E") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DeadReckoing;
|
|
else if (mode == "A") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.Autonomo;
|
|
else if (mode == "N") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
|
else UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
|
}
|
|
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
|
|
private static void ProcessarGxGSA(string sentenca)
|
|
{
|
|
// aceita $GPGSA, $GNGSA, $GLGSA, $GAGSA, $BDGSA…
|
|
var campos = sentenca.Split(',');
|
|
if (campos.Length < 17) return;
|
|
|
|
string modoSelecao = campos[1]; // M/A
|
|
TiposDimensaoCorrecaoGPS modoSolucao =(TiposDimensaoCorrecaoGPS)GPSUtils.ParseInt(campos[2]); // 1/2/3
|
|
// satélites usados: campos[3]..campos[14]
|
|
int satsUsados = 0;
|
|
for (int i = 3; i <= 14 && i < campos.Length; i++)
|
|
if (!string.IsNullOrWhiteSpace(campos[i])) satsUsados++;
|
|
|
|
double pdop = GPSUtils.ParseDouble(campos[15]);
|
|
double hdop = GPSUtils.ParseDouble(campos[16]);
|
|
double vdop = (campos.Length > 17) ? GPSUtils.ParseDouble(campos[17].Split('*')[0]) : double.NaN;
|
|
|
|
// Atualiza apenas o que faz sentido complementar
|
|
if (!double.IsInfinity(hdop) && !double.IsNaN(hdop)) UltimaLeitura.PrecisaoHorizontal = hdop;
|
|
if (satsUsados > 0) UltimaLeitura.NumeroSatelites = Math.Max(UltimaLeitura.NumeroSatelites, satsUsados);
|
|
|
|
// Se você quiser guardar os DOPs:
|
|
UltimaLeitura.PDOP = !double.IsInfinity(pdop) && !double.IsNaN(pdop) ? pdop : UltimaLeitura.PDOP;
|
|
UltimaLeitura.VDOP = !double.IsInfinity(vdop) && !double.IsNaN(vdop) ? vdop : UltimaLeitura.VDOP;
|
|
|
|
// Mapeia modoSolucao (não é igual ao “fix quality” do GGA!)
|
|
// 1=NoFix, 2=Fix2D, 3=Fix3D — pode guardar num campo próprio se tiver
|
|
UltimaLeitura.FixDimensao = modoSolucao; // crie int FixDimensao na sua struct, se não existir
|
|
}
|
|
|
|
private static void ProcessarGxRMC(string sentenca)
|
|
{
|
|
// Aceita $GPRMC, $GNRMC, $GLRMC, $GARMC, $BDRMC...
|
|
var raw = sentenca;
|
|
var campos = raw.Split(',');
|
|
|
|
if (campos.Length < 12) return;
|
|
|
|
string timeUTC = campos[1]; // hhmmss.ss
|
|
string status = campos[2]; // A=ativo, V=inválido
|
|
string latRaw = campos[3];
|
|
string latHem = campos[4];
|
|
string lonRaw = campos[5];
|
|
string lonHem = campos[6];
|
|
string spdKtsS = campos[7]; // knots
|
|
string cogS = campos[8]; // course over ground (graus)
|
|
string date = campos[9]; // ddmmyy
|
|
string magVarS = campos[10]; // pode estar vazio
|
|
string magHem = campos.Length > 11 ? campos[11] : "";
|
|
// mode pode vir no campo 12 (sem checksum) ou 12 com outro e 13 com checksum, depende do firmware
|
|
string mode = "";
|
|
if (campos.Length > 12)
|
|
{
|
|
var tmp = campos[12];
|
|
// tira checksum se estiver grudado
|
|
int asterix = tmp.IndexOf('*');
|
|
mode = (asterix >= 0 ? tmp.Substring(0, asterix) : tmp).Trim();
|
|
// alguns mandam mais um campo (navStatus) e o checksum só no final
|
|
if (mode.Length == 0 && campos.Length > 13)
|
|
{
|
|
tmp = campos[13];
|
|
asterix = tmp.IndexOf('*');
|
|
mode = (asterix >= 0 ? tmp.Substring(0, asterix) : tmp).Trim();
|
|
}
|
|
}
|
|
|
|
bool valido = status == "A";
|
|
|
|
// Converte lat/lon se válidos
|
|
if (valido && !string.IsNullOrWhiteSpace(latRaw) && !string.IsNullOrWhiteSpace(lonRaw))
|
|
{
|
|
if (latRaw.Length >= 4 && lonRaw.Length >= 5)
|
|
{
|
|
double lat = GPSUtils.DmmToDecimal(latRaw, 2);
|
|
double lon = GPSUtils.DmmToDecimal(lonRaw, 3);
|
|
if (!double.IsNaN(lat) && !double.IsInfinity(lat) && !double.IsNaN(lon) && !double.IsInfinity(lon))
|
|
{
|
|
if (latHem == "S") lat = -lat;
|
|
if (lonHem == "W") lon = -lon;
|
|
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
UltimaLeitura.Latitude = lat;
|
|
UltimaLeitura.Longitude = lon;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Velocidade (knots -> m/s e km/h, se quiser guardar)
|
|
if (double.TryParse(spdKtsS, NumberStyles.Float, CultureInfo.InvariantCulture, out double spdKts))
|
|
{
|
|
UltimaLeitura.Velocidade = spdKts;
|
|
}
|
|
|
|
// Course over ground (graus)
|
|
if (double.TryParse(cogS, NumberStyles.Float, CultureInfo.InvariantCulture, out double cog))
|
|
UltimaLeitura.CursoVerdadeiro = cog;
|
|
|
|
// Data/hora (UTC)
|
|
// timeUTC: hhmmss(.ss), date: ddmmyy
|
|
DateTime? dt = null;
|
|
if (!string.IsNullOrEmpty(timeUTC) && timeUTC.Length >= 6 && !string.IsNullOrEmpty(date) && date.Length == 6)
|
|
{
|
|
string hh = timeUTC.Substring(0, 2);
|
|
string mm = timeUTC.Substring(2, 2);
|
|
string ss = timeUTC.Substring(4, 2);
|
|
|
|
string dd = date.Substring(0, 2);
|
|
string MM = date.Substring(2, 2);
|
|
string yy = date.Substring(4, 2);
|
|
|
|
// yy -> 20yy (assumindo 2000+; ajuste se precisar 19xx)
|
|
int year = 2000 + int.Parse(yy, CultureInfo.InvariantCulture);
|
|
if (int.TryParse(dd, out int d) && int.TryParse(MM, out int M) &&
|
|
int.TryParse(hh, out int H) && int.TryParse(mm, out int m) && int.TryParse(ss, out int s))
|
|
{
|
|
try
|
|
{
|
|
dt = new DateTime(year, M, d, H, m, s, DateTimeKind.Utc);
|
|
}
|
|
catch { /* ignora datas inválidas */ }
|
|
}
|
|
}
|
|
if (dt.HasValue) UltimaLeitura.DataHora = dt.Value.ToLocalTime();
|
|
|
|
// Variação magnética (se quiser armazenar)
|
|
if (double.TryParse(magVarS, NumberStyles.Float, CultureInfo.InvariantCulture, out double magVar))
|
|
{
|
|
if (magHem == "W") magVar = -magVar;
|
|
UltimaLeitura.VariacaoMagnetica = magVar;
|
|
}
|
|
|
|
// Mode → promove QualidadeFix (não rebaixa)
|
|
// A=Autônomo, D=DGPS, R=RTK Fix, F=RTK Float, N=No Fix
|
|
if (!string.IsNullOrEmpty(mode))
|
|
{
|
|
if (mode == "R") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFixo;
|
|
else if (mode == "F") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.RTKFlutuante;
|
|
else if (mode == "D") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DGPS;
|
|
else if (mode == "E") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.DeadReckoing;
|
|
else if (mode == "A") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.Autonomo;
|
|
else if (mode == "N") UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
|
else UltimaLeitura.QualidadeFix = TiposCorrecaoGPS.SemCorrecao;
|
|
}
|
|
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
|
|
|
|
private static async Task AplicarCorrecaoRTK_Ntrip()
|
|
{
|
|
if (!Iniciado || LoopRTK_Ntrip || !APIService.HasInternet)
|
|
return;
|
|
|
|
LoopRTK_Ntrip = true;
|
|
|
|
// Configurações do NTRIP caster para RTK2Go
|
|
string host = "gps-ntrip.ibge.gov.br";
|
|
int port = 2101;
|
|
string mountpoint = "EESC0";
|
|
string username = "Zendion"; // Geralmente vazio para RTK2Go
|
|
string password = "vyNEF$5*"; // Geralmente vazio para RTK2Go
|
|
|
|
while (CorrecaoRTK_Ntrip && APIService.HasInternet) // Loop para reconectar em caso de falha
|
|
{
|
|
try
|
|
{
|
|
// Construa o cabeçalho da solicitação
|
|
string credentials = string.IsNullOrEmpty(username)
|
|
? ""
|
|
: Convert.ToBase64String(Encoding.ASCII.GetBytes($"{username}:{password}"));
|
|
|
|
string request = $"GET /{mountpoint} HTTP/1.0\r\n" +
|
|
$"User-Agent: NTRIP Client/2.0\r\n" +
|
|
$"Accept: */*\r\n" +
|
|
$"Connection: keep-alive\r\n" +
|
|
(!string.IsNullOrEmpty(credentials) ? $"Authorization: Basic {credentials}\r\n" : "") +
|
|
"\r\n";
|
|
|
|
// Estabeleça a conexão
|
|
using (TcpClient client = new TcpClient(host, port))
|
|
using (NetworkStream stream = client.GetStream())
|
|
using (StreamWriter writer = new StreamWriter(stream, Encoding.ASCII))
|
|
{
|
|
writer.Write(request);
|
|
writer.Flush();
|
|
|
|
// Leia a resposta
|
|
using (StreamReader reader = new StreamReader(stream, Encoding.ASCII))
|
|
{
|
|
string response = await reader.ReadLineAsync();
|
|
if (CorrecaoRTK_Ntrip)
|
|
{
|
|
if ((response ?? "").Contains("200 OK"))
|
|
{
|
|
Console.WriteLine("Conexão bem-sucedida ao mountpoint!");
|
|
|
|
byte[] buffer = new byte[4096];
|
|
int bytesRead;
|
|
while (CorrecaoRTK_Ntrip && (bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length)) > 0)
|
|
{
|
|
if (PortaGPS?.IsOpen ?? false)
|
|
{
|
|
PortaGPS.Write(buffer, 0, bytesRead); // Envia os dados RTCM para o GPS
|
|
//Console.WriteLine($"Enviando {bytesRead} bytes de correção RTCM para o GPS");
|
|
UltimoEnvioCorrecaoRTK = DateTime.Now;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"Falha na conexão com NTRIP: {response}");
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"Erro na conexão RTK: {ex.Message}");
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|
}
|
|
}
|
|
|
|
LoopRTK_Ntrip = false;
|
|
}
|
|
|
|
|
|
|
|
public static void AtualizarCoordenadasGPS()
|
|
{
|
|
PenultimaLeitura.Ntrip_ativado = UltimaLeitura.Ntrip_ativado;
|
|
PenultimaLeitura.Heartbeat = UltimaLeitura.Heartbeat;
|
|
|
|
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
|
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
|
|
|
if (Variaveis.IsAgroMonitor)
|
|
{
|
|
//EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, false, LoRaBaseService.parametrosModel.address);
|
|
return;
|
|
}
|
|
|
|
DefinirAnguloCarroGPS();
|
|
|
|
AtualizaDadosRedis();
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Iniciado)
|
|
{
|
|
var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;
|
|
CorredorTrajetoriaModel CorredorAtual = _Trajetoria?.CorredorAtual;
|
|
var _GPSTrajetoria = Variaveis.OperacaoEmAndamento.GPSTrajetoria;
|
|
|
|
// ✅ Criando uma cópia eficiente de UltimaLeitura sem copiar manualmente cada propriedade
|
|
var novaLeitura = UltimaLeitura.Clone();
|
|
|
|
// ✅ Adiciona a nova leitura à trajetória GPS
|
|
_GPSTrajetoria.Add(novaLeitura);
|
|
|
|
// ✅ Calcula a distância entre os dois últimos pontos, mas só se houver pelo menos 2 pontos
|
|
if (_GPSTrajetoria.Count > 1)
|
|
{
|
|
int ultimo = _GPSTrajetoria.Count - 1;
|
|
int penultimo = _GPSTrajetoria.Count - 2;
|
|
|
|
double distancia = GPSUtils.DistanciaEntrePontos(
|
|
_GPSTrajetoria[ultimo], // Último ponto
|
|
_GPSTrajetoria[penultimo] // Penúltimo ponto
|
|
);
|
|
|
|
if (CorredorAtual != null)
|
|
{
|
|
CorredorAtual.DistanciaPercorridaTotal += distancia;
|
|
if (_Trajetoria.CorredorAtual.Dentro)
|
|
{
|
|
CorredorAtual.DistanciaPercorridaCorredor += distancia;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
Variaveis.OperacaoEmAndamento.Trajetoria?.LoopAtualizaDados();
|
|
|
|
AtualizarTrajetoriaDinamica();
|
|
|
|
int EnderecoEquipamento = Convert.ToInt32(Variaveis.LoraService?.ParametrosGet?.address ?? Variaveis.LoraService?.ParametrosSet?.address ?? 0x00);
|
|
EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, Variaveis.OperacaoEmAndamento.Iniciado, EnderecoEquipamento);
|
|
|
|
if (Variaveis.LoraService.Iniciado && Variaveis.LoraService._ultimoRxDados > DateTime.UtcNow.AddSeconds(-60))
|
|
{
|
|
byte EnderecoBase = Variaveis.LoraBaseParametros.address;
|
|
EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.OrientacaoReal, false, EnderecoBase);
|
|
}
|
|
|
|
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
|
UltimaLeitura.Inicializado = Iniciado;
|
|
|
|
if (CorrecaoRTK_Ntrip && UltimoEnvioCorrecaoRTK.AddSeconds(TempoMin_Ntrip) < DateTime.Now)
|
|
{
|
|
UltimoEnvioCorrecaoRTK = DateTime.Now;
|
|
LoopRTK_Ntrip = false;
|
|
Task.Run(async () =>
|
|
{
|
|
await Task.Delay(5000);
|
|
await AplicarCorrecaoRTK_Ntrip();
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
|
|
public static void EnviarCoordenadasParaMapa(double Latitude, double Longitude, double Orientacao, bool EmFoco, int ID)
|
|
{
|
|
var Coordenadas = new
|
|
{
|
|
latitude = Latitude,
|
|
longitude = Longitude,
|
|
orientacao = GPSUtils.NormalizarAngulo(Orientacao - 180.0),
|
|
id = ID,
|
|
foco = EmFoco
|
|
};
|
|
Task.Run(async () =>
|
|
{
|
|
//Console.WriteLine(JsonConvert.SerializeObject(Coordenadas));
|
|
await Variaveis.MqttService.PublishAsync(
|
|
Variaveis.MqttService.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoCoordenadasGPS),
|
|
JsonConvert.SerializeObject(Coordenadas)
|
|
);
|
|
});
|
|
}
|
|
|
|
public static void AtualizarTrajetoriaDinamica()
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Trajetoria?._TrajetoriaDinamica?.Any() ?? false)
|
|
{
|
|
var Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria.TrajetoriaDinamica.Select(x => new
|
|
{
|
|
latitude = x.Latitude,
|
|
longitude = x.Longitude
|
|
}).ToArray();
|
|
|
|
Task.Run(async () =>
|
|
{
|
|
await Variaveis.MqttService.PublishAsync(
|
|
Variaveis.MqttService.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoTrajetoriaDinamica),
|
|
JsonConvert.SerializeObject(Trajetoria)
|
|
);
|
|
});
|
|
}
|
|
}
|
|
|
|
public static void AtualizarRuasSelecionadas(List<string> RuasSelecionadas)
|
|
{
|
|
Task.Run(async () =>
|
|
{
|
|
await Variaveis.MqttService.PublishAsync(
|
|
Variaveis.MqttService.Topicos.First(x => x.Topico == MapasVariaveisModel.TopicoSelecaoRuasMapa),
|
|
JsonConvert.SerializeObject("[" + string.Join(",", RuasSelecionadas.ToArray()) + "]"),
|
|
true
|
|
);
|
|
});
|
|
}
|
|
|
|
private static void DefinirAnguloCarroGPS()
|
|
{
|
|
int PontosConsiderarAngulo = 2;
|
|
double Angulo = 0.0;
|
|
double DistAngulo = 0.0;
|
|
|
|
try
|
|
{
|
|
var GPSTrajetoria = Variaveis.OperacaoEmAndamento.GPSTrajetoria;
|
|
|
|
// ✅ Se a trajetória estiver vazia, não há o que calcular
|
|
if (GPSTrajetoria == null || GPSTrajetoria.Count == 0)
|
|
{
|
|
Angulo = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
|
|
DistAngulo = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
|
|
}
|
|
else
|
|
{
|
|
// ✅ Obtém os últimos pontos, respeitando o número mínimo de leituras
|
|
var pontos = GPSTrajetoria
|
|
.OrderByDescending(x => x.Momento)
|
|
.ThenByDescending(x => x.DataHora)
|
|
.Take(Math.Min(PontosConsiderarAngulo, GPSTrajetoria.Count))
|
|
.ToList();
|
|
|
|
// ✅ Se houver apenas um ponto, adicionamos a última leitura do GPS
|
|
if (pontos.Count == 1)
|
|
{
|
|
pontos.Insert(0, UltimaLeitura);
|
|
}
|
|
|
|
// ✅ Ordena os pontos do mais antigo para o mais recente (somente uma vez)
|
|
pontos.Reverse();
|
|
|
|
// ✅ Calcula o ângulo médio e a distância total
|
|
double somaAngulo = 0;
|
|
for (int i = 0; i < pontos.Count - 1; i++)
|
|
{
|
|
somaAngulo += GPSUtils.CalcularOrientacao(pontos[i], pontos[i + 1]);
|
|
DistAngulo += GPSUtils.DistanciaEntrePontos(pontos[i], pontos[i + 1]);
|
|
}
|
|
double mediaAngulo = somaAngulo / (pontos.Count - 1);
|
|
|
|
// ✅ Corrige valores inválidos
|
|
Angulo = double.IsNaN(mediaAngulo) ? 0.0 : mediaAngulo;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
Angulo = 0.0;
|
|
DistAngulo = 0.0;
|
|
}
|
|
|
|
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
|
|
|
UltimaLeitura.OrientacaoMovimento = Angulo;
|
|
UltimaLeitura.Distancia = DistAngulo;
|
|
|
|
DefinirAnguloCarro();
|
|
}
|
|
|
|
public static void DefinirAnguloCarro()
|
|
{
|
|
double anguloFinal = 0.0;
|
|
|
|
bool imuIniciado = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU?.Iniciado ?? false;
|
|
bool gpsIniciado = Iniciado;
|
|
|
|
if (imuIniciado)
|
|
{
|
|
//Variaveis.OperacaoEmAndamento.DispSen.Dados?.DadosLeitura?.SensoresIMU?.FirstOrDefault()?.AtualizarOffset(UltimaLeitura);
|
|
}
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Simulando)
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoReal;
|
|
}
|
|
else if (gpsIniciado)
|
|
{
|
|
// O GPS está disponível
|
|
//anguloFinal = UltimaLeitura.OrientacaoReal;
|
|
|
|
double anguloGPS = UltimaLeitura.OrientacaoReal;
|
|
double anguloGPSMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
double distAnguloGPS = UltimaLeitura.Distancia;
|
|
|
|
//anguloFinal = CalcularAnguloUnificado(anguloGPS, UltimaLeitura.QualidadeFix, anguloGPSMovimento, distAnguloGPS);
|
|
//anguloFinal = GPSUtils.MisturarAngulosPorDistancia(anguloGPS, anguloGPSMovimento, distAnguloGPS, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras, 0.03, 0.3);
|
|
|
|
anguloFinal = anguloGPS;
|
|
}
|
|
else if (imuIniciado)
|
|
{
|
|
// Apenas o WT901C está disponível
|
|
anguloFinal = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU.RotacaoCorrigida;
|
|
}
|
|
else
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoMovimento;
|
|
}
|
|
|
|
// Atualiza o ângulo final
|
|
UltimaLeitura.AnguloCarroDefinido = anguloFinal;
|
|
}
|
|
|
|
public static double CalcularAnguloUnificado(double orientacaoReal, TiposCorrecaoGPS precisaoGPS, double anguloMovimento, double distanciaMovimento, double? anguloWT901C = null)
|
|
{
|
|
// Pesos para cada fonte de dados
|
|
double pesoGPS = 0.0;
|
|
double pesoWT901C = 0.0;
|
|
double pesoMovimento = 0.0;
|
|
|
|
// Definir pesos com base na precisão do GPS
|
|
switch (precisaoGPS)
|
|
{
|
|
/*case TiposCorrecaoGPS.RTKFixo:
|
|
pesoGPS = 1.0;
|
|
pesoWT901C = 0.0;
|
|
pesoMovimento = 0.0;
|
|
break;
|
|
|
|
case TiposCorrecaoGPS.RTKFlutuante:
|
|
pesoGPS = 0.8;
|
|
pesoWT901C = anguloWT901C.HasValue ? 0.2 : 0.0;
|
|
pesoMovimento = Math.Min(0.2, distanciaMovimento / 10.0);
|
|
break;*/
|
|
|
|
case TiposCorrecaoGPS.RTKFixo:
|
|
case TiposCorrecaoGPS.RTKFlutuante:
|
|
// Definir limites para o peso do ângulo do UM982
|
|
double minPesoMovimento = 0.0; // 0%
|
|
double maxPesoMovimento = 1.0; // 95%
|
|
double distanciaMin = 0.05; // Distância média quando o robô anda devagar
|
|
double distanciaMax = 1.0; // Distância máxima razoável para interpolação
|
|
|
|
// Cálculo do peso baseado na distância percorrida
|
|
pesoMovimento = minPesoMovimento + (maxPesoMovimento - minPesoMovimento) * Math.Min(1.0, Math.Max(0.0, (distanciaMovimento - distanciaMin) / (distanciaMax - distanciaMin)));
|
|
|
|
// O restante do peso vai para a orientação real do UM982
|
|
pesoGPS = 1.0 - pesoMovimento;
|
|
|
|
pesoWT901C = 0.0;
|
|
break;
|
|
|
|
case TiposCorrecaoGPS.DGPS:
|
|
pesoGPS = 0.5;
|
|
pesoWT901C = anguloWT901C.HasValue ? 0.4 : 0.0;
|
|
pesoMovimento = Math.Min(0.5, distanciaMovimento / 5.0);
|
|
break;
|
|
|
|
case TiposCorrecaoGPS.Autonomo:
|
|
default:
|
|
pesoGPS = 0.3;
|
|
pesoWT901C = anguloWT901C.HasValue ? 0.5 : 0.0;
|
|
pesoMovimento = Math.Min(0.8, distanciaMovimento / 3.0);
|
|
break;
|
|
}
|
|
|
|
// ⚠️ Ajuste quando WT901C estiver indisponível:
|
|
if (!anguloWT901C.HasValue)
|
|
{
|
|
// **Redistribuir o peso do WT901C para GPS e Movimento**
|
|
pesoGPS += pesoWT901C / 2.0;
|
|
pesoMovimento += pesoWT901C / 2.0;
|
|
pesoWT901C = 0.0; // WT901C não influencia se for `null`
|
|
}
|
|
|
|
// Normalizar os pesos para garantir que sempre somem 1
|
|
double somaPesos = pesoGPS + pesoWT901C + pesoMovimento;
|
|
pesoGPS /= somaPesos;
|
|
pesoWT901C /= somaPesos;
|
|
pesoMovimento /= somaPesos;
|
|
|
|
// Calcular o ângulo unificado
|
|
double anguloUnificado = (orientacaoReal * pesoGPS) + (anguloMovimento * pesoMovimento);
|
|
if (anguloWT901C.HasValue)
|
|
{
|
|
anguloUnificado += (anguloWT901C.Value * pesoWT901C);
|
|
}
|
|
|
|
// Ajustar para espaço circular (0 a 360 graus)
|
|
anguloUnificado = GPSUtils.NormalizarAngulo(anguloUnificado);
|
|
|
|
return anguloUnificado;
|
|
}
|
|
|
|
|
|
public static void AtualizaDadosRedis()
|
|
{
|
|
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Simulando ? historicoPosicao.Peek() : UltimaLeitura;
|
|
|
|
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", posicaoAtual.IdadeCorrecao < rtk_timeout),
|
|
("hAcc", posicaoAtual.PrecisaoCm),
|
|
("nSatelites", posicaoAtual.NumeroSatelites),
|
|
("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),
|
|
("age", posicaoAtual.IdadeCorrecao),
|
|
("heartbeat", posicaoAtual.Heartbeat)
|
|
);
|
|
}
|
|
|
|
public static Queue<GPSModel> historicoPosicao = new Queue<GPSModel>();
|
|
public static void AtualizarAtrasoPosicoes(int errosConsiderar = 0)
|
|
{
|
|
historicoPosicao.Enqueue(UltimaLeitura);
|
|
while (historicoPosicao.Count > 1 && (historicoPosicao.Count > errosConsiderar || errosConsiderar == 0))
|
|
{
|
|
historicoPosicao.Dequeue();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
public class GgaFix
|
|
{
|
|
public DateTime TsUtc { get; }
|
|
public double LatDeg { get; }
|
|
public double LonDeg { get; }
|
|
public double AltElipsoidalM { get; }
|
|
public TiposCorrecaoGPS FixQuality { get; }
|
|
|
|
public GgaFix(DateTime tsUtc, double latDeg, double lonDeg, double altElipsoidalM, TiposCorrecaoGPS fixQuality)
|
|
{
|
|
TsUtc = tsUtc;
|
|
LatDeg = latDeg;
|
|
LonDeg = lonDeg;
|
|
AltElipsoidalM = altElipsoidalM;
|
|
FixQuality = fixQuality;
|
|
}
|
|
}
|
|
|
|
public static class BaseFixService
|
|
{
|
|
private static int _lastReadHeartbeat = -1;
|
|
public static List<GgaFix> amostras_pos = new List<GgaFix>(1000);
|
|
private static DateTime? inicioProcesso = null;
|
|
private static DateTime? inicioFix = null;
|
|
private static DateTime? fimProcesso = null;
|
|
private static int segundosFixEstavel = 120;
|
|
private static int maxJanelaSegundos = 120;
|
|
public static double Progresso
|
|
{
|
|
get
|
|
{
|
|
double progresso = inicioFix is null ? 0 : (DateTime.UtcNow - inicioFix.Value).TotalSeconds / segundosFixEstavel * 100.0;
|
|
return progresso;
|
|
}
|
|
}
|
|
public static double ProgressoGeral
|
|
{
|
|
get
|
|
{
|
|
double progresso = inicioProcesso is null ? 0 : (DateTime.UtcNow - inicioProcesso.Value).TotalSeconds / maxJanelaSegundos * 100.0;
|
|
return progresso;
|
|
}
|
|
}
|
|
public static string ProgressoStr
|
|
{
|
|
get
|
|
{
|
|
string progresso = "";
|
|
if (CorrecaoEmAndamento)
|
|
{
|
|
progresso = $"Recebendo correção RTK via Ntrip. Progresso geral: {ProgressoGeral.ToString("0.00")}%, Progresso correção: {Progresso.ToString("0.00")}%";
|
|
}
|
|
else if (CorrecaoAbsoluta && inicioProcesso.HasValue && fimProcesso.HasValue)
|
|
{
|
|
progresso = $"Correção absoluta concluída com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds.ToString("0.00")} segundos";
|
|
}
|
|
else if (inicioProcesso.HasValue && fimProcesso.HasValue)
|
|
{
|
|
progresso = $"Correção absoluta falhou com {amostras_pos.Count} amostras em {(fimProcesso.Value - inicioProcesso.Value).TotalSeconds.ToString("0.00")} segundos";
|
|
}
|
|
else
|
|
{
|
|
progresso = $"Correção absoluta não realizada";
|
|
}
|
|
return progresso;
|
|
}
|
|
}
|
|
public static bool CorrecaoAbsoluta = false;
|
|
public static bool CorrecaoEmAndamento = false;
|
|
|
|
// ===== 1) Função principal =====
|
|
public static async Task<bool> FixarBaseViaNtripAsync(string portaUsb = "com3", string portaEntrada = "com2", string portaSaida = "com2", string baseId = "957", int segsFixEstavel = 120, int maxJanelaSegs = 600, double madK = 3.5, Func<Task> startNtrip = null, Func<Task> stopNtrip = null)
|
|
{
|
|
if (CorrecaoEmAndamento)
|
|
return false;
|
|
|
|
CorrecaoEmAndamento = true;
|
|
fimProcesso = null;
|
|
|
|
segundosFixEstavel = segsFixEstavel;
|
|
maxJanelaSegundos = maxJanelaSegs;
|
|
|
|
// 1.1 Config temporária como rover parado + NMEA
|
|
await ConfigurarComoRoverParadoAsync(portaUsb, portaEntrada);
|
|
|
|
// 1.2 Ligar NTRIP (injeta RTCM na portaEntrada)
|
|
if (startNtrip != null) await startNtrip();
|
|
|
|
try
|
|
{
|
|
// 2) Esperar FIX sustentado e coletar GNGGA
|
|
var amostras = await EsperarFixEAmostrarAsync();
|
|
|
|
if (amostras.Count < 10)
|
|
{
|
|
Console.WriteLine("Poucas amostras de RTK FIX coletadas. Tente aumentar o tempo ou verificar sinais.");
|
|
CorrecaoAbsoluta = false;
|
|
return CorrecaoAbsoluta;
|
|
}
|
|
|
|
// 3) Filtro robusto (MAD) + média final
|
|
var (lat, lon, h, nAmostras) = FiltrarEAgrupar(amostras, madK);
|
|
|
|
// 4) Alternar para base FIX + perfil RTCM
|
|
await AplicarBaseFixAsync(portaUsb, portaSaida, baseId, lat, lon, h);
|
|
|
|
Console.WriteLine($"[BASE/FIX] Coordenadas aplicadas (n={nAmostras}):");
|
|
Console.WriteLine($" lat = {lat:0.000000000}, lon = {lon:0.000000000}, h = {h:0.000}");
|
|
CorrecaoAbsoluta = true;
|
|
return CorrecaoAbsoluta;
|
|
}
|
|
finally
|
|
{
|
|
if (stopNtrip != null) await stopNtrip();
|
|
fimProcesso = DateTime.UtcNow;
|
|
CorrecaoEmAndamento = false;
|
|
}
|
|
}
|
|
|
|
// ===== 1.1 Rover parado + NMEA + limpar logs =====
|
|
private static async Task ConfigurarComoRoverParadoAsync(string portaUsb, string portaEntrada)
|
|
{
|
|
Console.WriteLine("Configurando base como modo rover parado...");
|
|
string freq = "1.0";
|
|
string[] cmds = {
|
|
// Ajuste de bauds
|
|
$"config {portaUsb} 115200\r\n",
|
|
$"config {portaEntrada} 115200\r\n",
|
|
|
|
// Limpa logs
|
|
$"unlog com1\r\n",
|
|
$"unlog com2\r\n",
|
|
$"unlog com3\r\n",
|
|
|
|
// Rover parado (vamos usar NTRIP p/ obter FIX)
|
|
$"mode rover uav\r\n",
|
|
|
|
// NMEA na USB
|
|
$"gngga {portaUsb} {freq}\r\n",
|
|
$"gpths {portaUsb} {freq}\r\n",
|
|
|
|
$"saveconfig\r\n"
|
|
};
|
|
|
|
await Task.Delay(1000);
|
|
foreach (var c in cmds)
|
|
{
|
|
var b = Encoding.ASCII.GetBytes(c);
|
|
PortaGPS.Write(b, 0, b.Length);
|
|
await Task.Delay(250);
|
|
}
|
|
}
|
|
|
|
// ===== 2) Coleta GNGGA com FIX sustentado =====
|
|
private static async Task<List<GgaFix>> EsperarFixEAmostrarAsync()
|
|
{
|
|
Console.WriteLine("Inciando coleta de dados...");
|
|
|
|
amostras_pos = new List<GgaFix>(1000);
|
|
inicioProcesso = DateTime.UtcNow;
|
|
inicioFix = null;
|
|
|
|
// Você já deve ter um leitor da COM que devolve linhas NMEA.
|
|
// Abaixo, vamos supor um método async que lê GGA parseado.
|
|
while (ProgressoGeral < 100)
|
|
{
|
|
// Lê próxima sentença (bloqueante/assíncrono)
|
|
var gga = await LerProximoGgaAsync(); // implemente no seu stack
|
|
|
|
if (gga is null) continue;
|
|
|
|
// Considera "RTK FIX" como qualidade válida
|
|
if (!new List<TiposCorrecaoGPS>() { TiposCorrecaoGPS.RTKFixo }.Contains(gga.FixQuality))
|
|
{
|
|
inicioFix = null; // reset
|
|
continue;
|
|
}
|
|
|
|
// Marca início da janela de FIX estável
|
|
if (inicioFix is null)
|
|
{
|
|
Console.WriteLine("RTK Fixo definido! Iniciando coleta de dados com precisão...");
|
|
inicioFix = DateTime.UtcNow;
|
|
}
|
|
|
|
amostras_pos.Add(gga);
|
|
Console.WriteLine("Nova coordenada registrada!");
|
|
|
|
// Verifica se já temos FIX estável pelo período necessário
|
|
if (Progresso >= 100)
|
|
break;
|
|
}
|
|
|
|
return amostras_pos;
|
|
}
|
|
|
|
// ===== 2.1) Ajuste a assinatura se quiser passar timeout e CT de fora
|
|
private static async Task<GgaFix> LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default)
|
|
{
|
|
var sw = System.Diagnostics.Stopwatch.StartNew();
|
|
int startHb = System.Threading.Volatile.Read(ref _lastReadHeartbeat);
|
|
|
|
// 1) Espera um novo heartbeat
|
|
while (!ct.IsCancellationRequested)
|
|
{
|
|
int currentHb = UltimaLeitura.Heartbeat; // <- leitura normal da propriedade
|
|
if (currentHb != startHb) break;
|
|
|
|
if (sw.ElapsedMilliseconds >= timeoutMs)
|
|
//throw new TimeoutException("Timeout aguardando nova leitura GGA.");
|
|
return new GgaFix(DateTime.UtcNow, 0, 0, 0, TiposCorrecaoGPS.SemCorrecao);
|
|
|
|
await Task.Delay(75, ct).ConfigureAwait(false);
|
|
}
|
|
ct.ThrowIfCancellationRequested();
|
|
|
|
// 2) Snapshot consistente
|
|
while (true)
|
|
{
|
|
int hbBefore = UltimaLeitura.Heartbeat;
|
|
|
|
// Captura TODOS os campos que você precisa em variáveis locais
|
|
DateTime tsUtc = UltimaLeitura.DataHora.ToUniversalTime();
|
|
double lat = UltimaLeitura.Latitude;
|
|
double lon = UltimaLeitura.Longitude;
|
|
double altElips = UltimaLeitura.AltitudeElipsoidal; // garanta que já é elipsoidal no parser
|
|
var fixQual = UltimaLeitura.QualidadeFix; // enum? ok.
|
|
|
|
int hbAfter = UltimaLeitura.Heartbeat;
|
|
|
|
// Se o heartbeat não mudou durante o snapshot, temos dados coerentes
|
|
if (hbBefore == hbAfter)
|
|
{
|
|
// marca como lido
|
|
System.Threading.Volatile.Write(ref _lastReadHeartbeat, hbAfter);
|
|
|
|
// monta o DTO
|
|
return new GgaFix(
|
|
tsUtc: tsUtc,
|
|
latDeg: lat,
|
|
lonDeg: lon,
|
|
altElipsoidalM: altElips,
|
|
fixQuality: fixQual
|
|
);
|
|
}
|
|
|
|
// caso contrário, alguém atualizou no meio — tenta de novo rápido
|
|
await Task.Yield();
|
|
}
|
|
}
|
|
|
|
// ===== 3) Filtro robusto (MAD) + média =====
|
|
private static (double lat, double lon, double h, int n) FiltrarEAgrupar(List<GgaFix> amostras, double madK = 3.5)
|
|
{
|
|
Console.WriteLine("Filtrando dados aferidos...");
|
|
// Medianas
|
|
var lats = amostras.Select(a => a.LatDeg).OrderBy(x => x).ToArray();
|
|
var lons = amostras.Select(a => a.LonDeg).OrderBy(x => x).ToArray();
|
|
var hs = amostras.Select(a => a.AltElipsoidalM).OrderBy(x => x).ToArray();
|
|
|
|
double medLat = Mediana(lats);
|
|
double medLon = Mediana(lons);
|
|
double medH = Mediana(hs);
|
|
|
|
// Desvios absolutos da mediana (MAD)
|
|
var dLat = amostras.Select(a => Math.Abs(a.LatDeg - medLat)).OrderBy(x => x).ToArray();
|
|
var dLon = amostras.Select(a => Math.Abs(a.LonDeg - medLon)).OrderBy(x => x).ToArray();
|
|
var dH = amostras.Select(a => Math.Abs(a.AltElipsoidalM - medH)).OrderBy(x => x).ToArray();
|
|
|
|
double madLat = Mediana(dLat) + 1e-12;
|
|
double madLon = Mediana(dLon) + 1e-12;
|
|
double madHgt = Mediana(dH) + 1e-12;
|
|
|
|
// Filtra outliers (|x - med| / MAD <= madK)
|
|
var filtradas = amostras.Where(a =>
|
|
(Math.Abs(a.LatDeg - medLat) / madLat) <= madK &&
|
|
(Math.Abs(a.LonDeg - medLon) / madLon) <= madK &&
|
|
(Math.Abs(a.AltElipsoidalM - medH) / madHgt) <= madK
|
|
).ToList();
|
|
|
|
// Média final
|
|
double lat = filtradas.Average(a => a.LatDeg);
|
|
double lon = filtradas.Average(a => a.LonDeg);
|
|
double h = filtradas.Average(a => a.AltElipsoidalM);
|
|
|
|
return (lat, lon, h, filtradas.Count);
|
|
|
|
double Mediana(double[] arr)
|
|
{
|
|
int n = arr.Length;
|
|
if (n == 0) return double.NaN;
|
|
return (n % 2 == 1) ? arr[n / 2] : 0.5 * (arr[n / 2 - 1] + arr[n / 2]);
|
|
}
|
|
}
|
|
|
|
// ===== 4) Aplicar base FIX + RTCM + save =====
|
|
private static async Task AplicarBaseFixAsync(string portaUsb, string portaSaida, string baseId, double latDeg, double lonDeg, double hEllipsM)
|
|
{
|
|
Console.WriteLine("Aplicando dados de correção...");
|
|
var ci = System.Globalization.CultureInfo.InvariantCulture;
|
|
// Desliga logs antes de trocar modo
|
|
string[] pre = {
|
|
$"unlog com1\r\n",
|
|
$"unlog com2\r\n",
|
|
$"unlog com3\r\n"
|
|
};
|
|
foreach (var c in pre) { PortaGPS.Write(Encoding.ASCII.GetBytes(c), 0, c.Length); await Task.Delay(150); }
|
|
|
|
var latStr = latDeg.ToString("0.000000000", ci);
|
|
var lonStr = lonDeg.ToString("0.000000000", ci);
|
|
var hStr = hEllipsM.ToString("0.000", ci);
|
|
|
|
var fix = Encoding.ASCII.GetBytes($"mode base {baseId} {latStr} {lonStr} {hStr}\r\n");
|
|
PortaGPS.Write(fix, 0, fix.Length);
|
|
await Task.Delay(250);
|
|
|
|
// Reativar RTCM no canal de saída para o LoRa
|
|
string[] rtcmCmds = {
|
|
// RTCM perfil (comece leve; ative mais constelações se o LoRa aguentar)
|
|
$"RTCM1006 {portaSaida} 10\r\n",
|
|
$"RTCM1033 {portaSaida} 30\r\n",
|
|
$"RTCM1074 {portaSaida} 1\r\n", // GPS MSM4
|
|
$"RTCM1124 {portaSaida} 1\r\n", // BeiDou MSM4
|
|
|
|
// (Opcional) ativar mais constelações:
|
|
$"RTCM1094 {portaSaida} 1\r\n", // Galileo MSM4
|
|
$"RTCM1084 {portaSaida} 1\r\n", // GLONASS MSM4
|
|
//$"RTCM1230 {portaSaida} 10\r\n",// Bias GLONASS (OBRIGATÓRIO se 1084 estiver ativo)
|
|
};
|
|
|
|
foreach (var c in rtcmCmds) { var b = Encoding.ASCII.GetBytes(c); PortaGPS.Write(b, 0, b.Length); await Task.Delay(200); }
|
|
|
|
// NMEA mínimo na USB p/ debug
|
|
var nmea = $"gngga {portaUsb} 1\r\n";
|
|
PortaGPS.Write(Encoding.ASCII.GetBytes(nmea), 0, nmea.Length);
|
|
await Task.Delay(150);
|
|
|
|
// Persistir
|
|
var save = "saveconfig\r\n";
|
|
PortaGPS.Write(Encoding.ASCII.GetBytes(save), 0, save.Length);
|
|
}
|
|
|
|
}
|
|
|
|
}
|