endurecido a trajetoria e revisado comportamento final de recuperação do barramento i2c no sensoriamento

AgroBase/AgroBase/Models/MapasModel.cs

@@ -436,77 +436,105 @@ namespace AgroBase.Models
         public void PopularTrajetoriaMapa(MapaFeatureCollectionModel dados = null)
         {
             if (dados != null)
-            {
                 mapaService.DefinirMapa(dados, mapaService.NomeArquivos ?? "Mapa");
-            }
 
-            if (mapaService.DadosMapa == null || mapaService.DadosMapa.features == null)
+            if (mapaService.DadosMapa?.features == null)
+                throw new InvalidOperationException("Mapa sem features para montar a trajetória.");
+
+            var ids = (RuasPercorrer ?? new List<string>())
+                .Select(x => x?.Trim())
+                .Where(x => !string.IsNullOrWhiteSpace(x))
+                .ToList();
+
+            if (ids.Count < 2)
+                throw new InvalidOperationException("Selecione ao menos duas ruas, na ordem de percurso.");
+
+            if (ids.Distinct(StringComparer.Ordinal).Count() != ids.Count)
+                throw new InvalidOperationException("A seleção contém IDs de rua repetidos.");
+
+            var ruasMontadas = new List<List<GPSModel>>();
+
+            foreach (string idSelecionado in ids)
             {
-                return;
+                var correspondencias = mapaService.DadosMapa.features
-            }
+                    .Where(x =>
+                        x?.properties != null &&
+                        string.Equals(
+                            x.properties.Id?.Trim(),
+                            idSelecionado,
+                            StringComparison.Ordinal
+                        )
+                    )
+                    .ToList();
 
-            TrajetoriaMapa = new List<List<GPSModel>>();
+                if (correspondencias.Count != 1)
-
-            double orientacaoReferencia = double.NaN;
-
-            foreach (string idSelecionado in RuasPercorrer)
                 {
-                MapaFeatureModel feature = mapaService.DadosMapa.features
+                    throw new InvalidOperationException(
-                    .FirstOrDefault(x =>
+                        $"A rua '{idSelecionado}' possui {correspondencias.Count} correspondências no mapa; esperado=1."
-                        x != null &&
-                        x.properties != null &&
-                        string.Equals(x.properties?.Id?.Trim(), idSelecionado?.Trim(), StringComparison.Ordinal)
                     );
-
-                if (feature?.geometry?.coordinates == null || feature.geometry.coordinates.Count < 2)
-                {
-                    continue;
                 }
 
-                List<GPSModel> rua = new List<GPSModel>();
+                var coordenadas = correspondencias[0].geometry?.coordinates;
 
-                foreach (List<double> ponto in feature.geometry.coordinates)
+                if (coordenadas == null || coordenadas.Count < 2)
+                    throw new InvalidOperationException($"A rua '{idSelecionado}' não possui geometria navegável.");
+
+                var rua = new List<GPSModel>();
+
+                for (int i = 0; i < coordenadas.Count; i++)
                 {
+                    List<double> ponto = coordenadas[i];
+
                     if (ponto == null || ponto.Count < 2)
+                        throw new InvalidOperationException($"Coordenada incompleta na rua '{idSelecionado}', ponto {i + 1}.");
+
+                    double longitude = ponto[0];
+                    double latitude = ponto[1];
+
+                    bool valida =
+                        !double.IsNaN(latitude) &&
+                        !double.IsInfinity(latitude) &&
+                        !double.IsNaN(longitude) &&
+                        !double.IsInfinity(longitude) &&
+                        latitude >= -90 && latitude <= 90 &&
+                        longitude >= -180 && longitude <= 180;
+
+                    if (!valida)
+                        throw new InvalidOperationException($"Coordenada inválida na rua '{idSelecionado}', ponto {i + 1}.");
+
+                    rua.Add(new GPSModel
                     {
-                        continue;
+                        Latitude = latitude,
+                        Longitude = longitude
+                    });
                 }
 
-                    rua.Add(new GPSModel { Longitude = ponto[0], Latitude = ponto[1] });
+                ruasMontadas.Add(rua);
             }
 
-                if (rua.Count < 2)
-                    continue;
-
-                double orientacaoRua = GPSUtils.CalcularOrientacao(rua[rua.Count - 1], rua[rua.Count - 2]);
-
-                if (double.IsNaN(orientacaoReferencia))
-                {
-                    orientacaoReferencia = orientacaoRua;
-                }
-                else
-                {
-                    double anguloOposto = (orientacaoRua + 180.0) % 360.0;
-                    double erro = Math.Abs(CalcularErroAngular(anguloOposto, orientacaoReferencia));
-                    if (erro <= 45.0)
-                    {
-                        rua.Reverse();
-                    }
-                }
-
-                TrajetoriaMapa.Add(rua);
-            }
-
-            if (!TrajetoriaMapa.Any())
-                return;
-
             OperacaoModel op = Variaveis.OperacaoEmAndamento;
 
             if (op == null)
-                return;
+                throw new InvalidOperationException("Operação indisponível para instalar a trajetória.");
 
-            op.Trajetoria = new TrajetoriaMapaOperacaoModel(TrajetoriaMapa);
+            var trajetoriaAnterior = op.Trajetoria;
-            op.Trajetoria.ProjetarTrajetoriaFixa();
+            var mapaAnterior = TrajetoriaMapa;
+            var novaTrajetoria = new TrajetoriaMapaOperacaoModel(ruasMontadas);
+
+            try
+            {
+                // A projeção consulta op.Trajetoria internamente; por isso a nova
+                // instância é instalada dentro da transação e restaurada na falha.
+                op.Trajetoria = novaTrajetoria;
+                novaTrajetoria.ProjetarTrajetoriaFixa();
+                TrajetoriaMapa = ruasMontadas;
+            }
+            catch
+            {
+                op.Trajetoria = trajetoriaAnterior;
+                TrajetoriaMapa = mapaAnterior;
+                throw;
+            }
         }
 
         private static double CalcularErroAngular(double atual, double referencia)

AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs

@@ -497,23 +497,38 @@ namespace AgroBase.Models.Modules
                     FreioAlterado ||
                     ReafirmacaoNecessaria;
 
-                OidFuncCode modoControle =
+                OidFuncCode modoControle;
-                    Freado ? OidFuncCode.ComandoCorrenteFreio :
+
-                    ComandoLigado ? OidFuncCode.ComandoVelocidade :
+                if (Freado)
-                    OidFuncCode.ComandoCorrente;
+                {
+                    // TESTE DIAGNÓSTICO:
+                    // mantém toda a lógica EmFreio ativa,
+                    // mas NÃO aplica frenagem elétrica nos OID.
+                    modoControle = OidFuncCode.ComandoCorrente;
+                }
+                else
+                {
+                    modoControle =
+                        ComandoLigado
+                            ? OidFuncCode.ComandoVelocidade
+                            : OidFuncCode.ComandoCorrente;
+                }
 
                 if (EnviaComando)
                 {
                     float setPoint = 0;
+
                     if (modoControle == OidFuncCode.ComandoCorrenteFreio)
                     {
                         setPoint = (float)CorrenteFreio;
                     }
-                    else {
+                    else if (modoControle == OidFuncCode.ComandoVelocidade)
+                    {
                         int mx =
                             Sentido_SP == Sentido.Horario ? 1 :
                             Sentido_SP == Sentido.Antihorario ? -1 :
                             0;
+
                         setPoint = RPM_SP * mx;
                     }
 
@@ -584,6 +599,7 @@ namespace AgroBase.Models.Modules
         public SensorServoModel Servo => Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?.FirstOrDefault(x => x.Inicializado && x.ID.Contains(Mod_ID));
         private double VelocidadeAtual => FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.VelocidadeMediaMs ?? 0);
         private bool StatusControleEmFreio => Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.MovMotor?.Leitura?.Freado ?? false;
+        private bool StatusComandoFreio => Variaveis.OperacaoEmAndamento.Controle?.EmFreio ?? false;
         private int? AnguloLido => (int?)Servo?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?.atual?.valor;
 
         // histerese de velocidade
@@ -622,7 +638,8 @@ namespace AgroBase.Models.Modules
                 return;
             }
 
-            bool emFreio = StatusControleEmFreio;
+            //bool emFreio = StatusControleEmFreio;
+            bool emFreio = StatusComandoFreio;
             bool transicao = (UltimoControle != emFreio);
 
             if (anguloInicial == -1) anguloInicial = AnguloInicial;

AgroBase/AgroBase/Services/GPSService.cs

@@ -2071,6 +2071,17 @@ namespace AgroBase.Services
                 return PenultimaLeitura.Clone();
         }
 
+        public static (GPSModel atual, GPSModel anterior) GetSnapshotPair()
+        {
+            lock (_stateLock)
+            {
+                return (
+                    UltimaLeitura.Clone(),
+                    PenultimaLeitura.Clone()
+                );
+            }
+        }
+
         private static bool PossuiHeadingValido(GPSModel leitura)
         {
             if (leitura == null)

AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs

@@ -1,4 +1,4 @@
-using AgroBase.Models;
+using AgroBase.Models;
 using AgroBase.Models.Modules;
 using AgroBase.Models.Operadores;
 using Newtonsoft.Json;
@@ -344,7 +344,29 @@ namespace AgroBase.Services.Operadores
                                                 lat = p.Posicao.Latitude,
                                                 lon = p.Posicao.Longitude,
                                                 tipo = (int)p.Tipo,
-                                                distanciaMargem = p.LarguraCorredor * 0.8
+                                                distanciaMargem = Math.Max(
+                                                    0.25,
+                                                    p.LarguraCorredor * 0.8
+                                                ),
+
+                                                /*
+                                                 * Identidade global preservada até o MPC.
+                                                 * Não renumerar após recortes/conversões.
+                                                 */
+                                                idxPonto = p.idxPonto,
+                                                idxCorredor = p.idxCorredor,
+
+                                                /*
+                                                 * O MPC usa um limiar de heading mais
+                                                 * conservador para bordas e ligações.
+                                                 */
+                                                estrutural =
+                                                    p.PontoBorda ||
+                                                    p.PontoLigacao ||
+                                                    p.Tipo == TipoPontoRua.BordaEntrada ||
+                                                    p.Tipo == TipoPontoRua.BordaSaida ||
+                                                    p.Tipo == TipoPontoRua.LigacaoEntrada ||
+                                                    p.Tipo == TipoPontoRua.LigacaoSaida
                                             })
                                             .ToList(),
                                 angulo_max_graus = pControle.DirAnguloMaximo,

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py

@@ -216,6 +216,20 @@ def _assinatura_mapa(mapa, p_ref):
             round(_safe_float(
                 ponto.get("distanciaMargem", 0.7)
             ), 3),
+            _safe_int(
+                ponto.get("idxPonto", ponto.get("IdxPonto", -1)),
+                -1,
+            ),
+            _safe_int(
+                ponto.get("idxCorredor", ponto.get("IdxCorredor", -1)),
+                -1,
+            ),
+            bool(
+                ponto.get(
+                    "estrutural",
+                    ponto.get("PontoEstrutural", False),
+                )
+            ),
         ))
 
     referencia = (
@@ -273,6 +287,81 @@ class ControladorMPC:
         self.velocidade_max = _safe_float(parametros_mpc.get("velocidade_max", 2.0), 2.0, min_value=max(self.velocidade_min, 1e-6))
         self.horizonte = _safe_float(parametros_mpc.get("horizonte", 2.0), 2.0, min_value=0.1)
 
+        # O progresso continua sendo autoridade exclusiva do C#. A direcao,
+        # porem, nao deve perseguir o waypoint discreto mais proximo: com
+        # pontos a cada ~0,8 m isso inverte o erro a cada passagem e produz
+        # zigue-zague. Estes parametros criam um alvo continuo adiante da
+        # projecao do rover sobre a polilinha, sem marcar nenhum ponto.
+        self._lookahead_reta_base_m = _safe_float(
+            parametros_mpc.get("lookahead_reta_base_m", 1.80),
+            1.80,
+            min_value=0.50,
+            max_value=8.0,
+        )
+        self._lookahead_reta_velocidade_s = _safe_float(
+            parametros_mpc.get("lookahead_reta_velocidade_s", 0.70),
+            0.70,
+            min_value=0.0,
+            max_value=4.0,
+        )
+        self._lookahead_reta_min_m = _safe_float(
+            parametros_mpc.get("lookahead_reta_min_m", 1.60),
+            1.60,
+            min_value=0.30,
+            max_value=8.0,
+        )
+        self._lookahead_reta_max_m = _safe_float(
+            parametros_mpc.get("lookahead_reta_max_m", 3.00),
+            3.00,
+            min_value=self._lookahead_reta_min_m,
+            max_value=12.0,
+        )
+        self._lookahead_manobra_base_m = _safe_float(
+            parametros_mpc.get("lookahead_manobra_base_m", 0.80),
+            0.80,
+            min_value=0.25,
+            max_value=4.0,
+        )
+        self._lookahead_manobra_velocidade_s = _safe_float(
+            parametros_mpc.get("lookahead_manobra_velocidade_s", 0.35),
+            0.35,
+            min_value=0.0,
+            max_value=2.0,
+        )
+        self._lookahead_manobra_max_m = _safe_float(
+            parametros_mpc.get("lookahead_manobra_max_m", 1.40),
+            1.40,
+            min_value=self._lookahead_manobra_base_m,
+            max_value=6.0,
+        )
+        self._janela_projecao_alvo_m = _safe_float(
+            parametros_mpc.get("janela_projecao_alvo_m", 5.0),
+            5.0,
+            min_value=self._lookahead_reta_max_m,
+            max_value=20.0,
+        )
+
+        # Amortecimento aplicado somente durante CaminhandoRua. Nas curvas o
+        # MPC preserva toda a autoridade angular necessaria para a manobra.
+        self._filtro_angulo_reta_tau_s = _safe_float(
+            parametros_mpc.get("filtro_angulo_reta_tau_s", 0.25),
+            0.25,
+            min_value=0.0,
+            max_value=2.0,
+        )
+        self._taxa_angulo_reta_graus_s = _safe_float(
+            parametros_mpc.get("taxa_angulo_reta_graus_s", 18.0),
+            18.0,
+            min_value=1.0,
+            max_value=120.0,
+        )
+        self._deadband_angulo_reta_graus = _safe_float(
+            parametros_mpc.get("deadband_angulo_reta_graus", 0.60),
+            0.60,
+            min_value=0.0,
+            max_value=5.0,
+        )
+
         self._beam_traj_min = _safe_int(parametros_mpc.get("beam_traj_min", 3), 3, min_value=1, max_value=20)
         self._beam_topN_max = _safe_int(parametros_mpc.get("beam_topN_max", 5), 5, min_value=1, max_value=50)
         self._beam_topN_min = _safe_int(parametros_mpc.get("beam_topN_min", 2), 2, min_value=1, max_value=self._beam_topN_max)
@@ -287,6 +376,22 @@ class ControladorMPC:
 
         self.visitados_execucao = np.zeros(len(self.pontos_info), dtype=self._np_bool) if self.pontos_info else np.zeros(0, dtype=self._np_bool)
 
+        # Índices globais são opcionais para manter compatibilidade. Quando
+        # presentes, eliminam a ambiguidade entre o índice da trajetória C#
+        # e o índice local de uma lista dinâmica enviada ao MPC.
+        self._idx_global_pontos = np.array([
+            _safe_int(
+                p.get("idxPonto", p.get("IdxPonto", i)),
+                i,
+            )
+            for i, p in enumerate(self.pontos_info)
+        ], dtype=np.int64)
+        self._idx_global_para_local = {
+            int(idx_global): int(i)
+            for i, idx_global in enumerate(self._idx_global_pontos)
+        }
+        self._ultima_divergencia_visitados = False
+
         try:
             if self.pontos_info:
                 P = np.array([p.get("xy", (0.0, 0.0)) for p in self.pontos_info], dtype=np.float32)
@@ -356,7 +461,69 @@ class ControladorMPC:
         self._s_nodes = s_nodes
         self._mg_nodes = mg_nodes
 
-    def cross_track_error_point(self, x, y):
+    def _projetar_na_trajetoria_local(self, x, y, idx_referencia=None):
+        """Projeta a pose apenas na vizinhanca do progresso corrente.
+
+        A busca global e ambigua em mapas agricolas: duas passadas paralelas
+        podem estar a apenas 1,5 m. Ancorar a busca no proximo indice da rota
+        impede o erro lateral de trocar silenciosamente para outra rua.
+        """
+        if self._seg_p0 is None or self._seg_p0.shape[0] == 0:
+            return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
+
+        total_segmentos = int(self._seg_p0.shape[0])
+
+        if idx_referencia is None:
+            i0 = 0
+            i1 = total_segmentos - 1
+        else:
+            idx = max(0, min(_safe_int(idx_referencia, 0), len(self.pontos_info) - 1))
+            s_base = float(self._s_nodes[idx])
+            s_min = max(0.0, s_base - 1.25)
+            s_max = min(
+                float(self._s_nodes[-1]),
+                s_base + float(self._janela_projecao_alvo_m),
+            )
+
+            i0 = max(0, int(np.searchsorted(self._s_nodes, s_min, side="right")) - 1)
+            i1 = min(
+                total_segmentos - 1,
+                int(np.searchsorted(self._s_nodes, s_max, side="left")),
+            )
+
+            # Nunca exclui o segmento que termina no proximo ponto
+            # autoritativo, mesmo diante de pequenos segmentos degenerados.
+            i0 = min(i0, max(0, idx - 1))
+            i1 = max(i1, min(total_segmentos - 1, idx))
+
+        P = np.array([x, y], dtype=float)
+        p0 = self._seg_p0[i0:i1 + 1]
+        vetores = self._seg_v[i0:i1 + 1]
+        comprimentos = self._seg_L[i0:i1 + 1]
+        l2 = np.maximum(comprimentos ** 2, 1e-12)
+
+        w = P - p0
+        t = np.clip(np.sum(w * vetores, axis=1) / l2, 0.0, 1.0)
+        projecoes = p0 + t[:, None] * vetores
+        d2 = np.sum((projecoes - P) ** 2, axis=1)
+        indice_relativo = int(np.argmin(d2))
+        i = i0 + indice_relativo
+
+        proj_i = projecoes[indice_relativo]
+        t_i = float(t[indice_relativo])
+        e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i]))
+        s_proj = float(self._s_nodes[i] + t_i * self._seg_L[i])
+        theta_path = float(
+            np.arctan2(self._seg_t_hat[i, 0], self._seg_t_hat[i, 1])
+        )
+
+        mg0 = self._mg_nodes[i]
+        mg1 = self._mg_nodes[i + 1] if (i + 1) < self._mg_nodes.size else mg0
+        margem = float((1.0 - t_i) * mg0 + t_i * mg1)
+
+        return e_lat, s_proj, i, proj_i, theta_path, margem
+
+    def cross_track_error_point(self, x, y, idx_referencia=None):
         try:
             """
             Retorna:
@@ -367,33 +534,124 @@ class ControladorMPC:
             theta_path -> orientação do caminho (rad) naquele segmento
             margem_interp -> margem interpolada (m) naquele s
             """
-            if self._seg_p0 is None or self._seg_p0.shape[0] == 0:
+            return self._projetar_na_trajetoria_local(
-                return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
+                x,
-
+                y,
-            P = np.array([x, y], float)
+                idx_referencia=idx_referencia,
-            w = P - self._seg_p0            # (M,2)
+            )
-            L2 = (self._seg_L ** 2)         # (M,)
-            t = np.clip(np.sum(w * self._seg_v, axis=1) / L2, 0.0, 1.0)  # (M,)
-            proj = self._seg_p0 + t[:, None] * self._seg_v               # (M,2)
-            d2 = np.sum((proj - P)**2, axis=1)                           # (M,)
-            i = int(np.argmin(d2))
-
-            # projeção escolhida
-            proj_i = proj[i]
-            e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i]))        # sinal pela normal esquerda
-            s_proj = float(self._s_nodes[i] + t[i] * self._seg_L[i])
-            theta_path = float(np.arctan2(self._seg_t_hat[i,0], self._seg_t_hat[i,1])) # 0=Norte se (x=E,y=N)
-
-            # margem interpolada entre nós i e i+1
-            mg0 = self._mg_nodes[i]
-            mg1 = self._mg_nodes[i+1] if (i+1) < self._mg_nodes.size else mg0
-            margem = float((1.0 - t[i]) * mg0 + t[i] * mg1)
-
-            return e_lat, s_proj, i, proj_i, theta_path, margem
         except Exception as e:
             _log(f"Erro ao calcular erro lateral: {e}")
             return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
 
+    def _xy_na_abscissa(self, s_alvo):
+        """Interpola um ponto continuo na polilinha pela distancia acumulada."""
+        if self._cl_xy is None or self._cl_xy.shape[0] == 0:
+            return (0.0, 0.0), 0
+
+        if self._cl_xy.shape[0] == 1 or self._seg_p0 is None:
+            return tuple(self._cl_xy[0]), 0
+
+        s = float(np.clip(s_alvo, 0.0, float(self._s_nodes[-1])))
+        i = int(np.searchsorted(self._s_nodes, s, side="right") - 1)
+        i = max(0, min(i, self._seg_p0.shape[0] - 1))
+
+        comprimento = float(self._seg_L[i])
+        t = 0.0 if comprimento <= 1e-9 else (s - float(self._s_nodes[i])) / comprimento
+        t = float(np.clip(t, 0.0, 1.0))
+        xy = self._seg_p0[i] + t * self._seg_v[i]
+        return (float(xy[0]), float(xy[1])), i
+
+    def _distancia_lookahead(self, status_carro, velocidade):
+        v = max(0.0, _safe_float(velocidade, 0.0))
+
+        if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
+            desejado = self._lookahead_reta_base_m + self._lookahead_reta_velocidade_s * v
+            return float(np.clip(
+                desejado,
+                self._lookahead_reta_min_m,
+                self._lookahead_reta_max_m,
+            ))
+
+        desejado = self._lookahead_manobra_base_m + self._lookahead_manobra_velocidade_s * v
+        return float(np.clip(
+            desejado,
+            self._lookahead_manobra_base_m,
+            self._lookahead_manobra_max_m,
+        ))
+
+    def _construir_alvo_direcional(self, x, y, idx_base, status_carro, velocidade):
+        """Cria alvo pure-pursuit continuo sem alterar o progresso visitado."""
+        n = len(self.pontos_info)
+        if n <= 0:
+            return {"xy": (float(x), float(y)), "_idx_base": 0}
+
+        idx = max(0, min(_safe_int(idx_base, 0), n - 1))
+        _, s_proj, _, _, _, margem = self._projetar_na_trajetoria_local(
+            x,
+            y,
+            idx_referencia=idx,
+        )
+
+        lookahead = self._distancia_lookahead(status_carro, velocidade)
+        s_alvo = min(float(self._s_nodes[-1]), float(s_proj) + lookahead)
+        xy_alvo, i_seg = self._xy_na_abscissa(s_alvo)
+
+        idx_meta = min(n - 1, i_seg + 1)
+        alvo = dict(self.pontos_info[idx_meta])
+        alvo["xy"] = xy_alvo
+        alvo["distanciaMargem"] = margem
+        alvo["_idx_base"] = idx
+        alvo["_s_projecao"] = float(s_proj)
+        alvo["_s_alvo"] = float(s_alvo)
+        alvo["_lookahead_m"] = float(max(0.0, s_alvo - s_proj))
+        return alvo
+
+    def _estabilizar_angulo_saida(
+        self,
+        angulo_desejado_rad,
+        status_carro,
+        comando_anterior,
+        dt_controle,
+    ):
+        """Filtra/rate-limita esterco somente na passada reta."""
+        desejado = float(np.clip(
+            angulo_desejado_rad,
+            -np.radians(self.angulo_max_graus),
+            np.radians(self.angulo_max_graus),
+        ))
+
+        if not _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
+            return desejado
+
+        anterior = np.radians(_safe_float(
+            _as_dict(comando_anterior).get("angulo", 0.0),
+            0.0,
+            min_value=-self.angulo_max_graus,
+            max_value=self.angulo_max_graus,
+        ))
+        dt = _safe_float(dt_controle, 0.5, min_value=0.05, max_value=1.5)
+
+        tau = float(self._filtro_angulo_reta_tau_s)
+        alpha = 1.0 if tau <= 1e-6 else dt / (tau + dt)
+        filtrado = anterior + alpha * (desejado - anterior)
+
+        max_delta = np.radians(self._taxa_angulo_reta_graus_s) * dt
+        filtrado = anterior + float(np.clip(
+            filtrado - anterior,
+            -max_delta,
+            max_delta,
+        ))
+
+        dead = np.radians(self._deadband_angulo_reta_graus)
+        if abs(desejado) <= dead and abs(anterior) <= 2.0 * dead:
+            filtrado = 0.0
+
+        return float(np.clip(
+            filtrado,
+            -np.radians(self.angulo_max_graus),
+            np.radians(self.angulo_max_graus),
+        ))
+
     def _get_costmap_direcional(self, contexto):
         """
         Lê o contrato VisualWorker.CostMapDirecional e devolve uma estrutura interna
@@ -775,27 +1033,162 @@ class ControladorMPC:
             _log(f"Erro ao consultar proximo ponto nao visitado: {e}")
             return 0
     
-    def _corrigir_pontos_visitados(self, x, y, pontos_visitados, idx_atual=-1, limite_max_avanco=5.0, limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True):
+    def _resolver_indice_local_autoritativo(self, idx_atual: int) -> int:
-        if (idx_atual > -1):
+        """Converte o índice publicado pelo C# para o índice local do mapa."""
-            for idx, ponto in enumerate(self.pontos_info):
+        n = len(self.pontos_info)
-                if (idx < idx_atual):
+        if n <= 0:
-                    pontos_visitados[idx] = True
+            return 0
-                else:
-                    break
-            return idx_atual
 
-        for idx, ponto in enumerate(self.pontos_info):
+        idx_bruto = _safe_int(idx_atual, 0)
-            if pontos_visitados[idx]:
+
-                continue
+        if idx_bruto in self._idx_global_para_local:
-            pos = ponto["xy"]
+            return self._idx_global_para_local[idx_bruto]
-            margem = ponto.get("distanciaMargem", 0.7)
+
-            dist = np.linalg.norm([x - pos[0], y - pos[1]])
+        # Compatibilidade com o contrato antigo, no qual o C# já publicava
-            if dist < margem:
+        # o índice na mesma base do array entregue ao MPC.
-                for i in range(idx + 1):
+        return max(0, min(idx_bruto, n))
-                    pontos_visitados[i] = True
+
-                return idx
+    def _sincronizar_visitados_com_csharp(self, pontos_visitados, idx_atual: int) -> int:
-            if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanco:
+        """Aplica exatamente o prefixo autoritativo publicado pelo C#.
+
+        O MPC pode avançar pontos em cópias usadas nas simulações, mas esse
+        avanço é especulativo e nunca pode sobreviver no estado da execução
+        real se o C# ainda não confirmou a mesma transição.
+        """
+        n = len(self.pontos_info)
+        if n <= 0:
+            return 0
+
+        idx_local = self._resolver_indice_local_autoritativo(idx_atual)
+        idx_local = max(0, min(idx_local, n))
+
+        havia_avanco_especulativo = bool(
+            np.any(pontos_visitados[idx_local:])
+        ) if idx_local < n else False
+
+        pontos_visitados[:] = False
+        if idx_local > 0:
+            pontos_visitados[:idx_local] = True
+
+        if havia_avanco_especulativo and not self._ultima_divergencia_visitados:
+            _log(
+                f"[MPC] Progresso especulativo descartado; "
+                f"C# autoritativo no idx_local={idx_local}."
+            )
+
+        self._ultima_divergencia_visitados = havia_avanco_especulativo
+
+        # Quando o C# sinaliza conclusão (idx == n), o chamador já ordena a
+        # parada. Retornamos n-1 apenas para manter acessos de debug seguros.
+        return min(idx_local, n - 1)
+
+    @staticmethod
+    def _progresso_segmento_xy(inicio, fim, robo):
+        a = np.asarray(inicio, dtype=float)
+        b = np.asarray(fim, dtype=float)
+        p = np.asarray(robo, dtype=float)
+        v = b - a
+        w = p - a
+        l2 = float(np.dot(v, v))
+
+        if not math.isfinite(l2) or l2 < 1e-8:
+            return float("-inf"), float("inf"), 0.0
+
+        comprimento = math.sqrt(l2)
+        produto = float(np.dot(w, v))
+        avanco = produto / comprimento
+        lateral = abs(float(v[0] * w[1] - v[1] * w[0])) / comprimento
+        parametro = produto / l2
+        return avanco, lateral, parametro
+
+    def _corrigir_pontos_visitados(
+        self,
+        x,
+        y,
+        pontos_visitados,
+        idx_atual=-1,
+        limite_max_avanco=5.0,
+        limite_max_pontos: int = 10,
+        velocidade: float = -1,
+        dt: float = -1,
+        look_ahead: bool = True,
+        theta=None,
+    ):
+        if idx_atual > -1:
+            return self._sincronizar_visitados_com_csharp(
+                pontos_visitados,
+                idx_atual,
+            )
+
+        n = len(self.pontos_info)
+        if n <= 0:
+            return 0
+
+        idx_primeiro = self._proximo_nao_visitado(pontos_visitados)
+        idx_primeiro = max(0, min(idx_primeiro, n - 1))
+        idx_limite = min(
+            n - 2,
+            idx_primeiro + max(1, int(limite_max_pontos)) - 1,
+        )
+
+        # Sem heading confiável, a simulação mantém a regra antiga de alvo
+        # sequencial e não tenta recuperar pontos pulados.
+        theta_valido = theta is not None and math.isfinite(float(theta))
+
+        if not theta_valido or idx_primeiro > idx_limite:
+            return idx_primeiro
+
+        robo = (float(x), float(y))
+        theta_robo = float(theta)
+
+        for idx in range(idx_primeiro, idx_limite + 1):
+            ponto = self.pontos_info[idx]
+            proximo = self.pontos_info[idx + 1]
+            inicio = ponto.get("xy", (0.0, 0.0))
+            fim = proximo.get("xy", inicio)
+
+            avanco, lateral, parametro = self._progresso_segmento_xy(
+                inicio,
+                fim,
+                robo,
+            )
+
+            margem_ponto = max(
+                0.30,
+                _safe_float(ponto.get("distanciaMargem", 0.7), 0.7),
+            )
+            tolerancia_lateral = min(1.50, margem_ponto + 0.35)
+            estrutural = bool(
+                ponto.get(
+                    "estrutural",
+                    ponto.get("PontoEstrutural", False),
+                )
+            )
+            avanco_minimo = 0.0 if estrutural else -0.10
+
+            vetor = np.asarray(fim, dtype=float) - np.asarray(inicio, dtype=float)
+            theta_trecho = math.atan2(float(vetor[0]), float(vetor[1]))
+            erro_heading = abs(
+                (theta_trecho - theta_robo + math.pi) % (2.0 * math.pi)
+                - math.pi
+            )
+            erro_maximo = math.radians(35.0 if estrutural else 45.0)
+
+            ponto_ficou_para_tras = (
+                math.isfinite(avanco)
+                and math.isfinite(lateral)
+                and math.isfinite(parametro)
+                and avanco >= avanco_minimo
+                and parametro >= (0.0 if estrutural else -0.10)
+                and lateral <= tolerancia_lateral
+                and erro_heading <= erro_maximo
+            )
+
+            if not ponto_ficou_para_tras:
                 break
+
+            pontos_visitados[idx] = True
+
         return self._proximo_nao_visitado(pontos_visitados)
 
     def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m):
@@ -859,14 +1252,17 @@ class ControladorMPC:
                 TipoMovimentoDirecional.MovimentoDiagonal: 0.0
             }
 
+            erro_ori_abs = abs(float(erro_ori_graus))
+            erro_lat_abs = abs(float(erro_lat_m))
+
             if status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]:
                 custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0
                 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.0
                 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 2.0
             else:
-                custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_graus >= 10.0 else 1.0
+                custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_abs >= 10.0 else 1.0
-                custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_graus >= 10.0 else 0.0
+                custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_abs >= 10.0 else 0.0
-                custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_graus <= 2.0 and erro_lat_m <= 0.4 else 2.0
+                custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_abs <= 2.0 and erro_lat_abs <= 0.4 else 2.0
 
             return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento)
         except Exception as e:
@@ -938,7 +1334,12 @@ class ControladorMPC:
                 if time_left_ms() <= 0.0:
                     return x, y, theta, visitados
                 x, y, theta = aplicar_passo_dt(x, y, theta, u, dt_eff)
-                idx_alvo = self._corrigir_pontos_visitados(x, y, visitados)
+                idx_alvo = self._corrigir_pontos_visitados(
+                    x,
+                    y,
+                    visitados,
+                    theta=theta,
+                )
 
             t_rem -= seg
 
@@ -1120,7 +1521,20 @@ class ControladorMPC:
                 time_left_ms=lambda: (deadline - now()) * 1000.0,
                 calc_omega_fn=self.gps_handler.calcular_omega
             )
-            idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real)
+            idx_alvo_correcao = self._corrigir_pontos_visitados(
+                x,
+                y,
+                self.visitados_execucao,
+                idx_proximo_ponto_real,
+                theta=theta,
+            )
+            ponto_alvo_real = self._construir_alvo_direcional(
+                x,
+                y,
+                idx_alvo_correcao,
+                status_carro,
+                velocidade,
+            )
 
             # -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
             S_ALVO   = float(self.horizonte)
@@ -1222,11 +1636,32 @@ class ControladorMPC:
                         }
 
                         #look_ahead = passo > 2 and status_carro not in [StatusCarroMapa.Manobrando.value, StatusCarroMapa.Direcionando.value]
-                        idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados, velocidade=v_sim, dt=dt_pred)
+                        if passo == 0:
-                        #if passo == 0:
+                            # O primeiro comando nasce obrigatoriamente do
-                        #    print(f"idx_alvo: {idx_alvo}")
+                            # próximo ponto confirmado pelo C#. O MPC não
-                        #if idx_alvo < len(self.pontos_info) - 1: idx_alvo += 1
+                            # pode usar a pose real para promover um ponto
-                        ponto_alvo = self.pontos_info[idx_alvo]
+                            # especulativamente e continuar andando enquanto
+                            # a trajetória autoritativa permanece parada.
+                            idx_alvo = idx_alvo_correcao
+                        else:
+                            idx_alvo = self._corrigir_pontos_visitados(
+                                x_atual,
+                                y_atual,
+                                visitados,
+                                velocidade=v_sim,
+                                dt=dt_pred,
+                                theta=theta_atual,
+                            )
+                        # A autoridade de visitados continua em idx_alvo. O
+                        # ponto usado para dirigir e continuo e fica adiante
+                        # da projecao local do candidato sobre a rota.
+                        ponto_alvo = self._construir_alvo_direcional(
+                            x_atual,
+                            y_atual,
+                            idx_alvo,
+                            status_carro,
+                            v_sim,
+                        )
 
                         pares_candidatos, custos_candidatos = self._gerar_angulos_candidatos_receding(
                             ponto_alvo,
@@ -1235,7 +1670,8 @@ class ControladorMPC:
                             contexto,
                             deadline,
                             15.0,
-                            dados_costmap=dados_costmap_ciclo
+                            dados_costmap=dados_costmap_ciclo,
+                            tipo_preferido=cmd_anterior_local.get("tipo"),
                         )
 
                         if ms_left() <= 0 or exp_budget <= 0:
@@ -1371,8 +1807,20 @@ class ControladorMPC:
                     tipo_final = TipoMovimentoDirecional.RodasDianteiras
                     simulacao_latlon = []
 
-            ponto_alvo_primario = self.pontos_info[idx_alvo_correcao]["xy"]
+            if not parada_necessaria:
-            erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(x, y)
+                angulo_final = self._estabilizar_angulo_saida(
+                    angulo_final,
+                    status_carro,
+                    comando_anterior,
+                    self.tempo_execucao_local,
+                )
+
+            ponto_alvo_primario = ponto_alvo_real["xy"]
+            erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(
+                x,
+                y,
+                idx_referencia=idx_alvo_correcao,
+            )
             _, erro_orientacao = self._calcula_erro_orientacao(x, y, theta, ponto_alvo_primario, theta_path, 0.5)
 
             _cmd = {
@@ -1385,6 +1833,9 @@ class ControladorMPC:
                 "simulacao": simulacao_latlon,
                 "erro_lateral": erro_lateral,
                 "erro_orientacao": erro_orientacao,
+                "idx_alvo_autoritativo": int(idx_alvo_correcao),
+                "alvo_direcional_xy": ponto_alvo_primario,
+                "lookahead_direcional_m": float(ponto_alvo_real.get("_lookahead_m", 0.0)),
                 "debug_custo": debug_custo,
                 "candidatos_testados": K,
                 "motivos": []
@@ -1400,13 +1851,14 @@ class ControladorMPC:
             ):
                 deadline_dbg = time.perf_counter() + 0.03
                 pares_dbg, _ = self._gerar_angulos_candidatos_receding(
-                    ponto_alvo=self.pontos_info[idx_alvo_correcao],
+                    ponto_alvo=ponto_alvo_real,
                     ponto_atual=(x, y, theta),
                     ponto_ref=(x, y, theta),
                     contexto=contexto,
                     deadline=deadline_dbg,
                     margem_ms=8.0,
-                    dados_costmap=dados_costmap_dbg
+                    dados_costmap=dados_costmap_dbg,
+                    tipo_preferido=comando_anterior.get("tipo"),
                 )
 
                 tipos_dbg = []
@@ -1460,7 +1912,17 @@ class ControladorMPC:
             "motivos": _safe_motivos(motivos),
         }
 
-    def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto, deadline, margem_ms, dados_costmap=None):
+    def _gerar_angulos_candidatos_receding(
+        self,
+        ponto_alvo,
+        ponto_atual,
+        ponto_ref,
+        contexto,
+        deadline,
+        margem_ms,
+        dados_costmap=None,
+        tipo_preferido=None,
+    ):
         """
         Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo:
         1) Ângulo ideal
@@ -1506,7 +1968,11 @@ class ControladorMPC:
             angulo_ideal_deg = float(np.degrees(angulo_ideal_rad))
             #print(f"Orient: {np.degrees(orient)}, delta theta: {np.degrees(delta_theta)}, angulo ideal: {angulo_ideal_deg}")
 
-            e_lat, _, _, _, theta_path, _ = self.cross_track_error_point(ponto_atual[0], ponto_atual[1])
+            e_lat, _, _, _, theta_path, _ = self.cross_track_error_point(
+                ponto_atual[0],
+                ponto_atual[1],
+                idx_referencia=ponto_alvo.get("_idx_base"),
+            )
             (_, e_ori) = self._calcula_erro_orientacao(ponto_atual[0], ponto_atual[1], ponto_atual[2], ponto_alvo["xy"], theta_path, 0.5)
 
             # 2) Tipos válidos conforme contexto
@@ -1516,7 +1982,20 @@ class ControladorMPC:
                 tipos_validos = [TipoMovimentoDirecional.MovimentoArco]
             elif abs(e_ori) > 15.0:
                 tipos_validos = [TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco]
-            elif abs(e_ori) <= 3.0 and abs(e_lat) <= 0.2:
+            else:
+                # Histerese de modo na passada. Antes, 0,20 m/3 graus era
+                # uma fronteira seca: qualquer ruido alternava Diagonal e
+                # RodasDianteiras. Entrar exige uma faixa estreita; depois de
+                # entrar, a faixa de permanencia e um pouco maior.
+                tipo_preferido_enum = _movimento_from_value(tipo_preferido)
+                manter_diagonal = (
+                    tipo_preferido_enum == TipoMovimentoDirecional.MovimentoDiagonal
+                    and abs(e_ori) <= 4.0
+                    and abs(e_lat) <= 0.30
+                )
+                entrar_diagonal = abs(e_ori) <= 1.5 and abs(e_lat) <= 0.12
+
+                if manter_diagonal or entrar_diagonal:
                     tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal]
                 else:
                     tipos_validos = [TipoMovimentoDirecional.RodasDianteiras]
@@ -2379,9 +2858,18 @@ class ControladorMPC:
                 # alvo mais próximo após avançar (mantido)
                 idx_alvo_sim = self._corrigir_pontos_visitados(
                     x_sim, y_sim, pontos_visitados,
-                    velocidade=v_planejado, dt=sub_dt
+                    velocidade=v_planejado,
+                    dt=sub_dt,
+                    theta=theta_sim,
                 )
-                ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"]
+                alvo_direcional_sim = self._construir_alvo_direcional(
+                    x_sim,
+                    y_sim,
+                    idx_alvo_sim,
+                    contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value),
+                    v_planejado,
+                )
+                ponto_alvo_sim = alvo_direcional_sim["xy"]
 
                 # --------------------------
                 # ERROS / CUSTOS
@@ -2391,7 +2879,11 @@ class ControladorMPC:
                 
                 # --- PATCH A: usa theta do CAMINHO (segmento projetado) como referência
                 # e usa também margem local pra normalização do custo lateral
-                e_lat, _, _, _, theta_path, margem = self.cross_track_error_point(x_sim, y_sim)
+                e_lat, _, _, _, theta_path, margem = self.cross_track_error_point(
+                    x_sim,
+                    y_sim,
+                    idx_referencia=idx_alvo_sim,
+                )
 
                 # erro de orientação
                 (o_norm, erro_ori_sim) = self._calcula_erro_orientacao(x_sim, y_sim, theta_sim, ponto_alvo_sim, theta_path, 0.5)
@@ -2403,7 +2895,9 @@ class ControladorMPC:
                 # ângulo relativo ao alvo e suavidade (parcialmente mantido)
                 vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim])
                 vetor_alvo_norm = vetor_alvo / (np.linalg.norm(vetor_alvo) + 1e-9)
-                vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
+                # Mesmo referencial de _nova_posicao: x=Leste usa sin(theta)
+                # e y=Norte usa cos(theta).
+                vetor_movel = np.array([np.sin(theta_sim), np.cos(theta_sim)])
                 cos_delta = float(np.dot(vetor_movel, vetor_alvo_norm))
                 delta_angulo = abs(angulo_testado - prev_ang)
 

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py

@@ -1511,22 +1511,56 @@ class ContextoGlobalRedis:
 
             handler = GPSHandler(float(p_ref[0]), float(p_ref[1]))
 
-            for p in points_iter(pontos):
+            indices_globais = set()
+            identidade_ambigua = False
+
+            for indice_local, p in enumerate(points_iter(pontos)):
                 try:
                     lat = cls._float(p.get("lat", 0.0), 0.0)
                     lon = cls._float(p.get("lon", 0.0), 0.0)
 
+                    idx_ponto = cls._int(
+                        p.get("idxPonto", p.get("IdxPonto", indice_local)),
+                        indice_local,
+                    )
+                    idx_corredor = cls._int(
+                        p.get("idxCorredor", p.get("IdxCorredor", -1)),
+                        -1,
+                    )
+
+                    if idx_ponto in indices_globais:
+                        identidade_ambigua = True
+                        raise ValueError(
+                            f"idxPonto global duplicado: {idx_ponto}"
+                        )
+
                     x, y = handler.converter_latlon_para_xz(lat, lon)
 
                     pontos_info.append({
                         "xy": (x, y),
                         "tipo": cls._int(p.get("tipo", 3), 3),
                         "distanciaMargem": cls._float(p.get("distanciaMargem", 0.7), 0.7),
+                        "idxPonto": idx_ponto,
+                        "idxCorredor": idx_corredor,
+                        "estrutural": cls._bool(
+                            p.get(
+                                "estrutural",
+                                p.get("PontoEstrutural", False),
+                            )
+                        ),
                     })
 
+                    indices_globais.add(idx_ponto)
+
                 except Exception as e:
                     mostrar_log(f"Erro ao converter ponto MPC: {p} | {e}")
 
+            if identidade_ambigua:
+                mostrar_log(
+                    "Mapa MPC rejeitado: identidade global dos pontos é ambígua."
+                )
+                return []
+
         except Exception as e:
             mostrar_log(f"Erro ao atualizar pontos do mapa: {e}")
             cls.atualizar_ctx_dict(CtxKey.DadosOperacao, pontos_mapa=pontos_info)

Firmware/Modulos/I2CService.h

@@ -3213,7 +3213,7 @@ int I2CService::_pinoTcaEn = -1;
 int I2CService::_pinoMuxRst = -1;
 int I2CService::_pinoAdsPwrEn = -1;
 
-unsigned long I2CService::LimiteTempoI2C = 2000;
+unsigned long I2CService::LimiteTempoI2C = 4000;
 
 bool I2CService::I2CIniciado = false;
 bool I2CService::MuxIniciado = false;

Firmware/Modulos/SensorTemperaturaNtcModel.h

@@ -514,7 +514,7 @@ class SensorTemperaturaNtc : public ComponenteCAN {
      * 4 NTC * 4 conversões * 2 Hz = 32 conversões/s.
      */
     static constexpr int
-      QuantidadeAmostras = 4;
+      QuantidadeAmostras = 3;
 
     /*
      * Filtro final leve.