correcao peso imu, descoberta oid e orientacao real gnss

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;
-                //anguloFinal = UltimaLeitura.OrientacaoReal;
+            }
-
+            else if (gpsIniciado && UltimaLeitura.Distancia > 0.25)
-                double heading = UltimaLeitura.OrientacaoReal;
+            {
-                double angMov = UltimaLeitura.OrientacaoMovimento;
+                anguloFinal = 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;
             }
             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),

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)

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