From cec03f3df410aa48bf9190b8c6f88ead4664c8d5 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Fri, 12 Jun 2026 10:44:53 -0300 Subject: [PATCH] correcao peso imu, descoberta oid e orientacao real gnss --- AgroBase/AgroBase/Services/GPSService.cs | 38 ++++++++++--------- AgroBase/AgroBase/Services/OIDCanService.cs | 2 +- .../workers/health_worker/modulos/imu.py | 4 +- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index 0684fdc6d..ba4985698 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -131,6 +131,7 @@ namespace AgroBase.Services // NMEA só na USB (COM1) $"gngga {porta_usb} {freq}\r\n", $"gpths {porta_usb} {freq}\r\n", + $"gnvtg {porta_usb} {freq}\r\n", $"saveconfig\r\n" }; @@ -384,7 +385,7 @@ namespace AgroBase.Services 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] : ""; + string base_id = campos.Length > 14 ? campos[14].Split('*')[0] : ""; // Conversão de Latitude (ddmm.mmmm) double latitude = 0; @@ -1192,6 +1193,7 @@ namespace AgroBase.Services bool imuIniciado = Variaveis.OperacaoEmAndamento.Sensoriamento?.IMU?.Iniciado ?? false; bool gpsIniciado = Iniciado; + bool headingValido = UltimaLeitura.TimestampOri.frequencia >= 1.0; if (imuIniciado) { @@ -1202,30 +1204,21 @@ namespace AgroBase.Services { anguloFinal = UltimaLeitura.OrientacaoReal; } - else if (gpsIniciado) + else if (gpsIniciado && headingValido) { - // O GPS está disponível - //anguloFinal = UltimaLeitura.OrientacaoReal; - - double heading = UltimaLeitura.OrientacaoReal; - double angMov = UltimaLeitura.OrientacaoMovimento; - double distJan = UltimaLeitura.Distancia; - double velPercent = Variaveis.OperacaoEmAndamento.Simulando ? Variaveis.OperacaoEmAndamento.Controle.RPM_SP : Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.RPMMedio ?? 0; - bool rtkFix = UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.RTKFixo; - double angAnterior = UltimaLeitura.AnguloCarroDefinido; - - //anguloFinal = FundirHeadingComMovimento(heading, angMov, distJan, velPercent, rtkFix, angFundidoAnterior: angAnterior, alfaLowPass: 0.25); - - anguloFinal = heading; + anguloFinal = UltimaLeitura.OrientacaoReal; + } + else if (gpsIniciado && UltimaLeitura.Distancia > 0.25) + { + anguloFinal = UltimaLeitura.OrientacaoMovimento; } else if (imuIniciado) { - // Apenas o WT901C está disponível anguloFinal = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU.YawSeguro; } else { - anguloFinal = UltimaLeitura.OrientacaoMovimento; + anguloFinal = UltimaLeitura.AnguloCarroDefinido; } // Atualiza o ângulo final @@ -1323,6 +1316,15 @@ namespace AgroBase.Services { GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Simulando ? historicoPosicao.Peek() : UltimaLeitura; + bool rtkValido = + posicaoAtual.IdadeCorrecao >= 0 && + posicaoAtual.IdadeCorrecao < rtk_timeout && + ( + posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFixo || + posicaoAtual.QualidadeFix == TiposCorrecaoGPS.RTKFlutuante || + posicaoAtual.QualidadeFix == TiposCorrecaoGPS.DGPS + ); + RedisService.AtualizarCampos( RedisService.ModKey(T_Code.Gps), ("conectado", Iniciado), @@ -1331,7 +1333,7 @@ namespace AgroBase.Services ("lon", posicaoAtual.Longitude), ("theta", posicaoAtual.AnguloCarroDefinido), ("fix", (int)posicaoAtual.QualidadeFix), - ("rtk", posicaoAtual.IdadeCorrecao < rtk_timeout), + ("rtk", rtkValido), ("hAcc", posicaoAtual.PrecisaoCm), ("nSatelites", posicaoAtual.NumeroSatelites), ("freq.posicao", posicaoAtual.TimestampPos.frequencia), diff --git a/AgroBase/AgroBase/Services/OIDCanService.cs b/AgroBase/AgroBase/Services/OIDCanService.cs index 1430db6a1..857688a7e 100644 --- a/AgroBase/AgroBase/Services/OIDCanService.cs +++ b/AgroBase/AgroBase/Services/OIDCanService.cs @@ -109,7 +109,7 @@ namespace AgroBase.Services private static void EnviarComandoDescoberta(byte idTx, byte idRx) { - CanManager.CanService.AdicionarMensagemNaFila(Dispositivo, new byte[] { idTx }, new byte[] { idRx }, (byte)OidParametros.CodErro, (byte)OidParametros.CodErro, null, true, false, true); + CanManager.CanService.AdicionarMensagemNaFila(Dispositivo, new byte[] { idTx }, new byte[] { idRx }, (byte)OidFuncCode.Consulta, (byte)OidFuncCode.Consulta, new byte[] { (byte)OidParametros.CodErro }, true, false, true); } private static void EnviarComando(T_Code Dispositivo, byte idTx, byte idRx, byte funcCode, byte[] payload = null, bool get = true) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 8707cee03..404de858e 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py @@ -169,11 +169,11 @@ class CameraIMU(ModuloDiagnosticoBase): # OAK-D Lite presa no chassi/cabine deve ser a principal. if "oak-d" in modelo or "snr" in dispositivo: - return 0.75 + return 1.0 # Multiespectral na haste entra como redundância. if "ffc" in modelo or "multiespectral" in modelo or "multispectral" in modelo: - return 0.35 + return 0.0 # Fallback seguro. return 0.50