ajustes em erro do mpc no ultimo ponto da rua, simulador agora clona o penultimo ponto, corrigido distancias das laterais, corrigido dentro do corredor

AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs

@@ -770,9 +770,9 @@ namespace AgroBase.Forms
                     ? "Aproximando"
                     : "Afastando";
             lblDistProx.Text =
-                $"Distância Próximo Ponto: {(proximoPonto?.DistanciaTrajeto ?? 0):F2} m";
+                $"Distância Próximo Ponto: {(proximoPonto?.DistanciaAtual ?? 0):F2} m";
             lblDistAnt.Text =
-                $"Distância Ponto Anterior: {(pontoMaisProximo?.DistanciaTrajeto ?? 0):F2} m";
+                $"Dist Esquerda: {dadosTrajetoria.DistanciaEsquerda * 100:F0} cm | Dist Direita: {dadosTrajetoria.DistanciaDireita * 100:F0} cm";
             lblTempoRestante.Text =
                 $"Tempo Restante: {dadosTrajetoria.TempoEstimadoRestante}";
             lblTempoOperacao.Text =

AgroBase/AgroBase/Models/MPCControllerAprimorado.cs

@@ -134,10 +134,10 @@ public class MPCControllerAprimorado
                 PontoTrajetoriaModel pontoAtual = op.Trajetoria._TrajetoriaFixa[idxNovoPonto].Clone();
 
                 int idxRuaEsquerda = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaEsquerda;
-                double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Direcao);
+                double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Posicao);
 
                 int idxRuaDireita = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaDireita;
-                double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Direcao);
+                double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Posicao);
 
                 double erroLat = Math.Abs(distEsq - distDir);
 

AgroBase/AgroBase/Models/MapaDinamicoModel.cs

@@ -1068,16 +1068,16 @@ namespace AgroBase.Models
              */
 
             float larguraEsquerdaCm =
-                (float)VariaveisEquipamento.LarguraEsquerda;
+                (float)VariaveisEquipamento.LarguraEsquerdaCm;
 
             float larguraDireitaCm =
-                (float)VariaveisEquipamento.LarguraDireita;
+                (float)VariaveisEquipamento.LarguraDireitaCm;
 
             float comprimentoFrenteCm =
-                (float)VariaveisEquipamento.ComprimentoFrente;
+                (float)VariaveisEquipamento.ComprimentoFrenteCm;
 
             float comprimentoTrasCm =
-                (float)VariaveisEquipamento.ComprimentoTras;
+                (float)VariaveisEquipamento.ComprimentoTrasCm;
 
             float larguraTotalCm =
                 larguraEsquerdaCm + larguraDireitaCm;

AgroBase/AgroBase/Models/SonarCamModel.cs

@@ -245,7 +245,7 @@ namespace AgroBase.Models
                 }
 
                 // Definir a direção do desvio com base nos percentuais
-                if (DistanciaMedia_mm < (minDepth * 1.1) && (DistanciaDaLateralDireita < VariaveisEquipamento.LarguraDireita || DistanciaLivreEsquerda < VariaveisEquipamento.LarguraEsquerda))
+                if (DistanciaMedia_mm < (minDepth * 1.1) && (DistanciaDaLateralDireita < VariaveisEquipamento.LarguraDireitaCm || DistanciaLivreEsquerda < VariaveisEquipamento.LarguraEsquerdaCm))
                 {
                     direcaoDesvio = Direcao.Parado;
                 }
@@ -331,7 +331,7 @@ namespace AgroBase.Models
         {
             if (PercentualDireita > 0 || PercentualEsquerda > 0)
             {
-                double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralEsquerda) * (1 + PercentualEsquerda)) + SonarCamModel.MargemSegurancaDesvio;
+                double deslocamento = ((((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2.0) + DistanciaDaLateralEsquerda) * (1 + PercentualEsquerda)) + SonarCamModel.MargemSegurancaDesvio;
                 if (deslocamento < 0)
                 {
                     deslocamento = 0;
@@ -348,7 +348,7 @@ namespace AgroBase.Models
         {
             if (PercentualDireita > 0 || PercentualEsquerda > 0)
             {
-                double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralDireita) * (1 + PercentualDireita)) + SonarCamModel.MargemSegurancaDesvio;
+                double deslocamento = ((((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2.0) + DistanciaDaLateralDireita) * (1 + PercentualDireita)) + SonarCamModel.MargemSegurancaDesvio;
                 if (deslocamento < 0)
                 {
                     deslocamento = 0;

AgroBase/AgroBase/Models/SonarModel.cs

@@ -627,7 +627,7 @@ namespace AgroBase.Models
                 if (Adireita && !Aesquerda)
                 {
                     //return DistanciaInicial + SonarModel.MargemSegurancaDesvio;
-                    double deslocamento = VariaveisEquipamento.LarguraDireita - DistanciaFinal + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = VariaveisEquipamento.LarguraDireitaCm - DistanciaFinal + SonarModel.MargemSegurancaDesvio;
                     if (deslocamento < 0)
                     {
                         deslocamento = 0;
@@ -637,7 +637,7 @@ namespace AgroBase.Models
                 else if (Aesquerda && !Adireita)
                 {
                     //return DistanciaFinal + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
-                    double deslocamento = VariaveisEquipamento.LarguraEsquerda + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
                     if (deslocamento < 0)
                     {
                         deslocamento = 0;
@@ -646,7 +646,7 @@ namespace AgroBase.Models
                 }
                 else if (Adireita && Aesquerda)
                 {
-                    double deslocamento = ((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2) + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
                     if (deslocamento < 0)
                     {
                         deslocamento = 0;
@@ -666,7 +666,7 @@ namespace AgroBase.Models
                 if (Adireita && !Aesquerda)
                 {
                     //return DistanciaInicial + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
-                    double deslocamento = VariaveisEquipamento.LarguraDireita + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = VariaveisEquipamento.LarguraDireitaCm + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
                     if(deslocamento < 0)
                     {
                         deslocamento = 0;
@@ -676,7 +676,7 @@ namespace AgroBase.Models
                 else if (Aesquerda && !Adireita)
                 {
                     //return DistanciaFinal + SonarModel.MargemSegurancaDesvio;
-                    double deslocamento = VariaveisEquipamento.LarguraEsquerda - DistanciaInicial + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm - DistanciaInicial + SonarModel.MargemSegurancaDesvio;
                     if (deslocamento < 0)
                     {
                         deslocamento = 0;
@@ -685,7 +685,7 @@ namespace AgroBase.Models
                 }
                 else if (Adireita && Aesquerda)
                 {
-                    double deslocamento = ((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2) + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
+                    double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
                     if (deslocamento < 0)
                     {
                         deslocamento = 0;

AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs

@@ -416,33 +416,43 @@ namespace AgroBase.Models
         public double DistanciaDireita { get; private set; }
         private void AtualizarDistanciasLaterais()
         {
-            DistanciaEsquerda = CalcularDistanciaLateral(true, GPSPosicaoAtual, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
+            var referenciaCorredor = PontoMaisProximo?.Posicao ?? PontoAtual?.Posicao;
-            DistanciaDireita = CalcularDistanciaLateral(false, GPSPosicaoAtual, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
+            DistanciaEsquerda = CalcularDistanciaLateral(true, GPSPosicaoAtual, CorredorAtual?.idxRuaEsquerda ?? -1, referenciaCorredor);
+            DistanciaDireita = CalcularDistanciaLateral(false, GPSPosicaoAtual, CorredorAtual?.idxRuaDireita ?? -1, referenciaCorredor);
         }
-
+        public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, GPSModel referenciaCorredor)
-        public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
         {
-            if (posicaoAtual == null) return 0;
+            if (posicaoAtual == null)
+                return 0;
 
-            // Verifica se o corredor atual existe
+            if (!_CorredorAtualDefinido)
-            if (!_CorredorAtualDefinido) return 0;
+                return 0;
 
             if (RuasPlantacao == null || idxRua < 0 || idxRua >= RuasPlantacao.Count)
                 return 0;
 
+            if (referenciaCorredor == null)
+                return 0;
+
             List<GPSModel> rua = new List<GPSModel>(RuasPlantacao[idxRua]);
 
             if (rua.Count < 2)
                 return 0;
 
-            double d0 = double.MaxValue;
+            // =========================================================
+            // 1. Encontrar região da rua mais próxima do rover
+            // =========================================================
+
+            double menorDistancia = double.MaxValue;
             int idx = -1;
+
             for (int i = 0; i < rua.Count; i++)
             {
                 double dist = GPSUtils.DistanciaEntrePontos(posicaoAtual, rua[i]);
-                if (dist < d0)
+
+                if (dist < menorDistancia)
                 {
-                    d0 = dist;
+                    menorDistancia = dist;
                     idx = i;
                 }
             }
@@ -450,52 +460,86 @@ namespace AgroBase.Models
             if (idx < 0)
                 return 0;
 
-            GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)];
+            int idxPa = Math.Max(0, idx - 1);
-            GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)];
+            int idxPb = Math.Min(rua.Count - 1, idx + 1);
-            int idxPa = rua.IndexOf(p0);
-            int idxPb = rua.IndexOf(p1);
-            double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[idxPb], rua[idxPa]);
-            double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[idxPa], rua[idxPb]);
 
-            GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[idxPa], anguloAprojetar, DistanciaManobraEntreRuas);
+            if (idxPa == idxPb)
-            GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[idxPb], anguloBprojetar, DistanciaManobraEntreRuas);
+                return 0;
+
+            GPSModel pa = rua[idxPa];
+            GPSModel pb = rua[idxPb];
+
+            // =========================================================
+            // 2. Prolongar o segmento local
+            // =========================================================
+
+            double anguloA = GPSUtils.CalcularOrientacao(pb, pa);
+            double anguloB = GPSUtils.CalcularOrientacao(pa, pb);
+            GPSModel pontoA = GPSUtils.GerarPontoDeslocado(pa, anguloA, DistanciaManobraEntreRuas);
+            GPSModel pontoB = GPSUtils.GerarPontoDeslocado(pb, anguloB, DistanciaManobraEntreRuas);
+
+            var trechoRua = new List<GPSModel>
+            {
+                pontoA
+            };
 
-            List<GPSModel> novaRua = new List<GPSModel>();
-            novaRua.Add(pontoA);
             for (int i = idxPa; i <= idxPb; i++)
-            {
+                trechoRua.Add(rua[i]);
-                novaRua.Add(rua[i]);
-            }
-            novaRua.Add(pontoB);
 
-            rua = novaRua;
+            trechoRua.Add(pontoB);
+            int qtdPontos = Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(trechoRua))) + 1);
+            trechoRua = InterpolarRotaPorDistancia(trechoRua, qtdPontos);
 
-            int qtdPontosRua = Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(rua))) + 1);
+            // =========================================================
-            rua = InterpolarRotaPorDistancia(rua, Math.Max(2, qtdPontosRua));
+            // 3. Distância absoluta centro do rover -> rua
+            // =========================================================
 
-            // Calcula a menor distância entre o robô e a rua
+            double distanciaCentro = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, trechoRua);
-            double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, rua);
 
-            // Calcula o vetor entre os pontos da rua e o vetor entre o ponto A e o GPS
+            // =========================================================
-            double crossProduct =
+            // 4. Descobrir qual lado da rua é o INTERIOR do corredor
-                (pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) -
+            //
-                (pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude);
+            // Não depende do sentido A->B da polyline.
+            // =========================================================
 
-            // Ajusta a distância com base no sentido de deslocamento do robô
+            double crossReferencia =
-            if ((direcaoAtual == DirecaoCarroRua.Volta && ladoEsquerdo) || (direcaoAtual == DirecaoCarroRua.Ida && !ladoEsquerdo))
+                (pontoB.Longitude - pontoA.Longitude) *
-            {
+                (referenciaCorredor.Latitude - pontoA.Latitude) -
-                distancia *= crossProduct > 0 ? 1 : -1;
+                (pontoB.Latitude - pontoA.Latitude) *
-            }
+                (referenciaCorredor.Longitude - pontoA.Longitude);
-            else
-            {
-                distancia *= crossProduct > 0 ? -1 : 1;
-            }
 
-            // Ajusta a largura do robô no cálculo final
+            double crossRobo =
-            double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
+                (pontoB.Longitude - pontoA.Longitude) *
-            distancia -= (larguraAjuste / 100.0);
+                (posicaoAtual.Latitude - pontoA.Latitude) -
+                (pontoB.Latitude - pontoA.Latitude) *
+                (posicaoAtual.Longitude - pontoA.Longitude);
 
-            return distancia;
+            /*
+             * A trajetória central define o lado interno da rua.
+             *
+             * Mesmo sinal:
+             * rover está do lado interno.
+             *
+             * Sinal diferente:
+             * rover atravessou a rua e está fora do corredor.
+             */
+
+            const double epsilon = 1e-15;
+
+            bool referenciaSobreLinha = Math.Abs(crossReferencia) <= epsilon;
+            bool roboSobreLinha = Math.Abs(crossRobo) <= epsilon;
+
+            bool mesmoLado = referenciaSobreLinha || roboSobreLinha || Math.Sign(crossReferencia) == Math.Sign(crossRobo);
+            double distanciaAssinada = mesmoLado ? distanciaCentro : -distanciaCentro;
+
+            // =========================================================
+            // 5. Converter distância do CENTRO para folga da CARROCERIA
+            // =========================================================
+
+            double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerdaCm : VariaveisEquipamento.LarguraDireitaCm;
+            double folga = distanciaAssinada - larguraAjuste / 100.0;
+
+            return folga;
         }
         [JsonProperty]
         public PontoTrajetoriaModel PontoAtual { get; private set; }
@@ -3432,7 +3476,7 @@ namespace AgroBase.Models
                 double larguraMaximaSegura = Math.Max(6.0, LarguraCorredorPadrao * 4.0);
                 double larguraRover = Math.Max(
                     0,
-                    (VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 100.0
+                    (VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 100.0
                 );
                 double larguraMinimaSegura = Math.Max(0.50, larguraRover + 0.15);
 
@@ -3765,7 +3809,7 @@ namespace AgroBase.Models
                 Corredores[idx] = CorredorAtual;
 
                 double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).FirstOrDefault(), CorredorAtual.FirstOrDefault());
-                GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua, anguloProjetar1);
+                GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua * 0.8, anguloProjetar1);
 
                 var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
 
@@ -3837,7 +3881,7 @@ namespace AgroBase.Models
                     Direcao = direcaoAtual,
                     Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
                     Visitado = false,
-                    LarguraCorredor = ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
+                    LarguraCorredor = larguraCorredorMenor // ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
                 };
                 _trajetoriaFixa.Add(PontoFinal);
 
@@ -5422,12 +5466,16 @@ namespace AgroBase.Models
         public double FatorLarguraCorredor { get; set; }
         public DadosVisuaisCorredor DadosVisuais { get; set; } = new DadosVisuaisCorredor();
 
+
+        private int _confirmacoesForaCorredor = 0;
+        private const int ConfirmacoesSaidaCorredor = 3;
         public void AtualizarDados()
         {
-            Dentro = false;
-
             if (Pontos == null || Pontos.Count == 0)
             {
+                Dentro = false;
+                _confirmacoesForaCorredor = 0;
+
                 DistanciaRestante = 0;
                 Progresso = 0;
                 Concluido = true;
@@ -5436,46 +5484,108 @@ namespace AgroBase.Models
 
             int idxReferencia = Pontos.FindLastIndex(x => x.Visitado);
             idxReferencia = Math.Max(0, idxReferencia);
+
             int idxInicio = Math.Max(0, idxReferencia - 2);
             int idxFim = Math.Min(Pontos.Count - 1, idxReferencia + 3);
 
-            /*
+            bool dentroAgora = false;
-             * Somente a vizinhanca do progresso atual pode definir Dentro.
+
-             * Pontos visitados antigos conservam propriedades diagnosticas,
-             * mas nao podem manter o rover eternamente dentro da rua.
-             */
             for (int i = idxInicio; i <= idxFim; i++)
             {
                 var ponto = Pontos[i];
 
-                if (
+                if (ponto == null)
+                    continue;
+
+                /*
+                 * Ponto Rua representa geometria física.
+                 * Não precisa já ter sido confirmado como Visitado
+                 * para provar que o rover está dentro da passada.
+                 */
+                bool ruaValida =
+                    ponto.Tipo == TipoPontoRua.Rua &&
+                    ponto.NaMargem;
+
+                /*
+                 * Bordas continuam exigindo progresso confirmado,
+                 * para não declarar entrada/saída prematuramente.
+                 */
+                bool bordaValida =
                     ponto.Visitado &&
                     ponto.NaMargem &&
-                    (ponto.Tipo == TipoPontoRua.Rua ||
+                    (
-                        (
+                        (ponto.Tipo == TipoPontoRua.BordaEntrada &&
-                            (ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) ||
+                         !ponto.Aproximando)
-                            (ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
+                        ||
-                        )
+                        (ponto.Tipo == TipoPontoRua.BordaSaida &&
-                    )
+                         ponto.Aproximando)
-                )
+                    );
+
+                if (ruaValida || bordaValida)
                 {
-                    Dentro = true;
+                    dentroAgora = true;
                     break;
                 }
             }
 
-            var naoVisitados = Pontos.Where(x => !x.Visitado).ToList();
+            /*
-            DistanciaRestante = GPSUtils.DistanciaDoTrecho(naoVisitados.Select(x => x.Posicao).ToList());
+             * Entrada é imediata.
+             * Saída exige algumas amostras consecutivas.
+             *
+             * Isso impede jitter GNSS/waypoint de transformar
+             * uma passada reta em Direcionando por um único ciclo.
+             */
+            if (dentroAgora)
+            {
+                Dentro = true;
+                _confirmacoesForaCorredor = 0;
+            }
+            else if (Dentro)
+            {
+                _confirmacoesForaCorredor++;
+
+                if (_confirmacoesForaCorredor >= ConfirmacoesSaidaCorredor)
+                {
+                    Dentro = false;
+                    _confirmacoesForaCorredor = 0;
+                }
+            }
+            else
+            {
+                Dentro = false;
+            }
+
+            var naoVisitados = Pontos
+                .Where(x => !x.Visitado)
+                .ToList();
+
+            DistanciaRestante =
+                GPSUtils.DistanciaDoTrecho(
+                    naoVisitados.Select(x => x.Posicao).ToList()
+                );
 
             if (naoVisitados.Count > 0)
             {
-                double distanciaEntrada = naoVisitados[0].DistanciaAtual;
+                double distanciaEntrada =
+                    naoVisitados[0].DistanciaAtual;
 
-                if (!double.IsNaN(distanciaEntrada) && !double.IsInfinity(distanciaEntrada) && distanciaEntrada > 0)
+                if (!double.IsNaN(distanciaEntrada) &&
+                    !double.IsInfinity(distanciaEntrada) &&
+                    distanciaEntrada > 0)
+                {
                     DistanciaRestante += distanciaEntrada;
+                }
             }
 
-            Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaCorredor / DistanciaTotal) * 100.0, 0, 100) : 0;
+            Progresso =
+                DistanciaTotal > 0
+                    ? FuncoesMatematicas.Clamp(
+                        (DistanciaPercorridaCorredor / DistanciaTotal) * 100.0,
+                        0,
+                        100
+                    )
+                    : 0;
+
             Concluido = naoVisitados.Count == 0;
         }
 
@@ -5592,11 +5702,7 @@ namespace AgroBase.Models
             );
         }
 
-        public void AtualizarPropriedades(
+        public void AtualizarPropriedades(GPSModel posicaoAtual, GPSModel posicaoAnterior, bool permitirMarcarVisitado = true)
-            GPSModel posicaoAtual,
-            GPSModel posicaoAnterior,
-            bool permitirMarcarVisitado = true
-        )
         {
             AtualizarDistanciaAtual(posicaoAtual);
             AtualizarDistanciaAnterior(posicaoAnterior);
@@ -5612,31 +5718,49 @@ namespace AgroBase.Models
 
         private void AtualizarVisitado()
         {
-            if (Visitado) return; // Se já foi visitado, não faz nada
+            if (Visitado)
+                return;
 
             double espacamento =
-                Tipo == TipoPontoRua.CruvaEntreCorredores || Tipo == TipoPontoRua.Desvio
+                Tipo == TipoPontoRua.CruvaEntreCorredores ||
+                Tipo == TipoPontoRua.Desvio
                     ? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva
                     : TrajetoriaMapaOperacaoModel.DistanciaEntrePontos;
 
             espacamento = Math.Max(0.10, espacamento);
 
-            double limiteDistancia =
+            double limiteDistancia = espacamento * 0.75 + Math.Max(0, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras);
-                espacamento * 0.75 +
-                Math.Max(0, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras);
-
             limiteDistancia = Math.Max(0.25, Math.Min(DistanciaMargem, limiteDistancia));
 
-            if (PontoBorda || PontoLigacao)
+            bool estrutural = PontoBorda || PontoLigacao;
+
+            if (estrutural)
+            {
+                /*
+                 * Bordas e ligações são marcos estruturais.
+                 *
+                 * Quando este método é chamado com autorização de visita
+                 * no fluxo normal, o ponto já é o próximo ponto
+                 * autoritativo da trajetória.
+                 *
+                 * Em manobras o bearing para o ponto pode mudar rapidamente,
+                 * tornando "Aproximando" instável mesmo quando o rover passa
+                 * fisicamente sobre o waypoint.
+                 */
                 limiteDistancia = Math.Min(limiteDistancia, 0.80);
 
-            bool cruzouJanelaDeAceitacao =
+                if (NaMargem && DistanciaAtual <= limiteDistancia)
-                DistanciaAtual <= limiteDistancia &&
+                {
-                (
+                    Visitado = true;
-                    Aproximando ||
+                }
-                    DistanciaAnterior <= limiteDistancia ||
+
-                    DistanciaAtual <= 0.25
+                return;
-                );
+            }
+
+            /*
+             * Pontos comuns continuam usando a janela endurecida.
+             */
+            bool cruzouJanelaDeAceitacao = DistanciaAtual <= limiteDistancia && (Aproximando || DistanciaAnterior <= limiteDistancia || DistanciaAtual <= 0.25);
 
             if (NaMargem && cruzouJanelaDeAceitacao)
             {

AgroBase/AgroBase/Models/Variaveis.cs

@@ -821,10 +821,10 @@ namespace AgroBase.Models
         public static string TopicoMqttComandos { get; } = $"agrobot/v1/rover/<id>/cmd";
         public static string TopicoMqttTelemetria { get; } = $"agrobot/v1/rover/<id>/telemetry";
         public static string TopicoMqttParametros { get; } = $"agrobot/v1/rover/<id>/parameters";
-        public static double LarguraEsquerda { get; } = 53.0; // 62
+        public static double LarguraEsquerdaCm { get; } = 53.0; // 62
-        public static double LarguraDireita { get; } = 53.0; // 22
+        public static double LarguraDireitaCm { get; } = 53.0; // 22
-        public static double ComprimentoFrente { get; } = 90.0; // 7
+        public static double ComprimentoFrenteCm { get; } = 90.0; // 7
-        public static double ComprimentoTras { get; } = 40.0; // 107
+        public static double ComprimentoTrasCm { get; } = 40.0; // 107
         public static double DistanciaEntreEixosCm { get; set; } = 92.0;
         public static double BitolaRodasCm { get; set; } = 75.0;
         public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
@@ -833,7 +833,7 @@ namespace AgroBase.Models
         {
             get
             {
-                return ((LarguraEsquerda + LarguraDireita + 10.0) * 10.0);
+                return ((LarguraEsquerdaCm + LarguraDireitaCm + 10.0) * 10.0);
             }
         }
         public static double TensaoMinimaBateria { get; set; } = 30.0;

AgroBase/AgroBase/Services/GPSService.cs

@@ -209,6 +209,19 @@ namespace AgroBase.Services
 
             lock (_stateLock)
             {
+                /*
+                 * A leitura simulada deve obedecer à mesma semântica temporal
+                 * da leitura GNSS real:
+                 *
+                 * PenultimaLeitura = amostra imediatamente anterior
+                 * UltimaLeitura    = nova amostra
+                 *
+                 * Sem isso, a posição anterior fica congelada durante toda
+                 * a simulação e contamina Aproximando, orientação anterior,
+                 * deslocamento GNSS e recuperação da trajetória.
+                 */
+                CopiarPosicaoParaPenultima();
+
                 double orientacaoReal =
                     GPSUtils.NormalizarAngulo(leitura.OrientacaoReal);
 
@@ -248,11 +261,10 @@ namespace AgroBase.Services
                 UltimaLeitura.Heartbeat = leitura.Heartbeat;
                 UltimaLeitura.Inicializado = true;
 
-                // Monitor/lock do C# é reentrante; os dois métodos abaixo usam o
-                // mesmo lock e permanecem em uma única transação de leitura GNSS.
                 AtualizarAtrasoPosicoes(
                     Math.Max(0, atrasoPosicaoPassos)
                 );
+
                 AtualizarCoordenadasGPS();
             }
         }

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

@@ -1597,6 +1597,7 @@ class ControladorMPC:
             tipo_anterior = TipoMovimentoDirecional(comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value))
             simulacao_latlon = []
             debug_custo = {}
+            dados_costmap_ciclo = {}
 
             ultimo_ponto = idx_proximo_ponto_real >= len(self.pontos_info)
             if ultimo_ponto: