1295 lines
53 KiB
C#
1295 lines
53 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 double RaioDaTerra = 6378137; // Raio da Terra em Km
|
|
public static double ConversaoNosKmh = 1.852;
|
|
public static double ConversaoNosMs = 0.514;
|
|
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];
|
|
string elevacaoRaw = campos[i + 1];
|
|
string azimuteRaw = campos[i + 2];
|
|
string snrRaw = campos[i + 3];
|
|
|
|
//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]); // 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;
|
|
}
|
|
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
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
private static double ConvertToDecimalDegrees(string nmeaCoordinate, string direction, int digits)
|
|
{
|
|
// Divide a string em graus e minutos
|
|
int degrees = int.Parse(nmeaCoordinate.Substring(0, digits));
|
|
double minutes = double.Parse(nmeaCoordinate.Substring(digits), CultureInfo.InvariantCulture);
|
|
|
|
// Converte para graus decimais
|
|
double decimalDegrees = degrees + (minutes / 60);
|
|
|
|
// Se a direção for Sul ou Oeste, o valor deve ser negativo
|
|
if (direction == "S" || direction == "W")
|
|
{
|
|
decimalDegrees *= -1;
|
|
}
|
|
|
|
return decimalDegrees;
|
|
}
|
|
|
|
private static double ConvertToDouble(string value)
|
|
{
|
|
double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double result);
|
|
return result;
|
|
}
|
|
|
|
private static DateTime ParseDateTime(string date, string time, int timeZoneOffset)
|
|
{
|
|
// DDMMAA, HHMMSS.ss
|
|
string dateTimeFormat = "ddMMyyHHmmss.ff";
|
|
|
|
// Verifica se o formato de data e hora está completo
|
|
if (date.Length == 6 && (time.Length == 6 || time.Length == 9))
|
|
{
|
|
string dateTimeStr = date + time;
|
|
if (DateTime.TryParseExact(dateTimeStr, dateTimeFormat, CultureInfo.InvariantCulture, DateTimeStyles.None, out DateTime result))
|
|
{
|
|
// Ajusta o DateTime para o fuso horário especificado
|
|
result = result.AddHours(timeZoneOffset);
|
|
return result;
|
|
}
|
|
}
|
|
|
|
// Se a conversão falhar, retorne DateTime.MinValue ou lance uma exceção, conforme sua necessidade
|
|
return DateTime.MinValue;
|
|
}
|
|
|
|
private static double ConvertKnotsToKmh(double knots)
|
|
{
|
|
return knots * 1.852;
|
|
}
|
|
|
|
public static double CalcularOrientacao(GPSModel P1, GPSModel P2)
|
|
{
|
|
// Converter latitudes e longitudes de graus para radianos
|
|
double latA = P1.Latitude * (Math.PI / 180.0);
|
|
double lonA = P1.Longitude * (Math.PI / 180.0);
|
|
double latB = P2.Latitude * (Math.PI / 180.0);
|
|
double lonB = P2.Longitude * (Math.PI / 180.0);
|
|
|
|
// Calcular a diferença de longitude
|
|
double deltaLon = lonB - lonA;
|
|
|
|
// Calcular a direção
|
|
double y = Math.Sin(deltaLon) * Math.Cos(latB);
|
|
double x = Math.Cos(latA) * Math.Sin(latB) - Math.Sin(latA) * Math.Cos(latB) * Math.Cos(deltaLon);
|
|
double direcaoRadianos = Math.Atan2(y, x);
|
|
|
|
// Converter a direção de radianos para graus
|
|
double direcaoGraus = direcaoRadianos * (180.0 / Math.PI);
|
|
|
|
// Normalizar a direção para que esteja no intervalo de 0 a 360 graus
|
|
direcaoGraus = (direcaoGraus + 360) % 360;
|
|
|
|
return direcaoGraus;
|
|
}
|
|
|
|
public static double CalcularOrientacaoMagnetica(double magX, double magY)
|
|
{
|
|
double heading = Math.Atan2(magY, magX) * (180.0 / Math.PI);
|
|
if (heading < 0)
|
|
{
|
|
heading += 360.0;
|
|
}
|
|
return heading;
|
|
}
|
|
|
|
public static double CalcularOrientacaoMagneticaCompensada(double magX, double magY, double magZ, double accX, double accY, double accZ)
|
|
{
|
|
// Calcular os ângulos de inclinação
|
|
double roll = Math.Atan2(accY, accZ);
|
|
double pitch = Math.Atan2(-accX, Math.Sqrt(accY * accY + accZ * accZ));
|
|
|
|
// Compensar a inclinação
|
|
double magXComp = magX * Math.Cos(pitch) + magZ * Math.Sin(pitch);
|
|
double magYComp = magX * Math.Sin(roll) * Math.Sin(pitch) + magY * Math.Cos(roll) - magZ * Math.Sin(roll) * Math.Cos(pitch);
|
|
|
|
// Calcular a orientação magnética compensada
|
|
double heading = Math.Atan2(magYComp, magXComp) * (180.0 / Math.PI);
|
|
if (heading < 0)
|
|
{
|
|
heading += 360.0;
|
|
}
|
|
return heading;
|
|
}
|
|
|
|
public static double CalcularMenorDistanciaAteTrecho(GPSModel posicaoRobo, List<GPSModel> rua, double espacoEntrePontos = 0.2)
|
|
{
|
|
double menorDistancia = double.MaxValue;
|
|
|
|
for (int i = 0; i < rua.Count - 1; i++)
|
|
{
|
|
GPSModel pontoA = rua[i];
|
|
GPSModel pontoB = rua[i + 1];
|
|
|
|
// Comprimento do segmento em metros
|
|
double distanciaSegmento = DistanciaEntrePontos(pontoA, pontoB);
|
|
|
|
// Quantidade de pontos a interpolar
|
|
int numInterpolacoes = (int)(distanciaSegmento / espacoEntrePontos);
|
|
|
|
for (int j = 0; j <= numInterpolacoes; j++)
|
|
{
|
|
double fator = (double)j / numInterpolacoes;
|
|
GPSModel pontoInterpolado = InterpolarPonto(pontoA, pontoB, fator);
|
|
|
|
// Calcula a distância até o ponto interpolado
|
|
double distancia = DistanciaEntrePontos(posicaoRobo, pontoInterpolado);
|
|
menorDistancia = Math.Min(menorDistancia, distancia);
|
|
}
|
|
}
|
|
|
|
return menorDistancia;
|
|
}
|
|
|
|
public static double NormalizarAngulo(double angulo)
|
|
{
|
|
if (angulo < 0)
|
|
{
|
|
angulo += 360;
|
|
}
|
|
angulo %= 360;
|
|
return angulo;
|
|
}
|
|
|
|
public static double CalcularDiferencaAngulo(double anguloVariavel, double anguloRef)
|
|
{
|
|
// Calcula a diferença entre o ângulo variável e o ângulo de referência
|
|
double diferenca = anguloVariavel - anguloRef;
|
|
|
|
// Normaliza a diferença para estar entre -180 e 180 graus
|
|
diferenca = ((diferenca + 180) % 360) - 180;
|
|
|
|
// Se a diferença ainda estiver fora do intervalo, ajuste manualmente
|
|
if (diferenca > 180)
|
|
{
|
|
diferenca -= 360;
|
|
}
|
|
else if (diferenca < -180)
|
|
{
|
|
diferenca += 360;
|
|
}
|
|
|
|
return diferenca;
|
|
}
|
|
|
|
public static double DistanciaEntrePontos(GPSModel P1, GPSModel P2)
|
|
{
|
|
double lat1 = P1.Latitude;
|
|
double lat2 = P2.Latitude;
|
|
double lon1 = P1.Longitude;
|
|
double lon2 = P2.Longitude;
|
|
|
|
|
|
double dLat = ToRadians(lat2 - lat1);
|
|
double dLon = ToRadians(lon2 - lon1);
|
|
lat1 = ToRadians(lat1);
|
|
lat2 = ToRadians(lat2);
|
|
|
|
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
|
|
Math.Sin(dLon / 2) * Math.Sin(dLon / 2) * Math.Cos(lat1) * Math.Cos(lat2);
|
|
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
|
double distance = RaioDaTerra * c;
|
|
|
|
//distance = distance * 0.5;
|
|
|
|
return distance;
|
|
}
|
|
|
|
public static double DistanciaDoTrecho(List<GPSModel> Trecho)
|
|
{
|
|
double d = 0;
|
|
for (int i = 0; i < Trecho.Count - 1; i++)
|
|
{
|
|
double _d = DistanciaEntrePontos(Trecho[i], Trecho[i + 1]);
|
|
//Console.WriteLine($"{i} - {_d}");
|
|
d += _d;
|
|
}
|
|
return d;
|
|
}
|
|
|
|
public static bool CompararDirecaoTrajeto(List<GPSModel> Trecho1, List<GPSModel> Trecho2)
|
|
{
|
|
double MargemErroAngulo = 90;
|
|
double anguloTrecho1 = CalcularOrientacao(Trecho1[0], Trecho1[Trecho1.Count - 1]);
|
|
double anguloTrecho2 = CalcularOrientacao(Trecho2[0], Trecho2[Trecho2.Count - 1]);
|
|
|
|
if ((anguloTrecho1 + MargemErroAngulo) >= anguloTrecho2 && (anguloTrecho1 - MargemErroAngulo) <= anguloTrecho2)
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public static bool CompararDirecaoPontos(double anguloP1, double anguloP2)
|
|
{
|
|
double MargemErroAngulo = 45;
|
|
|
|
if ((anguloP1 + MargemErroAngulo) >= anguloP2 && (anguloP1 - MargemErroAngulo) <= anguloP2)
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public static GPSModel ProjetarPontoDeslocado(GPSModel pontoOriginal, double distancia, double angulo)
|
|
{
|
|
// Convertendo ângulo em radianos
|
|
double anguloRad = (Math.PI / 180) * angulo;
|
|
|
|
// Calculando deslocamento
|
|
double deltaLat = (distancia * Math.Cos(anguloRad)) / RaioDaTerra;
|
|
double deltaLon = (distancia * Math.Sin(anguloRad)) / (RaioDaTerra * Math.Cos(pontoOriginal.Latitude * Math.PI / 180));
|
|
|
|
// Convertendo deslocamento de radianos para graus
|
|
deltaLat = deltaLat * (180 / Math.PI);
|
|
deltaLon = deltaLon * (180 / Math.PI);
|
|
|
|
// Criando novo ponto com o deslocamento
|
|
GPSModel pontoDeslocado = new GPSModel
|
|
{
|
|
Latitude = pontoOriginal.Latitude + deltaLat,
|
|
Longitude = pontoOriginal.Longitude + deltaLon,
|
|
Altitude = pontoOriginal.Altitude,
|
|
DataHora = pontoOriginal.DataHora,
|
|
NumeroSatelites = pontoOriginal.NumeroSatelites,
|
|
PrecisaoHorizontal = pontoOriginal.PrecisaoHorizontal,
|
|
Velocidade = pontoOriginal.Velocidade
|
|
};
|
|
|
|
return pontoDeslocado;
|
|
}
|
|
|
|
private static double ToRadians(double angle)
|
|
{
|
|
return Math.PI / 180 * angle;
|
|
}
|
|
|
|
public static void AtualizarCoordenadasGPS()
|
|
{
|
|
if (Variaveis.IsAgroMonitor)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (VariaveisOperacao.PosicaoBase != null)
|
|
{
|
|
byte enderecoBase = Variaveis.OperacaoEmAndamento.DispSen.Dados.LoRaParametrosBase.address;
|
|
EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.CursoVerdadeiro, false, enderecoBase);
|
|
}
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Iniciado)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoria.Add(new GPSModel()
|
|
{
|
|
NumeroSatelites = UltimaLeitura.NumeroSatelites,
|
|
Altitude = UltimaLeitura.Altitude,
|
|
DataHora = UltimaLeitura.DataHora,
|
|
Latitude = UltimaLeitura.Latitude,
|
|
Longitude = UltimaLeitura.Longitude,
|
|
PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal,
|
|
Velocidade = UltimaLeitura.Velocidade
|
|
});
|
|
int idxCorredor = Variaveis.OperacaoEmAndamento.Trajetoria.IdxCorredorAtual;
|
|
if (Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count() - 1 < idxCorredor)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Add(new List<GPSModel>());
|
|
}
|
|
|
|
if (idxCorredor < Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count())
|
|
{
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[idxCorredor].Add(Variaveis.OperacaoEmAndamento.GPSTrajetoria[Variaveis.OperacaoEmAndamento.GPSTrajetoria.Count - 1]);
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Count - 1 < idxCorredor)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Add(0);
|
|
}
|
|
|
|
if (Variaveis.OperacaoEmAndamento.GPSTrajetoria.Count > 1 && Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Count() > idxCorredor)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida[idxCorredor] += DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.GPSTrajetoria[Variaveis.OperacaoEmAndamento.GPSTrajetoria.Count - 1], Variaveis.OperacaoEmAndamento.GPSTrajetoria[Variaveis.OperacaoEmAndamento.GPSTrajetoria.Count - 2]);
|
|
}
|
|
}
|
|
}
|
|
|
|
DefinirAnguloCarroGPS();
|
|
|
|
int ID = Convert.ToInt32(Variaveis.ConexaoLoRa.parametros.address);
|
|
double angulo = ((Variaveis.OperacaoEmAndamento.Sensoriamento?.AnguloCarro ?? 0) - 180);
|
|
EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, angulo, Variaveis.OperacaoEmAndamento.Iniciado, ID);
|
|
|
|
Variaveis.OperacaoEmAndamento.Trajetoria?.LoopAtualizaDados();
|
|
}
|
|
|
|
public static void EnviarCoordenadasParaMapa(double Latitude, double Longitude, double Orientacao, bool EmFoco, int ID)
|
|
{
|
|
var Coordenadas = new
|
|
{
|
|
latitude = Latitude,
|
|
longitude = Longitude,
|
|
orientacao = Orientacao,
|
|
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)
|
|
{
|
|
List<GPSModel> TJ = new List<GPSModel>(Variaveis.OperacaoEmAndamento.Trajetoria.TrajetoriaDinamica);
|
|
var Trajetoria = TJ.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
|
|
);
|
|
});
|
|
}
|
|
|
|
public static List<GPSModel> InterpolarPontos(GPSModel inicio, GPSModel destino, double distanciaPasso)
|
|
{
|
|
List<GPSModel> pontos = new List<GPSModel>();
|
|
var distanciaTotal = DistanciaEntrePontos(inicio, destino);
|
|
var passos = distanciaTotal / distanciaPasso;
|
|
for (int i = 0; i <= passos; i++)
|
|
{
|
|
var frac = i / passos;
|
|
var lat = inicio.Latitude + (destino.Latitude - inicio.Latitude) * frac;
|
|
var lon = inicio.Longitude + (destino.Longitude - inicio.Longitude) * frac;
|
|
pontos.Add(new GPSModel { Latitude = lat, Longitude = lon });
|
|
}
|
|
return pontos;
|
|
}
|
|
|
|
public static GPSModel InterpolarPonto(GPSModel pontoInicial, GPSModel pontoFinal, double t)
|
|
{
|
|
// Interpolar latitude e longitude
|
|
double latInterpolada = pontoInicial.Latitude + t * (pontoFinal.Latitude - pontoInicial.Latitude);
|
|
double lonInterpolada = pontoInicial.Longitude + t * (pontoFinal.Longitude - pontoInicial.Longitude);
|
|
|
|
// Interpolar orientação, se necessário
|
|
double orientacaoInterpolada = pontoInicial.CursoVerdadeiro + t * (pontoFinal.CursoVerdadeiro - pontoInicial.CursoVerdadeiro);
|
|
|
|
return new GPSModel
|
|
{
|
|
Latitude = latInterpolada,
|
|
Longitude = lonInterpolada,
|
|
CursoVerdadeiro = orientacaoInterpolada
|
|
};
|
|
}
|
|
|
|
public static List<GPSModel> InterpolarRota(List<GPSModel> rotaOriginal, int novaQuantidadeDePontos)
|
|
{
|
|
var rotaInterpolada = new List<GPSModel>();
|
|
int quantidadeOriginal = rotaOriginal.Count;
|
|
|
|
for (int i = 0; i < novaQuantidadeDePontos; i++)
|
|
{
|
|
// Proporção do ponto na rota original
|
|
double t = (double)i / (novaQuantidadeDePontos - 1);
|
|
double indiceInterpolado = t * (quantidadeOriginal - 1);
|
|
|
|
int indiceInferior = (int)Math.Floor(indiceInterpolado);
|
|
int indiceSuperior = Math.Min(indiceInferior + 1, quantidadeOriginal - 1);
|
|
|
|
double proporcao = indiceInterpolado - indiceInferior;
|
|
|
|
var pontoInterpolado = InterpolarPonto(
|
|
rotaOriginal[indiceInferior],
|
|
rotaOriginal[indiceSuperior],
|
|
proporcao);
|
|
|
|
rotaInterpolada.Add(pontoInterpolado);
|
|
}
|
|
|
|
return rotaInterpolada;
|
|
}
|
|
|
|
public static GPSModel PontoMedio(GPSModel P1, GPSModel P2)
|
|
{
|
|
return new GPSModel()
|
|
{
|
|
Latitude = (P1.Latitude + P2.Latitude) / 2,
|
|
Longitude = (P1.Longitude + P2.Longitude) / 2,
|
|
};
|
|
}
|
|
|
|
public static GPSModel GerarPontoDeslocado(GPSModel pontoInicial, double angulo, double distancia)
|
|
{
|
|
double latitude = pontoInicial.Latitude;
|
|
double longitude = pontoInicial.Longitude;
|
|
|
|
// Conversão de ângulo de graus para radianos
|
|
double anguloRad = angulo * (Math.PI / 180);
|
|
|
|
// Raio da Terra em metros
|
|
double raioTerra = GPSService.RaioDaTerra;
|
|
|
|
// Calcular deslocamento de latitude em radianos
|
|
double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
|
|
// Converter de radianos para graus
|
|
double latAtt = latitude + deltaLat * (180 / Math.PI);
|
|
|
|
// Calcular deslocamento de longitude em radianos
|
|
double deltaLong = distancia * Math.Sin(anguloRad) / (raioTerra * Math.Cos(latitude * (Math.PI / 180)));
|
|
// Converter de radianos para graus
|
|
double longAtt = longitude + deltaLong * (180 / Math.PI);
|
|
|
|
return new GPSModel()
|
|
{
|
|
Latitude = latAtt,
|
|
Longitude = longAtt,
|
|
};
|
|
}
|
|
|
|
|
|
public static bool EstaEntrePontos(GPSModel posicao, GPSModel pontoA, GPSModel pontoB, double margem = 0.000001)
|
|
{
|
|
/*// Calcule a distância entre os pontos usando a fórmula de Haversine ou similar
|
|
double distanciaAToB = DistanciaEntrePontos(pontoA, pontoB);
|
|
double distanciaAToPonto = DistanciaEntrePontos(pontoA, posicao);
|
|
double distanciaBToPonto = DistanciaEntrePontos(pontoB, posicao);
|
|
|
|
// Verifica se a soma das distâncias do pontoA ao ponto e do ponto ao pontoB
|
|
// é aproximadamente igual à distância entre pontoA e pontoB
|
|
return Math.Abs((distanciaAToPonto + distanciaBToPonto) - distanciaAToB) < 0.001;*/
|
|
|
|
// Verifica se o ponto atual está entre pontoA e pontoB em latitude e longitude, com uma margem de erro
|
|
bool dentroLatitude = (posicao.Latitude >= Math.Min(pontoA.Latitude, pontoB.Latitude) - margem &&
|
|
posicao.Latitude <= Math.Max(pontoA.Latitude, pontoB.Latitude) + margem);
|
|
|
|
bool dentroLongitude = (posicao.Longitude >= Math.Min(pontoA.Longitude, pontoB.Longitude) - margem &&
|
|
posicao.Longitude <= Math.Max(pontoA.Longitude, pontoB.Longitude) + margem);
|
|
|
|
return dentroLatitude && dentroLongitude;
|
|
}
|
|
|
|
|
|
private static void DefinirAnguloCarroGPS()
|
|
{
|
|
int PontosConsiderarAngulo = 2;
|
|
double Angulo = 0.0;
|
|
double DistAngulo = 0.0;
|
|
|
|
int Indice = Variaveis.OperacaoEmAndamento.Mapa.mapaService.MapaUrl != "" ? Variaveis.OperacaoEmAndamento.Trajetoria?.IdxCorredorAtual ?? 0 : 0;
|
|
|
|
try
|
|
{
|
|
double somaAngulo = 0;
|
|
var Pontos = !Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Any() || Indice >= Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count() ? new List<GPSModel>() :
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice].Count > PontosConsiderarAngulo ?
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice].OrderByDescending(x => x.DataHora).Take(PontosConsiderarAngulo).ToList() :
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice].OrderByDescending(x => x.DataHora).ToList();
|
|
if (Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count() > Indice && Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice].Count == 1 && Indice > 0)
|
|
{
|
|
Pontos = new List<GPSModel>()
|
|
{
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice - 1][Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice - 1].Count - 1],
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Indice][0]
|
|
};
|
|
}
|
|
Pontos = Pontos.OrderBy(x => x.DataHora).ToList();
|
|
for (int i = 0; i < Pontos.Count() - 1; i++)
|
|
{
|
|
somaAngulo += CalcularOrientacao(Pontos[i], Pontos[i + 1]);
|
|
DistAngulo += DistanciaEntrePontos(Pontos[i], Pontos[i + 1]);
|
|
}
|
|
double mediaAngulo = somaAngulo / (Pontos.Count() - 1);
|
|
|
|
Angulo = (!(mediaAngulo >= 0 || mediaAngulo <= 0)) ? 0 : mediaAngulo;
|
|
}
|
|
catch
|
|
{
|
|
Angulo = 0.0;
|
|
DistAngulo = 0.0;
|
|
}
|
|
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.VariacaoMagneticaGPS = UltimaLeitura.VariacaoMagnetica;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.CursoVerdadeiroCarroGPS = UltimaLeitura.CursoVerdadeiro;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPS = UltimaLeitura.OrientacaoReal;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPSMovimento = Angulo;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.DistAnguloCarroGPS = DistAngulo;
|
|
|
|
DefinirAnguloCarro();
|
|
}
|
|
|
|
public static void DefinirAnguloCarro()
|
|
{
|
|
double anguloFinal = 0.0;
|
|
|
|
bool witIniciado = WT901CService.Iniciado;
|
|
bool gpsIniciado = Iniciado;
|
|
|
|
if (witIniciado && gpsIniciado)
|
|
{
|
|
// Obtém os ângulos de ambos os sensores
|
|
double anguloWT901C = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroBussola;
|
|
double cursoRealGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.CursoVerdadeiroCarroGPS;
|
|
double anguloGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPS;
|
|
double anguloGPSMovimento = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPSMovimento;
|
|
double distAnguloGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.DistAnguloCarroGPS;
|
|
double variacaoMagnetica = Variaveis.OperacaoEmAndamento.Sensoriamento.VariacaoMagneticaGPS;
|
|
|
|
anguloFinal = UnificarAngulos(anguloWT901C, cursoRealGPS, anguloGPSMovimento, distAnguloGPS, anguloGPS, variacaoMagnetica);
|
|
}
|
|
else if (witIniciado)
|
|
{
|
|
// Apenas o WT901C está disponível
|
|
anguloFinal = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroBussola;
|
|
}
|
|
else if (gpsIniciado)
|
|
{
|
|
// Apenas o GPS está disponível
|
|
double cursoRealGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.CursoVerdadeiroCarroGPS;
|
|
double anguloGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPS;
|
|
double anguloGPSMovimento = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPSMovimento;
|
|
double distAnguloGPS = Variaveis.OperacaoEmAndamento.Sensoriamento.DistAnguloCarroGPS;
|
|
double variacaoMagnetica = Variaveis.OperacaoEmAndamento.Sensoriamento.VariacaoMagneticaGPS;
|
|
|
|
anguloFinal = UnificarAngulos(null, cursoRealGPS, anguloGPSMovimento, distAnguloGPS, anguloGPS, variacaoMagnetica);
|
|
}
|
|
else
|
|
{
|
|
anguloFinal = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPSMovimento;
|
|
}
|
|
|
|
// Atualiza o ângulo final
|
|
//anguloFinal = UltimaLeitura.OrientacaoReal;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro = anguloFinal;
|
|
}
|
|
|
|
public static double UnificarAngulos(double? anguloWT901C, double cursoVerdadeiroGPS, double anguloGPSMovimento, double distGPS, double orientacaoRealGPS, double variacaoMagnetica)
|
|
{
|
|
double anguloFinal = 0.0;
|
|
|
|
// Parâmetros ajustáveis
|
|
double difMax = 50.0; // Diferença angular máxima permitida
|
|
double pesoMaxHeading = 0.8; // Peso máximo para o heading (orientacaoRealGPS)
|
|
double pesoMaxGps = 0.6; // Peso máximo combinado do GPS e movimento
|
|
double pesoMaxGpsMovimento = 0.4; // Peso máximo para o ângulo de movimento
|
|
double distanciaMaxima = 1.8; // Distância máxima em metros para peso do GPS
|
|
|
|
// Corrigir ângulos com variação magnética
|
|
cursoVerdadeiroGPS = ((cursoVerdadeiroGPS - variacaoMagnetica) + 360) % 360;
|
|
anguloGPSMovimento = ((anguloGPSMovimento - variacaoMagnetica) + 360) % 360;
|
|
orientacaoRealGPS = ((orientacaoRealGPS - variacaoMagnetica) + 360) % 360;
|
|
|
|
// Normalização do WT901C
|
|
if (anguloWT901C.HasValue)
|
|
anguloWT901C = ((anguloWT901C.Value - variacaoMagnetica) + 360) % 360;
|
|
|
|
// Inicialização de pesos
|
|
double pesoWT901C = 0.0;
|
|
double pesoHeading = 0.0;
|
|
double pesoGPS = 0.0;
|
|
double pesoGPSMovimento = 0.0;
|
|
|
|
if (anguloWT901C.HasValue)
|
|
{
|
|
// Diferença angular entre WT901C e orientacaoRealGPS
|
|
double diferencaWT901C_Heading = Math.Min(
|
|
(anguloWT901C.Value - orientacaoRealGPS + 360) % 360,
|
|
(orientacaoRealGPS - anguloWT901C.Value + 360) % 360
|
|
);
|
|
|
|
// Diferença angular entre WT901C e cursoVerdadeiroGPS
|
|
double diferencaWT901C_GPS = Math.Min(
|
|
(anguloWT901C.Value - cursoVerdadeiroGPS + 360) % 360,
|
|
(cursoVerdadeiroGPS - anguloWT901C.Value + 360) % 360
|
|
);
|
|
|
|
// Peso baseado nas diferenças angulares
|
|
pesoHeading = Math.Max(0, 1 - Math.Pow(diferencaWT901C_Heading / difMax, 2)) * pesoMaxHeading;
|
|
pesoGPS = Math.Max(0, 1 - Math.Pow(diferencaWT901C_GPS / difMax, 2)) * pesoMaxGps;
|
|
pesoGPSMovimento = Math.Min(distGPS / distanciaMaxima, pesoMaxGpsMovimento);
|
|
|
|
// Peso do WT901C é complementar
|
|
pesoWT901C = 1 - (pesoHeading + pesoGPS + pesoGPSMovimento);
|
|
}
|
|
else
|
|
{
|
|
// Diferença entre heading e curso verdadeiro
|
|
double diferencaHeading_Curso = Math.Min(
|
|
(orientacaoRealGPS - cursoVerdadeiroGPS + 360) % 360,
|
|
(cursoVerdadeiroGPS - orientacaoRealGPS + 360) % 360
|
|
);
|
|
|
|
// Peso baseado na diferença entre heading e curso verdadeiro
|
|
pesoHeading = pesoMaxHeading;
|
|
pesoGPS = Math.Max(0, 1 - Math.Pow(diferencaHeading_Curso / difMax, 2)) * pesoMaxGps;
|
|
pesoGPSMovimento = Math.Min(distGPS / distanciaMaxima, pesoMaxGpsMovimento);
|
|
}
|
|
|
|
// Coordenadas no círculo unitário
|
|
double xHeading = Math.Cos(Math.PI * orientacaoRealGPS / 180.0);
|
|
double yHeading = Math.Sin(Math.PI * orientacaoRealGPS / 180.0);
|
|
double xGPS = Math.Cos(Math.PI * cursoVerdadeiroGPS / 180.0);
|
|
double yGPS = Math.Sin(Math.PI * cursoVerdadeiroGPS / 180.0);
|
|
double xGPSMovimento = Math.Cos(Math.PI * anguloGPSMovimento / 180.0);
|
|
double yGPSMovimento = Math.Sin(Math.PI * anguloGPSMovimento / 180.0);
|
|
double xWT901C = 0.0;
|
|
double yWT901C = 0.0;
|
|
|
|
if (anguloWT901C.HasValue)
|
|
{
|
|
xWT901C = Math.Cos(Math.PI * anguloWT901C.Value / 180.0);
|
|
yWT901C = Math.Sin(Math.PI * anguloWT901C.Value / 180.0);
|
|
}
|
|
|
|
// Combinação ponderada dos vetores
|
|
double xFinal = (pesoHeading * xHeading) + (pesoWT901C * xWT901C) + (pesoGPS * xGPS) + (pesoGPSMovimento * xGPSMovimento);
|
|
double yFinal = (pesoHeading * yHeading) + (pesoWT901C * yWT901C) + (pesoGPS * yGPS) + (pesoGPSMovimento * yGPSMovimento);
|
|
|
|
// Verificar magnitude para consistência
|
|
double magnitude = Math.Sqrt(xFinal * xFinal + yFinal * yFinal);
|
|
if (magnitude < 0.5) // Alta divergência
|
|
{
|
|
anguloFinal = orientacaoRealGPS;
|
|
}
|
|
else
|
|
{
|
|
anguloFinal = (Math.Atan2(yFinal, xFinal) * 180.0 / Math.PI + 360) % 360;
|
|
}
|
|
|
|
return anguloFinal;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
public static double CalcularDistanciaPerpendicular(GPSModel ponto, GPSModel segmentoA, GPSModel segmentoB)
|
|
{
|
|
double latA = segmentoA.Latitude, lonA = segmentoA.Longitude;
|
|
double latB = segmentoB.Latitude, lonB = segmentoB.Longitude;
|
|
double latP = ponto.Latitude, lonP = ponto.Longitude;
|
|
|
|
// Vetores
|
|
double vx = latB - latA;
|
|
double vy = lonB - lonA;
|
|
double wx = latP - latA;
|
|
double wy = lonP - lonA;
|
|
|
|
// Projeção do vetor P em relação a AB
|
|
double c1 = wx * vx + wy * vy;
|
|
double c2 = vx * vx + vy * vy;
|
|
double b = c1 / c2;
|
|
|
|
// Ponto projetado na reta AB
|
|
double latProj = latA + b * vx;
|
|
double lonProj = lonA + b * vy;
|
|
|
|
// Distância entre o ponto real e o ponto projetado na reta
|
|
return DistanciaEntrePontos(new GPSModel() { Latitude = latProj, Longitude = lonProj }, ponto);
|
|
}
|
|
|
|
public static double CalcularDistanciaPerpendicular2(GPSModel pontoA, GPSModel pontoB, GPSModel pontoRobo)
|
|
{
|
|
// Converter latitude e longitude para metros usando uma projeção aproximada
|
|
var pontoAMetros = ConverterLatLongParaMetros(pontoA, pontoRobo);
|
|
var pontoBMetros = ConverterLatLongParaMetros(pontoB, pontoRobo);
|
|
var pontoRoboMetros = ConverterLatLongParaMetros(pontoRobo, pontoA);
|
|
|
|
// Vetores
|
|
var vetorAB = (x: pontoBMetros.x - pontoAMetros.x, y: pontoBMetros.y - pontoAMetros.y);
|
|
var vetorAR = (x: pontoRoboMetros.x - pontoAMetros.x, y: pontoRoboMetros.y - pontoAMetros.y);
|
|
|
|
// Produto escalar e magnitude do vetor AB
|
|
double produtoEscalar = vetorAR.x * vetorAB.x + vetorAR.y * vetorAB.y;
|
|
double magnitudeAB = Math.Sqrt(vetorAB.x * vetorAB.x + vetorAB.y * vetorAB.y);
|
|
|
|
// Projeção do vetor AR sobre AB (posição projetada do robô no segmento AB)
|
|
double t = produtoEscalar / (magnitudeAB * magnitudeAB);
|
|
t = Math.Max(0, Math.Min(1, t)); // Restringir t entre 0 e 1 para manter no segmento
|
|
|
|
// Coordenadas do ponto projetado
|
|
var pontoProjetado = (
|
|
x: pontoAMetros.x + t * vetorAB.x,
|
|
y: pontoAMetros.y + t * vetorAB.y
|
|
);
|
|
|
|
// Distância entre o ponto projetado e o robô
|
|
double distancia = Math.Sqrt(
|
|
Math.Pow(pontoRoboMetros.x - pontoProjetado.x, 2) +
|
|
Math.Pow(pontoRoboMetros.y - pontoProjetado.y, 2)
|
|
);
|
|
|
|
return distancia;
|
|
}
|
|
|
|
public static (double x, double y) ConverterLatLongParaMetros(GPSModel ponto, GPSModel referencia)
|
|
{
|
|
double lat1 = GrausParaRadianos(referencia.Latitude);
|
|
double lon1 = GrausParaRadianos(referencia.Longitude);
|
|
double lat2 = GrausParaRadianos(ponto.Latitude);
|
|
double lon2 = GrausParaRadianos(ponto.Longitude);
|
|
|
|
double dLat = lat2 - lat1;
|
|
double dLon = lon2 - lon1;
|
|
|
|
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
|
|
Math.Cos(lat1) * Math.Cos(lat2) *
|
|
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
|
|
|
|
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
|
|
|
double distancia = RaioDaTerra * c;
|
|
|
|
// Calcular coordenadas x e y relativas ao ponto de referência
|
|
double x = distancia * Math.Cos(lat1) * Math.Sin(dLon);
|
|
double y = distancia * Math.Sin(dLat);
|
|
|
|
return (x, y);
|
|
}
|
|
|
|
private static double GrausParaRadianos(double graus)
|
|
{
|
|
return graus * Math.PI / 180;
|
|
}
|
|
|
|
|
|
}
|
|
}
|