831 lines
33 KiB
C#
831 lines
33 KiB
C#
using AgroBase.Models;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.IO.Ports;
|
|
using System.Linq;
|
|
using System.Net.Sockets;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class GPSService
|
|
{
|
|
public static SerialPort PortaGPS = null;
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
if (PortaGPS != null && !PortaGPS.IsOpen)
|
|
{
|
|
try
|
|
{
|
|
PortaGPS.Open();
|
|
}
|
|
catch
|
|
{
|
|
PortaGPS = null;
|
|
}
|
|
}
|
|
return PortaGPS != null && PortaGPS.IsOpen;
|
|
}
|
|
}
|
|
|
|
public static GPSModel UltimaLeitura = new GPSModel();
|
|
public static GPSModel PenultimaLeitura = new GPSModel();
|
|
|
|
public static int TaxaAmostragemHz { get; set; } = 5;
|
|
|
|
public static bool CorrecaoRTK = true;
|
|
|
|
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 AplicarCorrecaoRTK());
|
|
}
|
|
}
|
|
|
|
private static void DefinirDispositivo()
|
|
{
|
|
if (Iniciado)
|
|
{
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Endereco = PortaGPS.PortName,
|
|
Dispositivo = T_Code.Gps,
|
|
Erro = !Iniciado,
|
|
Versao = "1",
|
|
});
|
|
}
|
|
}
|
|
|
|
private static void ConfigurarModulo()
|
|
{
|
|
|
|
}
|
|
|
|
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();
|
|
|
|
// 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]);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Erro ao processar dados da porta serial: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private static void ProcessarDadosNMEA(string nmeaData)
|
|
{
|
|
var linhas = nmeaData.Split('\n');
|
|
foreach (var linha in linhas)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(linha)) continue;
|
|
|
|
var sentenca = linha.Trim();
|
|
|
|
// Identifica o tipo de sentença
|
|
if (sentenca.StartsWith("$GNGGA"))
|
|
{
|
|
ProcessarGNGGA(sentenca);
|
|
AtualizarCoordenadasGPS();
|
|
}
|
|
else if (sentenca.StartsWith("$GNRMC"))
|
|
{
|
|
ProcessarGNRMC(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GNVTG"))
|
|
{
|
|
ProcessarGNVTG(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GPGSV") || sentenca.StartsWith("$GLGSV") || sentenca.StartsWith("$GBGSV") || sentenca.StartsWith("$GAGSV"))
|
|
{
|
|
ProcessarGSV(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GPVTG"))
|
|
{
|
|
ProcessarGPVTG(sentenca);
|
|
}
|
|
else if (sentenca.StartsWith("$GNTHS") || sentenca.StartsWith("$GPTHS"))
|
|
{
|
|
ProcessarGNTHS(sentenca);
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"Sentença desconhecida: {sentenca}");
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNGGA(string sentenca)
|
|
{
|
|
var campos = sentenca.Split(',');
|
|
|
|
string horaUTC = campos[1].Replace(".", ",");
|
|
string latitudeRaw = campos[2].Replace(".", ",");
|
|
string hemisferioLat = campos[3].Replace(".", ",");
|
|
string longitudeRaw = campos[4].Replace(".", ",");
|
|
string hemisferioLon = campos[5].Replace(".", ",");
|
|
string qualidade = campos[6].Replace(".", ",");
|
|
string satelitesUsados = campos[7].Replace(".", ",");
|
|
string hdop = campos[8].Replace(".", ",");
|
|
string altitudeRaw = campos[9].Replace(".", ",");
|
|
|
|
// Conversão de Latitude
|
|
double latitude = 0;
|
|
if (!string.IsNullOrEmpty(latitudeRaw))
|
|
{
|
|
double latitudeGraus = double.Parse(latitudeRaw.Substring(0, 2));
|
|
double latitudeMinutos = double.Parse(latitudeRaw.Substring(2)) / 60.0;
|
|
latitude = latitudeGraus + latitudeMinutos;
|
|
if (hemisferioLat == "S") latitude *= -1;
|
|
}
|
|
|
|
|
|
// Conversão de Longitude
|
|
double longitude = 0;
|
|
if (!string.IsNullOrEmpty(longitudeRaw))
|
|
{
|
|
double longitudeGraus = double.Parse(longitudeRaw.Substring(0, 3));
|
|
double longitudeMinutos = double.Parse(longitudeRaw.Substring(3)) / 60.0;
|
|
longitude = longitudeGraus + longitudeMinutos;
|
|
if (hemisferioLon == "W") longitude *= -1;
|
|
}
|
|
|
|
double.TryParse(altitudeRaw, out double altitude);
|
|
|
|
double.TryParse(hdop, out double precisao);
|
|
|
|
int.TryParse(satelitesUsados, out int nsatelites);
|
|
|
|
int.TryParse(qualidade, out int fix);
|
|
|
|
|
|
//Console.WriteLine($"GNGGA: Hora={horaUTC}, Latitude={latitude}, Longitude={longitude}, Qualidade={qualidade}, Satélites={satelitesUsados}, HDOP={hdop}, Altitude={altitude}");
|
|
|
|
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
|
|
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
|
|
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
|
|
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
|
|
PenultimaLeitura.NumeroSatelites = UltimaLeitura.NumeroSatelites;
|
|
PenultimaLeitura.QualidadeFix = UltimaLeitura.QualidadeFix;
|
|
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
|
|
|
|
// Armazenar os valores na última leitura
|
|
UltimaLeitura.Momento = DateTime.Now;
|
|
UltimaLeitura.Latitude = latitude;
|
|
UltimaLeitura.Longitude = longitude;
|
|
UltimaLeitura.Altitude = altitude;
|
|
UltimaLeitura.PrecisaoHorizontal = precisao;
|
|
UltimaLeitura.NumeroSatelites = nsatelites;
|
|
UltimaLeitura.QualidadeFix = (TiposCorrecaoGPS)fix;
|
|
|
|
// Parse a hora do formato HHmmss.ss
|
|
if (!string.IsNullOrEmpty(horaUTC) && TimeSpan.TryParseExact(horaUTC.Substring(0, 6), "hhmmss", CultureInfo.InvariantCulture, out TimeSpan timeOfDay))
|
|
{
|
|
DateTime currentDate = DateTime.UtcNow.Date;
|
|
UltimaLeitura.DataHora = currentDate.Add(timeOfDay);
|
|
}
|
|
}
|
|
|
|
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 ProcessarGPVTG(string sentenca)
|
|
{
|
|
var campos = sentenca.Split(',');
|
|
|
|
if (campos.Length >= 9)
|
|
{
|
|
string cursoVerdadeiro = campos[1].Replace(".", ",");
|
|
string referenciaCurso = campos[2]; // T = Verdadeiro, M = Magnético
|
|
string velocidadeSobreSoloKnots = campos[5].Replace(".", ",");
|
|
string velocidadeSobreSoloKmh = campos[7].Replace(".", ",");
|
|
|
|
//Console.WriteLine($"GPVTG: 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 curso = 0;
|
|
double.TryParse(cursoVerdadeiro, out curso);
|
|
UltimaLeitura.CursoVerdadeiro = curso;
|
|
|
|
double velocidade = 0;
|
|
double.TryParse(velocidadeSobreSoloKmh, out velocidade);
|
|
UltimaLeitura.Velocidade = velocidade;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("GPVTG: Sentença malformada ou incompleta.");
|
|
}
|
|
}
|
|
|
|
private static void ProcessarGNTHS(string 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.Momento = UltimaLeitura.Momento;
|
|
PenultimaLeitura.OrientacaoReal = UltimaLeitura.OrientacaoReal;
|
|
PenultimaLeitura.TipoOrientacao = UltimaLeitura.TipoOrientacao;
|
|
|
|
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 async Task AplicarCorrecaoRTK()
|
|
{
|
|
// 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 = "Diego21*"; // Geralmente vazio para RTK2Go
|
|
|
|
while (true) // 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 (response.Contains("200 OK"))
|
|
{
|
|
Console.WriteLine("Conexão bem-sucedida ao mountpoint!");
|
|
|
|
byte[] buffer = new byte[4096];
|
|
int bytesRead;
|
|
while ((bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length)) > 0)
|
|
{
|
|
if (CorrecaoRTK)
|
|
{
|
|
PortaGPS.Write(buffer, 0, bytesRead); // Envia os dados RTCM para o GPS
|
|
//Console.WriteLine($"Enviando {bytesRead} bytes de correção RTCM para o GPS");
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"Falha na conexão: {response}");
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"Erro na conexão RTK: {ex.Message}");
|
|
await Task.Delay(5000); // Aguarde antes de tentar novamente
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
public static void AtualizarCoordenadasGPS()
|
|
{
|
|
if (Variaveis.IsAgroMonitor)
|
|
{
|
|
return;
|
|
}
|
|
|
|
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.DistanciaPercorrida += distancia;
|
|
}
|
|
}
|
|
}
|
|
|
|
DefinirAnguloCarroGPS();
|
|
|
|
Variaveis.OperacaoEmAndamento.Trajetoria?.LoopAtualizaDados();
|
|
|
|
AtualizarTrajetoriaDinamica();
|
|
|
|
int EnderecoEquipamento = Convert.ToInt32(Variaveis.ConexaoLoRa.parametros.address);
|
|
EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, Variaveis.OperacaoEmAndamento.Iniciado, EnderecoEquipamento);
|
|
|
|
if (VariaveisOperacao.PosicaoBase != null)
|
|
{
|
|
byte EnderecoBase = Variaveis.OperacaoEmAndamento.DispSen.Dados.LoRaParametrosBase.address;
|
|
EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.OrientacaoReal, false, EnderecoBase);
|
|
}
|
|
|
|
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
|
UltimaLeitura.Inicializado = Iniciado;
|
|
}
|
|
|
|
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 () =>
|
|
{
|
|
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(UltimaLeitura, PenultimaLeitura);
|
|
DistAngulo = GPSUtils.DistanciaEntrePontos(UltimaLeitura, PenultimaLeitura);
|
|
}
|
|
else
|
|
{
|
|
// ✅ Obtém os últimos pontos, respeitando o número mínimo de leituras
|
|
var pontos = GPSTrajetoria
|
|
.OrderByDescending(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 witIniciado = WT901CService.Iniciado;
|
|
bool gpsIniciado = Iniciado;
|
|
|
|
WT901CService.DadosLeitura.AtualizarOffset(UltimaLeitura);
|
|
|
|
/*if (witIniciado && gpsIniciado)
|
|
{
|
|
// Obtém os ângulos de ambos os sensores
|
|
double anguloWT901C = WT901CService.DadosLeitura.AnguloCorrigido;
|
|
double anguloGPS = UltimaLeitura.OrientacaoReal;
|
|
double anguloGPSMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
double distAnguloGPS = UltimaLeitura.Distancia;
|
|
|
|
anguloFinal = CalcularAnguloUnificado(anguloGPS, UltimaLeitura.QualidadeFix, anguloGPSMovimento, distAnguloGPS, anguloWT901C);
|
|
}
|
|
else if (witIniciado)
|
|
{
|
|
// Apenas o WT901C está disponível
|
|
anguloFinal = WT901CService.DadosLeitura.AnguloCorrigido;
|
|
}
|
|
else if (gpsIniciado)
|
|
{
|
|
// Apenas o GPS está disponível
|
|
double anguloGPS = UltimaLeitura.OrientacaoReal;
|
|
double anguloGPSMovimento = UltimaLeitura.OrientacaoMovimento;
|
|
double distAnguloGPS = UltimaLeitura.Distancia;
|
|
|
|
anguloFinal = CalcularAnguloUnificado(anguloGPS, UltimaLeitura.QualidadeFix, anguloGPSMovimento, distAnguloGPS);
|
|
}
|
|
else
|
|
{
|
|
anguloFinal = UltimaLeitura.OrientacaoMovimento;
|
|
}*/
|
|
|
|
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);
|
|
}
|
|
else if (witIniciado)
|
|
{
|
|
// Apenas o WT901C está disponível
|
|
anguloFinal = WT901CService.DadosLeitura.AnguloCorrigido;
|
|
}
|
|
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;
|
|
}
|
|
|
|
}
|
|
}
|