From 72211b06d35fbb89c948413a59d714c402ae97db Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 21 Sep 2026 21:32:02 -0300 Subject: [PATCH] ajustado ku, estabilizado movimento do mpc --- .../IHM/Operacao/ucOperacaoParametros.cs | 40 +- .../Models/Modules/DirecionalModel.cs | 2 +- .../Models/Operacoes/OperacaoModel.cs | 116 +++++- AgroBase/AgroBase/Models/Variaveis.cs | 4 +- AgroBase/AgroBase/Services/GPSService.cs | 358 ++++++++++++++++-- .../Operadores/HealthWorkerService.cs | 4 +- .../workers/manager_worker/modulos/mpc.py | 101 ++++- 7 files changed, 555 insertions(+), 70 deletions(-) diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs index 0a6b7428b..5bb24f774 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs @@ -136,13 +136,37 @@ namespace AgroBase.Forms.IHM.Operacao chkRegistrarPosProcessamento.FlatAppearance.BorderColor = ativo ? HmiTheme.Success : HmiTheme.Border; } - private void DefinirOperacaoParametrizando(OperacaoModel operacao, bool recarregarMapa = true) + private void DefinirOperacaoParametrizando( + OperacaoModel operacao, + bool recarregarMapa = true, + bool registrarComoFonteAutoritativa = true) { operacaoParametrizando = operacao; - _operacaoFonteCarregada = operacao; - _parametrosFonteCarregados = operacao?.Parametros; - _controleFonteCarregado = operacao?.Parametros?.Controle; + /* + * IMPORTANTE: + * + * A operação exibida na tela nem sempre é a operação autoritativa + * recebida da base. Ao trocar o modo no ComboBox, por exemplo, + * BtnRedefinir_Click cria uma nova OperacaoModel apenas como + * RASCUNHO local de edição. + * + * Se esse rascunho for registrado em _operacaoFonteCarregada, o + * refresh periódico da IHM compara o rascunho com + * Variaveis.OperacaoEmAndamento, entende que a fonte mudou e + * recarrega imediatamente a operação antiga, desfazendo a seleção + * do operador. + * + * Portanto somente cargas vindas da fonte autoritativa atualizam os + * marcadores usados por SincronizarComOperacaoAtualSeNecessario(). + * Um rascunho local altera somente operacaoParametrizando. + */ + if (registrarComoFonteAutoritativa) + { + _operacaoFonteCarregada = operacao; + _parametrosFonteCarregados = operacao?.Parametros; + _controleFonteCarregado = operacao?.Parametros?.Controle; + } CarregarDadosGerais(); _ucMovimento.DefinirOperacao(operacaoParametrizando); @@ -242,7 +266,13 @@ namespace AgroBase.Forms.IHM.Operacao novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados; } - DefinirOperacaoParametrizando(novaOperacao, recarregarMapa: false); + // É um rascunho local. Não substitui os marcadores da última + // fonte autoritativa, senão o refresh de 1 s restaura o modo antigo. + DefinirOperacaoParametrizando( + novaOperacao, + recarregarMapa: false, + registrarComoFonteAutoritativa: false + ); } diff --git a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs index 2604f74ec..e11a2219d 100644 --- a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs +++ b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs @@ -530,7 +530,7 @@ namespace AgroBase.Models.Modules */ private const double FolgaMinimaReferencia = 0.05; private const double FolgaAlertaReferencia = 6.0; - private const double FolgaMaximaReferencia = 8.0; + private const double FolgaMaximaReferencia = 9.5; private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120); private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30); private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30); diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 161e5840d..56cab2fa5 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -677,7 +677,7 @@ namespace AgroBase.Models ImuDirecionalPreferirArco = false, OakParadaPorObstaculo = false, DirAuxilioSonar = false, - MovAuxilioSonar = true, + MovAuxilioSonar = false, MpcHorizonte = 4.0, AnteciparManobraCorredorM = 0.0, @@ -1065,7 +1065,7 @@ namespace AgroBase.Models ImuDirecionalPreferirArco = false, OakParadaPorObstaculo = false, DirAuxilioSonar = false, - MovAuxilioSonar = true, + MovAuxilioSonar = false, MpcHorizonte = 4.0, AnteciparManobraCorredorM = 0, @@ -1308,6 +1308,19 @@ namespace AgroBase.Models }); } + private static bool ExigeSincronizacaoMapaCritico(OperacaoModel op) + { + /* + * Operacao Manual nao depende de mapa, trajetoria nem MPC. + * A barreira critica de MAPSYNC existe para os modos automaticos + * e nao pode bloquear a vida da operacao Manual. + * + * Mantemos todos os demais modos com o comportamento historico + * para evitar regressao em MapaGPS, RetornoBase, TreinamentoIA etc. + */ + return op?.Parametros?.Modo != ModoOperacao.Manual; + } + public static OperacaoModel CarregarParamerosOperacaoBase(OperacaoParametrosModel parametros) { if (parametros?.Modo == null) @@ -1346,7 +1359,34 @@ namespace AgroBase.Models novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao(); - RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); + bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(novaOp); + + /* + * Manual fica configurado assim que seus parametros locais sao + * carregados. Antes ele recebia configurado=false como MapaGPS, + * mas nunca existia um MAPSYNC capaz de promover esse estado para + * true. Isso mantinha o status eternamente em Parametrizando. + */ + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("configurado", !exigeMapaCritico), + ("mapa_barreira_critica", false), + ("mapa_confirmado", false) + ); + + if (!exigeMapaCritico) + { + HealthWorkerService.AtualizarDadosOperacao( + forcar: true, + sincronizarMapa: false, + configurado: true + ); + + Variaveis.MostrarLog( + "[OperacaoModel.CarregarParamerosOperacaoBase] " + + "Operacao Manual configurada sem MAPSYNC." + ); + } frmInstancial.frmIHM?._frmOperacao?.AtualizarParametrosTela(novaOp); @@ -1400,7 +1440,15 @@ namespace AgroBase.Models { T_Code.Atu, new Dictionary() { } }, }; - RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); + bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(op); + + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("configurado", !exigeMapaCritico), + ("mapa_barreira_critica", false), + ("mapa_confirmado", false) + ); + WeedWorkerService.ReiniciarLeituraAnalise(); VisualWorkerService.ReiniciarLeituraAnalise(); @@ -1416,8 +1464,13 @@ namespace AgroBase.Models op.GPSTrajetoria = new List(); - op.Trajetoria?.ProjetarTrajetoriaFixa(); - op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false; + if (exigeMapaCritico) + { + op.Trajetoria?.ProjetarTrajetoriaFixa(); + + if (op.Trajetoria != null) + op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false; + } op.Sensoriamento.UltimoRegistroLog = agora; op.idxLog = 0; @@ -1428,20 +1481,47 @@ namespace AgroBase.Models op.IniciarTimerLogsOperacao(); - // Barreira crítica: a operação só pode seguir para a fase de - // execução depois que o Manager/Python confirmar exatamente a - // trajetória completa projetada neste OperacaoModel. O método - // faz retry enquanto esta operação continuar sendo a atual. - bool mapaConfirmado = await HealthWorkerService - .SincronizarMapaCriticoAsync(op) - .ConfigureAwait(false); - - if (!mapaConfirmado) + if (exigeMapaCritico) { - Variaveis.MostrarLog( - "[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada." + // Barreira crítica somente para modos que realmente dependem + // da trajetoria/mapa no Manager/Python. + bool mapaConfirmado = await HealthWorkerService + .SincronizarMapaCriticoAsync(op) + .ConfigureAwait(false); + + if (!mapaConfirmado) + { + Variaveis.MostrarLog( + "[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada." + ); + return; + } + } + else + { + /* + * Manual nao tem MAPSYNC. Publica explicitamente a operacao + * como configurada e limpa qualquer barreira residual. + * Assim IniciarOperacao retorna normalmente, liberando tambem + * a trava _acaoLateralEmAndamento da IHM. + */ + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("configurado", true), + ("mapa_barreira_critica", false), + ("mapa_confirmado", false) + ); + + HealthWorkerService.AtualizarDadosOperacao( + forcar: true, + sincronizarMapa: false, + configurado: true + ); + + Variaveis.MostrarLog( + "[OperacaoModel.IniciarOperacao] " + + "Operacao Manual iniciada sem barreira MAPSYNC." ); - return; } await op.RealizarCalibragemInicialAsync(true); diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index 6a35219b3..e2244ed04 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -1020,8 +1020,8 @@ namespace AgroBase.Models public static double ComprimentoTrasCm { get; } = 40.0; // 107 public static double DistanciaEntreEixosCm { get; } = 92.0; public static double BitolaRodasCm { get; } = 75.0; - public static double KuDirecional { get; } = 2.65; - public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0; + public static double KuDirecional { get; } = 0.35; // 2.65 + public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0; // °/s public static double LeverArmFrontalCm { get; set; } = 37.0; // cm public static double LeverArmLateralCm { get; set; } = 0.0; // cm public static double LarguraEquipamentoMm diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index fc37fdb0f..906702e2f 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -58,9 +58,24 @@ namespace AgroBase.Services private static bool InverterHeading = true; private static int rtk_timeout = 60; - private static int TempoMin_Ntrip = 10; + + /* + * Política de RTCM do rover: + * 1) base local via MQTT é sempre a fonte preferencial; + * 2) NTRIP é somente contingência quando a base local some; + * 3) ao retornar RTCM local de forma estável, o rover faz failback + * automaticamente para a base local sem desabilitar a contingência. + */ + public static int RtcmLocalRecenteMs { get; set; } = 3000; + public static int RtcmLocalAusenteParaNtripMs { get; set; } = 6000; + public static int RtcmLocalEstavelParaFailbackMs { get; set; } = 2500; private static volatile bool LoopRTK_Ntrip = false; + + /// + /// Habilita NTRIP como contingência. Não significa que NTRIP seja + /// necessariamente a fonte ativa naquele instante. + /// public static volatile bool CorrecaoRTK_Ntrip = false; /// @@ -369,6 +384,15 @@ namespace AgroBase.Services private static long _lastRtcmReceivedMono; private static long _lastRtcmWrittenMono; + // RTCM por origem. O timestamp MQTT é atualizado mesmo enquanto NTRIP + // está conectado, pois ele é o watchdog que permite detectar o retorno + // da base local e executar failback automático. + private static long _lastMqttRtcmReceivedMono; + private static long _lastNtripRtcmReceivedMono; + private static long _mqttRtcmHealthySinceMono; + private static long _rtcmArbitrationStartMono; + private static int _preferenciaLocalAtiva; + private static long _serialBytesReceived; private static long _nmeaValid; private static long _nmeaInvalid; @@ -574,6 +598,16 @@ namespace AgroBase.Services try { novaPorta.Open(); + + // Uma nova sessão serial começa com uma janela limpa de + // arbitragem. Se a base local estiver disponível, os + // próximos RTCMs MQTT repovoarão imediatamente o watchdog. + long agoraMono = Stopwatch.GetTimestamp(); + Interlocked.Exchange(ref _rtcmArbitrationStartMono, agoraMono); + Interlocked.Exchange(ref _lastMqttRtcmReceivedMono, 0); + Interlocked.Exchange(ref _lastNtripRtcmReceivedMono, 0); + Interlocked.Exchange(ref _mqttRtcmHealthySinceMono, 0); + Interlocked.Exchange(ref _preferenciaLocalAtiva, 0); } catch (Exception ex) { @@ -1785,25 +1819,188 @@ namespace AgroBase.Services // RTCM VIA MQTT / FILA DE ESCRITA // ============================================================ + private static void RegistrarRtcmLocalRecebido(long agoraMono) + { + long anterior = Interlocked.Exchange( + ref _lastMqttRtcmReceivedMono, + agoraMono + ); + + double gapAnteriorMs = GetAgeMs(anterior); + int continuidadeMaxMs = Math.Max(500, RtcmLocalRecenteMs); + + if (anterior <= 0 || + double.IsInfinity(gapAnteriorMs) || + gapAnteriorMs > continuidadeMaxMs) + { + Interlocked.Exchange( + ref _mqttRtcmHealthySinceMono, + agoraMono + ); + + if (anterior <= 0 || + double.IsInfinity(gapAnteriorMs) || + gapAnteriorMs >= Math.Max( + 1000, + RtcmLocalAusenteParaNtripMs + )) + { + Interlocked.Exchange( + ref _preferenciaLocalAtiva, + 0 + ); + } + } + else if (Interlocked.Read(ref _mqttRtcmHealthySinceMono) <= 0) + { + Interlocked.CompareExchange( + ref _mqttRtcmHealthySinceMono, + anterior, + 0 + ); + } + } + + private static bool RtcmLocalRecente() + { + long ultimo = Interlocked.Read( + ref _lastMqttRtcmReceivedMono + ); + + if (ultimo <= 0) + return false; + + return GetAgeMs(ultimo) <= + Math.Max(500, RtcmLocalRecenteMs); + } + + private static bool RtcmLocalEstavelParaPreferencia() + { + if (!RtcmLocalRecente()) + return false; + + long desde = Interlocked.Read( + ref _mqttRtcmHealthySinceMono + ); + + if (desde <= 0) + return false; + + return GetAgeMs(desde) >= + Math.Max(0, RtcmLocalEstavelParaFailbackMs); + } + + private static bool DeveAtivarNtripComoFallback() + { + if (!CorrecaoRTK_Ntrip) + return false; + + // Enquanto a base local estiver chegando, NTRIP nunca toma a frente. + if (RtcmLocalRecente()) + return false; + + long ultimoLocal = Interlocked.Read( + ref _lastMqttRtcmReceivedMono + ); + + int ausenciaMinMs = + Math.Max(1000, RtcmLocalAusenteParaNtripMs); + + if (ultimoLocal > 0) + { + if (GetAgeMs(ultimoLocal) < ausenciaMinMs) + return false; + } + else + { + // No startup dá chance para a base local aparecer antes de abrir + // qualquer conexão NTRIP só porque ainda existe internet. + long inicio = Interlocked.Read( + ref _rtcmArbitrationStartMono + ); + + if (inicio <= 0) + return false; + + if (GetAgeMs(inicio) < ausenciaMinMs) + return false; + } + + Interlocked.Exchange(ref _preferenciaLocalAtiva, 0); + return true; + } + + private static string ObterFonteRtcmAtiva() + { + // A base local vence inclusive durante a pequena janela em que a + // sessão TCP NTRIP ainda está fechando. + if (RtcmLocalEstavelParaPreferencia()) + return "mqtt_local"; + + if (Ntrip_Conectado) + return "ntrip"; + + if (RtcmLocalRecente()) + return "mqtt_local"; + + return "nenhuma"; + } + public static void AplicarCorrecaoRTK_Mqtt(byte[] correcao, int bytesRead) { - /* - * Uma solução RTK não pode receber correções simultâneas de duas - * bases/referências diferentes. Se NTRIP estiver efetivamente - * conectado, ele é a fonte ativa e RTCM via MQTT é ignorado. - * - * Se o NTRIP cair, Ntrip_Conectado volta a false e o MQTT pode - * assumir novamente sem exigir reinicialização do serviço. - */ - if (Ntrip_Conectado) - return; - - if (correcao == null || bytesRead <= 0 || bytesRead > correcao.Length || bytesRead > RtcmMaxPayloadBytes) + if (correcao == null || + bytesRead <= 0 || + bytesRead > correcao.Length || + bytesRead > RtcmMaxPayloadBytes) { Interlocked.Increment(ref _rtcmDroppedInvalid); return; } + long agoraMono = Stopwatch.GetTimestamp(); + RegistrarRtcmLocalRecebido(agoraMono); + + bool localEstavel = RtcmLocalEstavelParaPreferencia(); + + if (localEstavel) + { + bool primeiraConfirmacao = + Interlocked.Exchange( + ref _preferenciaLocalAtiva, + 1 + ) == 0; + + bool vinhaDeNtrip = + Ntrip_Conectado || + GetAgeMs(Interlocked.Read( + ref _lastNtripRtcmReceivedMono + )) <= 3000.0; + + // Só é failback quando realmente havia contingência NTRIP em uso. + // No startup normal com a base local presente não limpa a fila nem + // gera um falso evento de troca de fonte. + if (primeiraConfirmacao && vinhaDeNtrip) + { + lock (_rtcmQueueLock) + { + _rtcmQueue.Clear(); + } + + Variaveis.MostrarLog( + "[GPSService.RTCM][FAILBACK] Base local MQTT estável; " + + "retornando à fonte preferencial local." + ); + } + } + + /* + * Durante a aquisição da base local, uma sessão NTRIP já estabelecida + * continua sendo usada. Assim evitamos trocar por causa de um único + * pacote MQTT isolado. Depois do tempo de estabilidade, MQTT assume. + */ + if (Ntrip_Conectado && !localEstavel) + return; + byte[] copia = new byte[bytesRead]; Buffer.BlockCopy(correcao, 0, copia, 0, bytesRead); @@ -1814,8 +2011,25 @@ namespace AgroBase.Services { EnsureBackgroundWorkersStarted(); + // Nunca mistura a contingência NTRIP com uma base local já estável. + // Mesmo que o socket NTRIP leve alguns instantes para perceber o + // failback, seus frames deixam de entrar na fila imediatamente. + if (source == RtcmSource.Ntrip && + RtcmLocalEstavelParaPreferencia()) + { + return; + } + long agora = Stopwatch.GetTimestamp(); + if (source == RtcmSource.Ntrip) + { + Interlocked.Exchange( + ref _lastNtripRtcmReceivedMono, + agora + ); + } + Interlocked.Increment(ref _rtcmReceived); Interlocked.Exchange(ref _lastRtcmReceivedMono, agora); @@ -1985,8 +2199,12 @@ namespace AgroBase.Services public static async Task IniciarNtripAsync() { - if (!Variaveis.IsAgroBase || !CorrecaoRTK_Ntrip) + if (!Variaveis.IsAgroBase || + !CorrecaoRTK_Ntrip || + !DeveAtivarNtripComoFallback()) + { return; + } lock (_workerLock) { @@ -1998,6 +2216,11 @@ namespace AgroBase.Services _ntripCts = new CancellationTokenSource(); + Variaveis.MostrarLog( + "[GPSService.RTCM][FAILOVER] RTCM local ausente; " + + "ativando NTRIP como contingência." + ); + _ntripTask = Task.Run( () => AplicarCorrecaoRTK_Ntrip( _ntripCts.Token @@ -2085,7 +2308,7 @@ namespace AgroBase.Services MountpointManual = NtripMountpoint, Username = NtripUsername, Password = NtripPassword, - UsarNtripV2 = true, + UsarNtripV2 = false, Comunitario = false, ExigeUsuario = true, ExigeSenha = true, @@ -2101,7 +2324,7 @@ namespace AgroBase.Services MountpointManual = NtripRtk2goMountpoint, Username = NtripRtk2goUsername, Password = NtripRtk2goPassword, - UsarNtripV2 = true, + UsarNtripV2 = false, Comunitario = true, ExigeUsuario = true, ExigeSenha = false, @@ -2213,6 +2436,26 @@ namespace AgroBase.Services !cancellationToken.IsCancellationRequested && CorrecaoRTK_Ntrip) { + /* + * O worker NTRIP pode permanecer vivo em espera. A decisão + * de sair da espera usa a janela completa de ausência local, + * e não apenas o limiar curto de "recente". Assim, depois + * de um failback, perder 1 ou 2 pacotes MQTT não reacende + * NTRIP antes dos 6 s configurados. + */ + if (!DeveAtivarNtripComoFallback()) + { + Ntrip_Conectado = false; + LimparNtripAtivo(); + + await Task.Delay( + TimeSpan.FromSeconds(1), + cancellationToken + ).ConfigureAwait(false); + + continue; + } + /* * Diferente da versão antiga, não encerramos o worker * apenas porque a internet caiu momentaneamente. @@ -2256,7 +2499,8 @@ namespace AgroBase.Services { if ( cancellationToken.IsCancellationRequested || - !CorrecaoRTK_Ntrip) + !CorrecaoRTK_Ntrip || + RtcmLocalEstavelParaPreferencia()) { break; } @@ -2309,7 +2553,8 @@ namespace AgroBase.Services { if ( cancellationToken.IsCancellationRequested || - !CorrecaoRTK_Ntrip) + !CorrecaoRTK_Ntrip || + RtcmLocalEstavelParaPreferencia()) { break; } @@ -2375,9 +2620,14 @@ namespace AgroBase.Services cancellationToken ).ConfigureAwait(false); + if (RtcmLocalEstavelParaPreferencia()) + { + break; + } + /* - * Retorno normal só ocorre quando a sessão foi - * encerrada por cancelamento/desativação. + * Retorno normal também pode ocorrer quando a + * base local reaparece e reassume a preferência. */ } catch (OperationCanceledException) @@ -2427,7 +2677,8 @@ namespace AgroBase.Services if ( !Ntrip_Conectado && !cancellationToken.IsCancellationRequested && - CorrecaoRTK_Ntrip) + CorrecaoRTK_Ntrip && + !RtcmLocalEstavelParaPreferencia()) { Variaveis.MostrarLog( "[GPSService.NTRIP][FALLBACK] " + @@ -2445,6 +2696,11 @@ namespace AgroBase.Services break; } + if (RtcmLocalEstavelParaPreferencia()) + { + continue; + } + Interlocked.Increment( ref _ntripReconnects ); @@ -3882,6 +4138,13 @@ namespace AgroBase.Services ); } + // A base local pode ter voltado enquanto o handshake HTTP + // estava em andamento. Nesse caso nem chegamos a assumir NTRIP. + if (RtcmLocalEstavelParaPreferencia()) + { + return; + } + /* * Antes de assumir a nova referência NTRIP, removemos * correções ainda pendentes da fonte anterior. Isso evita @@ -3966,6 +4229,15 @@ namespace AgroBase.Services !cancellationToken.IsCancellationRequested && CorrecaoRTK_Ntrip) { + if (RtcmLocalEstavelParaPreferencia()) + { + Variaveis.MostrarLog( + "[GPSService.RTCM][FAILBACK] Base local MQTT " + + "reassumiu; encerrando sessão NTRIP ativa." + ); + return; + } + int bytesRead; if (!recebeuDados) @@ -4025,6 +4297,13 @@ namespace AgroBase.Services bytesRead ); + // Pode ter ocorrido failback enquanto aguardávamos a rede. + // Não deixa um frame NTRIP tardio entrar depois da base local. + if (RtcmLocalEstavelParaPreferencia()) + { + return; + } + /* * Preserva a arquitetura do rover: * rede nunca escreve diretamente no UM982. @@ -4657,7 +4936,10 @@ namespace AgroBase.Services PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado; UltimaLeitura.Inicializado = Iniciado; - if (CorrecaoRTK_Ntrip && GetAgeMs(Interlocked.Read(ref _lastRtcmWrittenMono)) > TempoMin_Ntrip * 1000.0) + // NTRIP é contingência: só tenta iniciar quando a base local MQTT + // está ausente além da janela configurada. A presença de internet, + // por si só, nunca mais rouba prioridade da base local. + if (DeveAtivarNtripComoFallback()) { _ = IniciarNtripAsync(); } @@ -5195,8 +5477,15 @@ namespace AgroBase.Services ("rtcm.queue_depth", metrics.RtcmQueueDepth), ("rtcm.last_rx_age_ms", metrics.LastRtcmReceivedAgeMs), ("rtcm.last_write_age_ms", metrics.LastRtcmWrittenAgeMs), + ("rtcm.local.last_rx_age_ms", metrics.LastMqttRtcmReceivedAgeMs), + ("rtcm.local.recente", metrics.RtcmLocalRecente), + ("rtcm.local.estavel", metrics.RtcmLocalEstavel), + ("rtcm.fonte_ativa", metrics.RtcmFonteAtiva), ("rtcm.drop_stale", metrics.RtcmDroppedStale), - ("rtcm.write_errors", metrics.RtcmWriteErrors) + ("rtcm.write_errors", metrics.RtcmWriteErrors), + ("ntrip.connected", metrics.NtripConnected), + ("ntrip.provider", metrics.NtripProvider ?? string.Empty), + ("ntrip.mountpoint", metrics.NtripMountpoint ?? string.Empty) ); CorrecaoRTK_Ntrip = RedisService.GetField(RedisService.ModKey(T_Code.Gps), "correcao_ntrip", false); @@ -5623,6 +5912,24 @@ namespace AgroBase.Services ) ), + LastMqttRtcmReceivedAgeMs = + GetAgeMs( + Interlocked.Read( + ref _lastMqttRtcmReceivedMono + ) + ), + + LastNtripRtcmReceivedAgeMs = + GetAgeMs( + Interlocked.Read( + ref _lastNtripRtcmReceivedMono + ) + ), + + RtcmLocalRecente = RtcmLocalRecente(), + RtcmLocalEstavel = RtcmLocalEstavelParaPreferencia(), + RtcmFonteAtiva = ObterFonteRtcmAtiva(), + ConfigurationRuns = Interlocked.Read( ref _configurationRuns @@ -5984,6 +6291,11 @@ namespace AgroBase.Services public int RtcmQueueDepth { get; set; } public double LastRtcmReceivedAgeMs { get; set; } public double LastRtcmWrittenAgeMs { get; set; } + public double LastMqttRtcmReceivedAgeMs { get; set; } + public double LastNtripRtcmReceivedAgeMs { get; set; } + public bool RtcmLocalRecente { get; set; } + public bool RtcmLocalEstavel { get; set; } + public string RtcmFonteAtiva { get; set; } public long ConfigurationRuns { get; set; } public long ConfigurationErrors { get; set; } diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 9b328537d..a031d3c91 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -210,10 +210,10 @@ namespace AgroBase.Services.Operadores switch (id) { case "EF": - dianteiros.Add(-leitura); + dianteiros.Add(leitura); break; case "DF": - dianteiros.Add(leitura); + dianteiros.Add(-leitura); break; case "ET": traseiros.Add(-leitura); diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py index 66911832c..e46ee71fe 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py @@ -450,6 +450,34 @@ class ControladorMPC: 1.0, min_value=0.25, max_value=2.0, ) + # V23 - Pure Pursuit separado para SaindoRua. + # + # A ferradura em Manobrando ja foi validada com o look-ahead curto acima + # e deve manter a janela de projecao curta que evita atalhos entre os + # dois ramos do U. SaindoRua e outro problema: ainda estamos no trecho + # reto de aproximacao e queremos enxergar mais longe para evitar que + # pequenas mudancas de projecao façam o alvo cruzar de um lado para o + # outro do rover. Por isso ele recebe parametros proprios e uma projecao + # normal de transicao, sem herdar a janela curta de Manobrando. + self._dubins_saida_pp_lookahead_base_m = _safe_float( + parametros_mpc.get("dubins_saida_pp_lookahead_base_m", 1.30), + 1.30, min_value=0.60, max_value=3.0, + ) + self._dubins_saida_pp_lookahead_velocidade_s = _safe_float( + parametros_mpc.get("dubins_saida_pp_lookahead_velocidade_s", 0.35), + 0.35, min_value=0.0, max_value=2.0, + ) + self._dubins_saida_pp_lookahead_min_m = _safe_float( + parametros_mpc.get("dubins_saida_pp_lookahead_min_m", 1.20), + 1.20, min_value=0.40, max_value=3.0, + ) + self._dubins_saida_pp_lookahead_max_m = _safe_float( + parametros_mpc.get("dubins_saida_pp_lookahead_max_m", 1.80), + 1.80, min_value=0.60, max_value=4.0, + ) + if self._dubins_saida_pp_lookahead_max_m < self._dubins_saida_pp_lookahead_min_m: + self._dubins_saida_pp_lookahead_max_m = self._dubins_saida_pp_lookahead_min_m + # V22 - amortecimento exclusivo do handoff antecipado em SaindoRua. # # Nesse estado o rover ja opera a 15% e ainda percorre o trecho reto @@ -1420,8 +1448,16 @@ class ControladorMPC: dh = float(self._wrap_pi(h1 - h0)) return float(dh / max(s1 - s0, 1e-6)) - def _referencia_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto): - """V12: Pure Pursuit deterministico sobre a geometria Dubins. + def _referencia_seguidor_dubins( + self, x, y, theta, idx_base, velocidade, contexto, status_carro + ): + """V23: Pure Pursuit 4WS com perfis separados por fase. + + - Manobrando preserva integralmente o PP curto validado na ferradura. + - SaindoRua usa look-ahead maior e projecao de transicao, apropriados + ao trecho reto que antecede a curva. + + A equacao de curvatura e o ganho continuam compartilhados. A V11 usava tangente/heading local + curvatura estimada. Em uma ferradura isso podia aliviar o estercoamento simplesmente porque o @@ -1445,21 +1481,42 @@ class ControladorMPC: Nao ha arvore MPC, nem promessa de "virar depois": o valor calculado e o comando executado neste ciclo. """ + pp_saida = _status_in(status_carro, [StatusCarroMapa.SaindoRua]) + status_projecao = ( + StatusCarroMapa.SaindoRua.value + if pp_saida + else StatusCarroMapa.Manobrando.value + ) + e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_para_controle( - x, y, idx_base, StatusCarroMapa.Manobrando.value + x, y, idx_base, status_projecao ) s_max = float(self._s_nodes[-1]) if self._s_nodes is not None and len(self._s_nodes) else 0.0 v = max(0.0, float(velocidade)) - look = ( - float(self._dubins_pp_lookahead_base_m) - + float(self._dubins_pp_lookahead_velocidade_s) * v - ) - look = float(np.clip( - look, - float(self._dubins_pp_lookahead_min_m), - float(self._dubins_pp_lookahead_max_m), - )) + + if pp_saida: + look = ( + float(self._dubins_saida_pp_lookahead_base_m) + + float(self._dubins_saida_pp_lookahead_velocidade_s) * v + ) + look = float(np.clip( + look, + float(self._dubins_saida_pp_lookahead_min_m), + float(self._dubins_saida_pp_lookahead_max_m), + )) + perfil_pp = "saindo_rua" + else: + look = ( + float(self._dubins_pp_lookahead_base_m) + + float(self._dubins_pp_lookahead_velocidade_s) * v + ) + look = float(np.clip( + look, + float(self._dubins_pp_lookahead_min_m), + float(self._dubins_pp_lookahead_max_m), + )) + perfil_pp = "manobrando" s_alvo = min(s_max, float(s_proj) + look) xy_ref, i_ref = self._xy_na_abscissa(s_alvo) @@ -1517,10 +1574,14 @@ class ControladorMPC: "xy_ref": (float(xy_ref[0]), float(xy_ref[1])), "s_ref": float(s_alvo), "margem": float(margem), + "perfil_pp": perfil_pp, + "status_projecao": int(status_projecao), } - def _simular_preview_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl): - """Preview leve usando exatamente o mesmo Pure Pursuit da V12.""" + def _simular_preview_seguidor_dubins( + self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl, status_carro + ): + """Preview leve usando o mesmo perfil PP da fase real atual.""" v = max(float(velocidade), float(getattr(self, "_vmin_planejamento", 0.30))) dt = max(0.05, min(float(dt_ctrl), 0.50)) dist_passo = max(0.05, v * dt) @@ -1538,7 +1599,7 @@ class ControladorMPC: for _ in range(passos): ang, dbg = self._referencia_seguidor_dubins( - xs, ys, ths, idx_sim, v, contexto + xs, ys, ths, idx_sim, v, contexto, status_carro ) df_alvo, dr_alvo = self._angulos_eixos_4ws( TipoMovimentoDirecional.MovimentoArco, ang @@ -1678,7 +1739,7 @@ class ControladorMPC: idx_alvo_correcao, pos_lat, pos_lon, pos_theta, pos_latencia, ponto_alvo_real ): angulo_bruto_rad, dbg = self._referencia_seguidor_dubins( - x, y, theta, idx_alvo_correcao, velocidade, contexto + x, y, theta, idx_alvo_correcao, velocidade, contexto, status_carro ) pp_antecipado = _status_in( @@ -1696,7 +1757,7 @@ class ControladorMPC: preview_xy = self._simular_preview_seguidor_dubins( x, y, theta, idx_alvo_correcao, velocidade, contexto, - self.tempo_execucao_local, + self.tempo_execucao_local, status_carro, ) simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(preview_xy) or []) simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta)) @@ -1734,10 +1795,12 @@ class ControladorMPC: "debug_custo": {}, "debug_seguidor_dubins": { "modo": ( - "pure_pursuit_4ws_antecipado" + "pure_pursuit_4ws_saindo_rua" if pp_antecipado - else "pure_pursuit_4ws" + else "pure_pursuit_4ws_manobrando" ), + "perfil_pp": dbg.get("perfil_pp", "desconhecido"), + "status_projecao": dbg.get("status_projecao"), "lookahead_m": float(dbg["lookahead_m"]), "distancia_alvo_m": float(dbg["distancia_alvo_m"]), "alpha_graus": float(math.degrees(dbg["alpha"])),