aplicacao do lever arm

AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs

@@ -12,10 +12,8 @@ using System.Drawing.Drawing2D;
 using System.Globalization;
 using System.IO;
 using System.Linq;
-using System.Security.Cryptography;
 using System.Threading.Tasks;
 using System.Windows.Forms;
-using static AgroBase.Models.Enums;
 
 namespace AgroBase.Forms.Operacoes
 {
@@ -118,7 +116,7 @@ namespace AgroBase.Forms.Operacoes
             btnPlay.Text = tmrPlay.IsRunning ? "Pause" : "Pay";
         }
 
-        private void btnCarregarOperacao_Click(object sender, EventArgs e)
+        private async void btnCarregarOperacao_Click(object sender, EventArgs e)
         {
             OpenFileDialog ofd = new OpenFileDialog();
             ofd.Filter = "Arquivos de Operação|*.lgop";
@@ -129,15 +127,12 @@ namespace AgroBase.Forms.Operacoes
 
                 LogsOperacao = JsonConvert.DeserializeObject<OperacaoSensoriamentoLogModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, data.operacao)).ToList();
 
-                Task.Run(async () =>
-                {
                 GPSModel pos = GPSService.UltimaLeitura.Clone();
                 GPSService.UltimaLeitura = LogsOperacao.FirstOrDefault().Gps;
                 (bool sucesso, List<string> ruas) = Variaveis.OperacaoEmAndamento.CarregarParametrizacaoOperacao(null, null, ofd.FileName.Replace("data.lgop", "operacao.opr"));
                 bool condicaoOperacaoCarregada() => Variaveis.OperacaoEmAndamento.Trajetoria?.CorredorAtual != null;
                 bool resultado = await FuncoesGlobais.AguardarCondicaoAsync(condicaoOperacaoCarregada, 15000, 200);
                 GPSService.UltimaLeitura = pos;
-                });
 
                 LogsDirecional = JsonConvert.DeserializeObject<DirSensoriamentoLogModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, data.dir ?? Enums.T_Code.Dir.ToString())).ToList();
                 LogsMovimentacao = JsonConvert.DeserializeObject<MovSensoriamentoLogModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, data.mov ?? Enums.T_Code.Mov.ToString())).ToList();
@@ -1612,8 +1607,7 @@ namespace AgroBase.Forms.Operacoes
                     double yVal = ordenarPorCustoTotal ? m.custo_total : m.custo_map;
 
                     // nó do ângulo (resumo compacto)
-                    var angNode = tipoNode.Nodes.Add(
+                    var angNode = tipoNode.Nodes.Add($"ang {angleTxt}°  #{r.rank}  Δ={r.margin:0.000}  {yLabel}={yVal:0.0000}");
-                        $"ang {angleTxt}°  #{r.rank}  Δ={r.margin:0.000}  {yLabel}={yVal:0.0000}");
 
                     angNode.Tag = kv.Key;
 
@@ -1627,34 +1621,49 @@ namespace AgroBase.Forms.Operacoes
                     {
                         angNode.ForeColor = Color.DarkOrange;
                     }
+                    if (m.melhor)
+                    {
+                        angNode.NodeFont = new Font(tv.Font, FontStyle.Bold);
+                        angNode.ForeColor = Color.DarkBlue;
+                    }
 
                     // ---- Subgrupos
                     var pose = angNode.Nodes.Add("Pose/Alvo");
-                    pose.Nodes.Add($"x={m.x_sim:0.3}  y={m.y_sim:0.3}  θ={m.theta_sim:0.00}");
+                    pose.Nodes.Add($"Rover: X={m.x_sim:0.00000}  Y={m.y_sim:0.00000}  θ={m.theta_sim:0.00}");
-                    pose.Nodes.Add($"alvo: x={m.x_alvo:0.3}  y={m.y_alvo:0.3}");
+                    pose.Nodes.Add($"Alvo: X={m.x_alvo:0.00000}  Y={m.y_alvo:0.00000}");
 
                     var errs = angNode.Nodes.Add("Erros");
-                    errs.Nodes.Add($"epos={m.erro_posicao:0.3}  eposn={m.erro_posicao_n:0.3}");
+                    errs.Nodes.Add($"Posição={m.erro_posicao:0.00000}  Normalizado={m.erro_posicao_n:0.00000}");
-                    errs.Nodes.Add($"eori={m.erro_orientacao:0.3}  eorin={m.erro_orientacao_n:0.4}");
+                    errs.Nodes.Add($"Orientação={m.erro_orientacao:0.00000}  Normalizado={m.erro_orientacao_n:0.00000}");
-                    errs.Nodes.Add($"elat={m.erro_lateral:0.3}  elatn={m.erro_lateral_n:0.4}");
+                    errs.Nodes.Add($"Suavização={m.erro_orientacao:0.00000}");
-                    errs.Nodes.Add($"ere={m.erro_re:0.3}");
+                    errs.Nodes.Add($"Ré={m.erro_re:0.00000}");
+                    errs.Nodes.Add($"Lateral={m.erro_lateral:0.00000}  Normalizado={m.erro_lateral_n:0.00000}");
+                    
 
                     var costs = angNode.Nodes.Add("Custos");
-                    costs.Nodes.Add($"cpos={m.custo_posicao:0.4}  cori={m.custo_orientacao:0.4}");
+                    costs.Nodes.Add($"Posição={m.custo_posicao:0.00000}");
-                    costs.Nodes.Add($"csua={m.custo_suavizacao:0.4}  cre={m.custo_re:0.4}");
+                    costs.Nodes.Add($"Orientação={m.custo_orientacao:0.00000}");
-                    costs.Nodes.Add($"clat={m.custo_lateral:0.4}  clatd={m.custo_lateral_d:0.4}");
+                    costs.Nodes.Add($"Suavização={m.custo_suavizacao:0.00000}");
-                    costs.Nodes.Add($"cvw={m.custo_visual_worker:0.4}");
+                    costs.Nodes.Add($"Ré={m.custo_re:0.00000}");
-                    costs.Nodes.Add($"cmap={m.custo_map:0.4}  ctot={m.custo_total:0.4}");
+                    costs.Nodes.Add($"Ideal={m.custo_visual_worker:0.00000}");
+                    costs.Nodes.Add($"Lateral={m.custo_lateral:0.00000}");
+                    costs.Nodes.Add($"Lateral Derivado={m.custo_lateral_d:0.00000}");
+                    costs.Nodes.Add($"Lateral Flip={m.custo_flip:0.00000}");
+                    costs.Nodes.Add($"Somatória={m.custo_map:0.00000}");
+                    costs.Nodes.Add($"Total={m.custo_total:0.00000}");
 
                     var pesos = angNode.Nodes.Add("Pesos");
-                    pesos.Nodes.Add($"pos={m.peso_posicao:0.2}  ori={m.peso_orientacao:0.2}");
+                    pesos.Nodes.Add($"Posição={m.peso_posicao:0.00}");
-                    pesos.Nodes.Add($"suav={m.peso_suavizacao:0.2}  re={m.peso_re:0.2}  ideal={m.peso_ideal:0.2}");
+                    pesos.Nodes.Add($"Orientação={m.peso_orientacao:0.00}");
-                    pesos.Nodes.Add($"lat={m.peso_lateral:0.2}");
+                    pesos.Nodes.Add($"Suavização={m.peso_suavizacao:0.00}");
+                    pesos.Nodes.Add($"Ré={m.peso_re:0.00}");
+                    pesos.Nodes.Add($"Ideal={m.peso_ideal:0.00}");
+                    pesos.Nodes.Add($"Lateral={m.peso_lateral:0.00}");
                     if (m.peso_tipo_movimento != null)
                     {
-                        var ptip = pesos.Nodes.Add("tipo_mov");
+                        var ptip = pesos.Nodes.Add("Tipo de Movimento");
                         foreach (var kvp in m.peso_tipo_movimento)
-                            ptip.Nodes.Add($"{kvp.Key}: {kvp.Value:0.2}");
+                            ptip.Nodes.Add($"{kvp.Key}: {kvp.Value:0.00}");
                     }
                 }
             }

AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs

@@ -202,7 +202,11 @@ namespace AgroBase.Forms
             posicaoAtual = new GPSModel()
             {
                 DataHora = DateTime.Now,
+                Lon0 = double.Parse(txtLongitude.Text.Replace(".", ",")),
+                LongitudeAnt = double.Parse(txtLongitude.Text.Replace(".", ",")),
                 Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")),
+                LatitudeAnt = double.Parse(txtLatitude.Text.Replace(".", ",")),
+                Lat0 = double.Parse(txtLatitude.Text.Replace(".", ",")),
                 Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")),
                 Heartbeat = GPSService.UltimaLeitura.Heartbeat,
                 OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")),
@@ -222,7 +226,9 @@ namespace AgroBase.Forms
             {
                 DataHora = GPSService.UltimaLeitura.DataHora,
                 Longitude = GPSService.UltimaLeitura.Longitude,
+                LongitudeAnt = GPSService.UltimaLeitura.LongitudeAnt,
                 Latitude = GPSService.UltimaLeitura.Latitude,
+                LatitudeAnt = GPSService.UltimaLeitura.LatitudeAnt,
                 Heartbeat = GPSService.UltimaLeitura.Heartbeat,
                 OrientacaoReal = GPSService.UltimaLeitura.OrientacaoReal
             };

AgroBase/AgroBase/Models/GPSModel.cs

@@ -14,8 +14,15 @@ namespace AgroBase.Models
         public CanCommandFreqModel<double> TimestampOri { get; set; } = new CanCommandFreqModel<double>();
         public bool Inicializado { get; set; }
         public bool Ntrip_ativado { get; set; }
+        public bool EnuOriginSet { get; set; }
+        public double Lat0 { get; set; }
+        public double LatitudeAnt { get; set; }
         public double Latitude { get; set; }
+        public double X { get; set; }
+        public double LongitudeAnt { get; set; }
+        public double Lon0 { get; set; }
         public double Longitude { get; set; }
+        public double Y { get; set; }
         public DateTime DataHora { get; set; }
         public double Altitude { get; set; }
         public double AltitudeElipsoidal { get; set; }
@@ -71,7 +78,12 @@ namespace AgroBase.Models
                 Altitude = Altitude,
                 AltitudeElipsoidal = AltitudeElipsoidal, 
                 DataHora = DataHora,
+                EnuOriginSet = EnuOriginSet,
+                Lat0 = Lat0,
+                LatitudeAnt = LatitudeAnt, 
                 Latitude = Latitude,
+                Lon0 = Lon0,
+                LongitudeAnt = LongitudeAnt,
                 Longitude = Longitude,
                 PrecisaoHorizontal = PrecisaoHorizontal,
                 Velocidade = Velocidade,

AgroBase/AgroBase/Models/MapaDinamicoModel.cs

@@ -156,7 +156,25 @@ namespace AgroBase.Models
                 DrawTrajectory(g, _trajetoriaRobo, centerX, centerY, Color.Orange, Pens.OrangeRed);
 
                 // Desenha a posição atual do robô (ponto vermelho)
-                float PxToCm = DrawRobot(g, centerX, centerY, Color.Red, Color.Red, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)VariaveisEquipamento.ComprimentoFrente, (float)VariaveisEquipamento.ComprimentoTras, AnguloCarro);
+                //float PxToCm = DrawRobot(g, centerX, centerY, Color.Red, Color.Red, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)VariaveisEquipamento.ComprimentoFrente, (float)VariaveisEquipamento.ComprimentoTras, AnguloCarro);
+                // === NOVO: pega pixels do CENTRO (corrigido) e da ANTENA (bruto) ===
+                // Use sua própria função de conversão se já tiver.
+                PointF pCtr = LatLonToPanel(_posicaoAtual.Latitude, _posicaoAtual.Longitude, centerX, centerY);
+                PointF pAnt = LatLonToPanel(_posicaoAtual.LatitudeAnt, _posicaoAtual.LongitudeAnt, centerX, centerY);
+
+                // desenha o robô centrado no ponto corrigido e marca a antena bruta
+                float PxToCm = DrawRobotNew(
+                    g,
+                    pCtr.X, pCtr.Y,
+                    Color.ForestGreen,                           // cor do contorno do corpo
+                    (float)VariaveisEquipamento.LarguraEsquerda,
+                    (float)VariaveisEquipamento.LarguraDireita,
+                    (float)VariaveisEquipamento.ComprimentoFrente,
+                    (float)VariaveisEquipamento.ComprimentoTras,
+                    AnguloCarro,                                 // em rad
+                    pAnt.X, pAnt.Y,                              // antena "bruta" (opcional)
+                    desenharAntenaEsperada: true                 // mostra também a antena “prevista”
+                );
 
                 if (obstaculoDetectado != null)
                 {
@@ -315,6 +333,79 @@ namespace AgroBase.Models
             return PxToCm;
         }
 
+        private float DrawRobotNew(
+            Graphics g,
+            float xCtr, float yCtr,                 // centro (corrigido) em pixels
+            Color bodyBorder,
+            float larguraEsquerdaCm, float larguraDireitaCm,
+            float compFrenteCm, float compTrasCm,
+            float angleRad,                         // mesmo sistema do seu theta (0=Norte)
+            float? xAnt = null, float? yAnt = null, // antena bruta (pixels) - opcional
+            bool desenharAntenaEsperada = false     // desenhar antena “prevista” a partir do centro
+        )
+        {
+            float zoom = (float)_zoom;
+
+            // dimensões em cm
+            float larguraTotalCm = larguraEsquerdaCm + larguraDireitaCm;
+            float comprimentoTotalCm = compFrenteCm + compTrasCm;
+
+            // escala -> pixels (mantenha sua convenção)
+            float cmToPx = zoom / 1e7f;   // mesma que você já usa
+            float larguraPx = larguraTotalCm * cmToPx;
+            float comprimentoPx = comprimentoTotalCm * cmToPx;
+
+            // retângulo do robô centrado no (xCtr,yCtr)
+            PointF[] corners = new PointF[4];
+            corners[0] = CalcularPontoRotacionado(xCtr, yCtr, -larguraPx / 2f, -comprimentoPx / 2f, angleRad);
+            corners[1] = CalcularPontoRotacionado(xCtr, yCtr, +larguraPx / 2f, -comprimentoPx / 2f, angleRad);
+            corners[2] = CalcularPontoRotacionado(xCtr, yCtr, +larguraPx / 2f, +comprimentoPx / 2f, angleRad);
+            corners[3] = CalcularPontoRotacionado(xCtr, yCtr, -larguraPx / 2f, +comprimentoPx / 2f, angleRad);
+
+            using (var pen = new Pen(bodyBorder, 2f))
+                g.DrawPolygon(pen, corners);
+
+            // setinha de “frente” partindo do centro (opcional)
+            float frenteLenPx = compFrenteCm * cmToPx;
+            PointF pontaFrente = CalcularPontoRotacionado(xCtr, yCtr, 0f, -frenteLenPx, angleRad);
+            using (var penFront = new Pen(bodyBorder, 2f))
+                g.DrawLine(penFront, xCtr, yCtr, pontaFrente.X, pontaFrente.Y);
+
+            // === Antena bruta (se veio lat/lon da antena) ===
+            if (xAnt.HasValue && yAnt.HasValue)
+            {
+                // ligue centro ↔ antena (visual da alavanca)
+                using (var penLink = new Pen(Color.DarkGray, 1.5f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash })
+                    g.DrawLine(penLink, xCtr, yCtr, xAnt.Value, yAnt.Value);
+
+                // ponto da antena (bruto)
+                DrawPoint(g, xAnt.Value, yAnt.Value, Color.OrangeRed);  // cor diferente
+            }
+
+            // === Antena “esperada” calculada a partir do centro (sanity check) ===
+            if (desenharAntenaEsperada)
+            {
+                // offsets em cm do CENTRO para a antena (mesma lógica que você já tinha)
+                float dxCm = (larguraTotalCm / 2f) - larguraDireitaCm; // +esquerda
+                float dyCm = (comprimentoTotalCm / 2f) - compFrenteCm;     // +trás
+
+                // para o sistema de tela, seu Y “frente” foi -dy (igual ao que você fazia)
+                float dxPx = dxCm * cmToPx;
+                float dyPx = dyCm * cmToPx;
+
+                PointF antenaPrevista = CalcularPontoRotacionado(xCtr, yCtr, dxPx, -dyPx, angleRad);
+                DrawPoint(g, antenaPrevista.X, antenaPrevista.Y, Color.DeepSkyBlue);
+
+                // e ligue também, para comparar “previsto” x “bruto”
+                using (var penLink2 = new Pen(Color.DeepSkyBlue, 1f))
+                    g.DrawLine(penLink2, xCtr, yCtr, antenaPrevista.X, antenaPrevista.Y);
+            }
+
+            // relação px->cm (se você usa depois)
+            float pxToCm = larguraTotalCm / Math.Max(larguraPx, 1e-6f);
+            return pxToCm;
+        }
+
         private PointF CalcularPontoRotacionado(float centroX, float centroY, float offsetX, float offsetY, float angle)
         {
             // Converter o ângulo para radianos
@@ -391,5 +482,19 @@ namespace AgroBase.Models
         }
 
 
+
+
+        private PointF LatLonToPanel(double lat, double lon, float centerX, float centerY)
+        {
+            // Usa a mesma origem ENU do GeoLeverArm
+            var (xE, yN) = GeoLeverArm.GeodeticToENU(lat, lon, _posicaoAtual.Lat0, _posicaoAtual.Lon0);
+            float scale = (float)_zoom / 1e7f;
+
+            float px = centerX + (float)(xE * scale);  // Leste para +X
+            float py = centerY - (float)(yN * scale);  // Norte para -Y (tela cresce para baixo)
+
+            return new PointF(px, py);
+        }
+
     }
 }

AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs

@@ -95,6 +95,7 @@ namespace AgroBase.Models.Operadores
         public double custo_visual_worker { get; set; }
         public double custo_map { get; set; }
         public double custo_total { get; set; }
+        public bool melhor { get; set; }
     }
 
     public class CandMpcKey

AgroBase/AgroBase/Models/Variaveis.cs

@@ -404,8 +404,12 @@ namespace AgroBase.Models
             GPSModel novaPosicao = new GPSModel
             {
                 Momento = DateTime.Now,
+                Lat0 = GPSService.UltimaLeitura.Lat0, 
+                Lon0 = GPSService.UltimaLeitura.Lon0, 
                 Latitude = pos.Latitude + dLat,
+                LatitudeAnt = pos.Latitude + dLat,
                 Longitude = pos.Longitude + dLon,
+                LongitudeAnt = pos.Longitude + dLon,
                 OrientacaoReal = GPSUtils.NormalizarAngulo(theta * 180.0 / Math.PI),
                 Distancia = velocidadeMs * tempoDelta,
                 Velocidade = velocidadeMs,

AgroBase/AgroBase/Services/GPSService.cs

@@ -434,7 +434,9 @@ namespace AgroBase.Services
 
             PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
             PenultimaLeitura.Momento = UltimaLeitura.Momento;
+            PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
             PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
+            PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
             PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
             PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
             PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal;
@@ -448,7 +450,9 @@ namespace AgroBase.Services
             // Armazenar os valores na última leitura
             UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
             UltimaLeitura.Momento = DateTime.Now;
+            UltimaLeitura.LatitudeAnt = latitude;
             UltimaLeitura.Latitude = latitude;
+            UltimaLeitura.LongitudeAnt = longitude;
             UltimaLeitura.Longitude = longitude;
             UltimaLeitura.Altitude = altMSL;
             UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
@@ -470,6 +474,48 @@ namespace AgroBase.Services
                 var currentDateUtc = DateTime.UtcNow.Date;
                 UltimaLeitura.DataHora = currentDateUtc.Add(ts).ToLocalTime();
             }
+
+            // 1) define origem ENU na primeira leitura válida
+            if (!UltimaLeitura.EnuOriginSet && UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.RTKFixo) // tem fix
+            {
+                UltimaLeitura.Lat0 = latitude;
+                UltimaLeitura.Lon0 = longitude;
+                UltimaLeitura.EnuOriginSet = true;
+            }
+
+            // 2) se já temos origem e um heading válido, aplica lever arm
+            if (UltimaLeitura.EnuOriginSet)
+            {
+                // a) antena -> ENU
+                var (xAnt, yAnt) = GeoLeverArm.GeodeticToENU(latitude, longitude, UltimaLeitura.Lat0, UltimaLeitura.Lon0);
+
+                // b) heading atual (rad). Substitua pelo seu campo real!
+                double theta = UltimaLeitura.OrientacaoReal; // 0=N, +horário→L
+
+                // c) offsets (m)
+                double lFwd = (VariaveisEquipamento.ComprimentoTras - VariaveisEquipamento.ComprimentoFrente) / 200.0;
+                double lLeft = (VariaveisEquipamento.LarguraDireita - VariaveisEquipamento.LarguraEsquerda) / 200.0;
+
+                // d) compensação: posição do CENTRO
+                var (xCtr, yCtr) = GeoLeverArm.ApplyLeverArmENU(xAnt, yAnt, theta, lFwd, lLeft);
+
+                // e) opcional: volta para lat/lon
+                var (latCtr, lonCtr) = GeoLeverArm.ENUToGeodetic(xCtr, yCtr, UltimaLeitura.Lat0, UltimaLeitura.Lon0);
+
+                // f) salve os valores corrigidos (sem perder os brutos, se quiser)
+                PenultimaLeitura.X = UltimaLeitura.X;
+                PenultimaLeitura.Y = UltimaLeitura.Y;
+
+                UltimaLeitura.Latitude = latCtr;
+                UltimaLeitura.Longitude = lonCtr;
+                UltimaLeitura.X = xCtr;
+                UltimaLeitura.Y = yCtr;
+
+                // Se o resto do sistema espera lat/lon já compensados, você pode
+                // sobrescrever os campos:
+                // UltimaLeitura.Latitude  = latCtr;
+                // UltimaLeitura.Longitude = lonCtr;
+            }
         }
 
         private static void ProcessarGNRMC(string sentenca)
@@ -1649,4 +1695,49 @@ namespace AgroBase.Services
 
     }
 
+    public class GeoLeverArm
+    {
+        const double R = 6378137.0; // WGS84
+
+        static double ToRad(double deg) => deg * Math.PI / 180.0;
+        static double ToDeg(double rad) => rad * 180.0 / Math.PI;
+
+        // lat/lon -> ENU (aproximação local, boa para ~km)
+        public static (double x, double y) GeodeticToENU(double lat, double lon, double lat0, double lon0)
+        {
+            double latR = ToRad(lat), lonR = ToRad(lon);
+            double lat0R = ToRad(lat0), lon0R = ToRad(lon0);
+            double dLat = latR - lat0R, dLon = lonR - lon0R;
+            double xEast = dLon * Math.Cos(lat0R) * R;
+            double yNorth = dLat * R;
+            return (xEast, yNorth);
+        }
+
+        public static (double lat, double lon) ENUToGeodetic(double x, double y, double lat0, double lon0)
+        {
+            double lat0R = ToRad(lat0);
+            double dLat = y / R;
+            double dLon = x / (R * Math.Cos(lat0R));
+            double lat = lat0 + ToDeg(dLat);
+            double lon = lon0 + ToDeg(dLon);
+            return (lat, lon);
+        }
+
+        // Aplica lever arm: retorna o CENTRO dado ANTENA + heading.
+        // Convenção: θ=0 norte; +horário→leste (igual ao seu modelo: x+=sinθ, y+=cosθ).
+        public static (double xCtr, double yCtr) ApplyLeverArmENU(
+            double xAnt, double yAnt, double thetaRad, double lFwd, double lLeft)
+        {
+            double s = Math.Sin(thetaRad), c = Math.Cos(thetaRad);
+            // vetores no mundo
+            double fx = s, fy = c;     // frente
+            double lx = -c, ly = s;     // esquerda
+                                        // vetor centro->antena no mundo
+            double rx = lFwd * fx + lLeft * lx;
+            double ry = lFwd * fy + lLeft * ly;
+            // centro = antena - (centro->antena)
+            return (xAnt - rx, yAnt - ry);
+        }
+    }
+
 }

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/BrowsingTopicsState

@@ -29,5 +29,5 @@
       "top_topics_and_observing_domains": [  ]
    } ],
    "hex_encoded_hmac_key": "40F346D3248C3AFDF2BEE1FE496DBD32F7CED6E5AE98B881ABC421AA7E7B5642",
-   "next_scheduled_calculation_time": "13403636820470311"
+   "next_scheduled_calculation_time": "13403636820470678"
 }

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Local Storage/leveldb/LOG

@@ -1,3 +1,3 @@
-2025/09/25-16:03:01.465 6fbc Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
+2025/09/26-14:06:14.063 3ac0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
-2025/09/25-16:03:01.473 6fbc Recovering log #3
+2025/09/26-14:06:14.074 3ac0 Recovering log #3
-2025/09/25-16:03:01.476 6fbc Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log 
+2025/09/26-14:06:14.077 3ac0 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Local Storage/leveldb/LOG.old

@@ -1,3 +1,3 @@
-2025/09/25-15:43:45.802 6e20 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
+2025/09/26-14:03:00.019 39e4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
-2025/09/25-15:43:45.810 6e20 Recovering log #3
+2025/09/26-14:03:00.029 39e4 Recovering log #3
-2025/09/25-15:43:45.813 6e20 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log 
+2025/09/26-14:03:00.033 39e4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Network/Network Persistent State

@@ -1 +1 @@
-{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13403386981816386","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":16615},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:67d:1400:b1d8:f114:3804:472c","used_quic":true},"version":5},"network_qualities":{"CAASABiAgICA+P////8B":"4G","CAESABiAgICA+P////8B":"4G","CAISABiAgICA+P////8B":"4G","CAYSABiAgICA+P////8B":"Offline"}}}
+{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13403466393568231","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":14687},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:67d:1400:2c66:7456:c2f:b6e6","used_quic":true},"version":5},"network_qualities":{"CAASABiAgICA+P////8B":"4G","CAESABiAgICA+P////8B":"4G","CAISABiAgICA+P////8B":"4G","CAYSABiAgICA+P////8B":"Offline"}}}

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Network/TransportSecurity

@@ -1 +1 @@
-{"sts":[{"expiry":1790355489.322402,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1758819489.322412}],"version":2}
+{"sts":[{"expiry":1790441947.398009,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1758905947.398017}],"version":2}

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Session Storage/LOG

@@ -1,3 +1,3 @@
-2025/09/25-16:03:05.668 6fbc Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
+2025/09/26-14:13:03.573 3ac0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
-2025/09/25-16:03:05.669 6fbc Recovering log #3
+2025/09/26-14:13:03.574 3ac0 Recovering log #3
-2025/09/25-16:03:05.677 6fbc Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log 
+2025/09/26-14:13:03.577 3ac0 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Session Storage/LOG.old

@@ -1,3 +1,3 @@
-2025/09/25-15:44:11.254 6e20 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
+2025/09/26-14:04:37.387 39e4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
-2025/09/25-15:44:11.256 6e20 Recovering log #3
+2025/09/26-14:04:37.389 39e4 Recovering log #3
-2025/09/25-15:44:11.259 6e20 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log 
+2025/09/26-14:04:37.393 39e4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Site Characteristics Database/LOG

@@ -1,3 +1,3 @@
-2025/09/25-16:03:01.394 423c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
+2025/09/26-14:06:13.990 27dc Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
-2025/09/25-16:03:01.396 423c Recovering log #7
+2025/09/26-14:06:13.993 27dc Recovering log #7
-2025/09/25-16:03:01.397 423c Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log 
+2025/09/26-14:06:13.993 27dc Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Default/Site Characteristics Database/LOG.old

@@ -1,3 +1,3 @@
-2025/09/25-15:43:45.734 4d14 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
+2025/09/26-14:02:59.941 4510 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
-2025/09/25-15:43:45.736 4d14 Recovering log #7
+2025/09/26-14:02:59.944 4510 Recovering log #7
-2025/09/25-15:43:45.737 4d14 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log 
+2025/09/26-14:02:59.944 4510 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log 

AgroBase/AgroBase/bin/x64/Debug/AgroBase.exe.WebView2/EBWebView/Last Version

@@ -1 +1 @@
-140.0.3485.81
+140.0.3485.94

AgroBase/AgroBase/bin/x64/Debug/Python/Output/GeoJson.json

@@ -1 +1 @@
-{"type":"FeatureCollection","features":[{"type":"Feature","properties":{"Id":"1","Name":"CidadeJardimCerto","Length":18.438211678884382,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":"0","type":"LineString","coordinates":[[-47.3952092235,-22.172566764333332],[-47.395214685166664,-22.172591648666668],[-47.395217691333336,-22.1726149115],[-47.39522120783333,-22.172638106166666],[-47.395224592333335,-22.172662264666666],[-47.395227536833332,-22.172683503833333],[-47.395231278833336,-22.172709659],[-47.3952341445,-22.1727307235]]}},{"type":"Feature","properties":{"Id":"2","Name":"CidadeJardimCerto","Length":18.304756653641313,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":"1","type":"LineString","coordinates":[[-47.395219567666665,-22.172731676],[-47.3952166705,-22.172709694166667],[-47.395212956833333,-22.1726860135],[-47.395209649833333,-22.172662382166667],[-47.3952061285,-22.172638539666668],[-47.395203071333334,-22.172615091],[-47.395199642,-22.172589946333332],[-47.39519637716667,-22.172568656333333]]}},{"type":"Feature","properties":{"Id":"3","Name":"CidadeJardimCerto","Length":18.204085527823089,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":"2","type":"LineString","coordinates":[[-47.395182856666665,-22.172571987333335],[-47.395186353666666,-22.172596904666666],[-47.3951894365,-22.1726195275],[-47.395192361833331,-22.172642871333334],[-47.3951957415,-22.1726662635],[-47.3951991415,-22.1726895815],[-47.395202583833331,-22.172713842],[-47.395205228833333,-22.172734196333334]]}},{"type":"Feature","properties":{"Id":"4","Name":"CidadeJardimCerto","Length":18.206275148810604,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":"3","type":"LineString","coordinates":[[-47.3951911205,-22.172735722833334],[-47.395187572,-22.172712241833334],[-47.3951841675,-22.1726893895],[-47.395179907166664,-22.1726651245],[-47.3951765075,-22.172642284833334],[-47.395173248666666,-22.1726180985],[-47.395169798666664,-22.172594096166666],[-47.395167092166666,-22.172573705666668]]}},{"type":"Feature","properties":{"Id":"5","Name":"CidadeJardimCerto","Length":18.067139082589541,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":"4","type":"LineString","coordinates":[[-47.395153470333334,-22.172576570833332],[-47.395156353166669,-22.172600550333332],[-47.395159992333333,-22.172624829333333],[-47.395162920833336,-22.172647923333333],[-47.39516644,-22.172671954333332],[-47.395169917833336,-22.1726952245],[-47.3951736845,-22.172721525666667],[-47.395175992333336,-22.172737516833333]]}}]}
+{"type":"FeatureCollection","features":[{"type":"Feature","properties":{"Id":"1","Name":"CidadeJardimCerto","Length":18.438211678884382,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[-47.3952092235,-22.172566764333332],[-47.395214685166664,-22.172591648666668],[-47.395217691333336,-22.1726149115],[-47.39522120783333,-22.172638106166666],[-47.395224592333335,-22.172662264666666],[-47.395227536833332,-22.172683503833333],[-47.395231278833336,-22.172709659],[-47.3952341445,-22.1727307235]]}},{"type":"Feature","properties":{"Id":"2","Name":"CidadeJardimCerto","Length":18.304756653641313,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[-47.395219567666665,-22.172731676],[-47.3952166705,-22.172709694166667],[-47.395212956833333,-22.1726860135],[-47.395209649833333,-22.172662382166667],[-47.3952061285,-22.172638539666668],[-47.395203071333334,-22.172615091],[-47.395199642,-22.172589946333332],[-47.39519637716667,-22.172568656333333]]}},{"type":"Feature","properties":{"Id":"3","Name":"CidadeJardimCerto","Length":18.204085527823089,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[-47.395182856666665,-22.172571987333335],[-47.395186353666666,-22.172596904666666],[-47.3951894365,-22.1726195275],[-47.395192361833331,-22.172642871333334],[-47.3951957415,-22.1726662635],[-47.3951991415,-22.1726895815],[-47.395202583833331,-22.172713842],[-47.395205228833333,-22.172734196333334]]}},{"type":"Feature","properties":{"Id":"4","Name":"CidadeJardimCerto","Length":18.206275148810604,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[-47.3951911205,-22.172735722833334],[-47.395187572,-22.172712241833334],[-47.3951841675,-22.1726893895],[-47.395179907166664,-22.1726651245],[-47.3951765075,-22.172642284833334],[-47.395173248666666,-22.1726180985],[-47.395169798666664,-22.172594096166666],[-47.395167092166666,-22.172573705666668]]}},{"type":"Feature","properties":{"Id":"5","Name":"CidadeJardimCerto","Length":18.067139082589541,"Dist1":18.438211678884382,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[-47.395153470333334,-22.172576570833332],[-47.395156353166669,-22.172600550333332],[-47.395159992333333,-22.172624829333333],[-47.395162920833336,-22.172647923333333],[-47.39516644,-22.172671954333332],[-47.395169917833336,-22.1726952245],[-47.3951736845,-22.172721525666667],[-47.395175992333336,-22.172737516833333]]}}]}

AgroBase/AgroBase/bin/x64/Debug/Python/Output/mapa_gps.html

@@ -17,7 +17,7 @@
             <meta name="viewport" content="width=device-width,
                 initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
             <style>
-                #map_f63c9a42b387b7620b9c3dbf34b83eee {
+                #map_094a1d2cc67a698de1583e83f72ba778 {
                     position: relative;
                     width: 100.0%;
                     height: 100.0%;
@@ -54,14 +54,14 @@
 <body>
     
     
-            <div class="folium-map" id="map_f63c9a42b387b7620b9c3dbf34b83eee" ></div>
+            <div class="folium-map" id="map_094a1d2cc67a698de1583e83f72ba778" ></div>
         
 </body>
 <script>
     
     
-            var map_f63c9a42b387b7620b9c3dbf34b83eee = L.map(
+            var map_094a1d2cc67a698de1583e83f72ba778 = L.map(
-                "map_f63c9a42b387b7620b9c3dbf34b83eee",
+                "map_094a1d2cc67a698de1583e83f72ba778",
                 {
                     center: [0.0, 0.0],
                     crs: L.CRS.EPSG3857,
@@ -77,6 +77,26 @@
             
 
         
+    
+            var tile_layer_897decfc0132c82ed0e60422f9aea75c = L.tileLayer(
+                "https://tile.openstreetmap.org/{z}/{x}/{y}.png",
+                {
+  "minZoom": 0,
+  "maxZoom": 19,
+  "maxNativeZoom": 19,
+  "noWrap": false,
+  "attribution": "\u0026copy; \u003ca href=\"https://www.openstreetmap.org/copyright\"\u003eOpenStreetMap\u003c/a\u003e contributors",
+  "subdomains": "abc",
+  "detectRetina": false,
+  "tms": false,
+  "opacity": 1,
+}
+
+            );
+        
+    
+            tile_layer_897decfc0132c82ed0e60422f9aea75c.addTo(map_094a1d2cc67a698de1583e83f72ba778);
+        
 </script>
 
     <script>
@@ -96,7 +116,7 @@
         }
         trajeto_json_add({"features": []});
 
-        trajeto_json.addTo(map_f63c9a42b387b7620b9c3dbf34b83eee);
+        trajeto_json.addTo(map_094a1d2cc67a698de1583e83f72ba778);
 
         function adicionarGeometria(novaGeometria) {
             trajeto_json.addData(novaGeometria);
@@ -159,9 +179,9 @@
         
         var marcadorEquipamento = L.marker([0, 0], {
             icon: customIcon
-        }).addTo(map_f63c9a42b387b7620b9c3dbf34b83eee);
+        }).addTo(map_094a1d2cc67a698de1583e83f72ba778);
         
-        var marcadorBase = L.marker([0, 0], {}).addTo(map_f63c9a42b387b7620b9c3dbf34b83eee);
+        var marcadorBase = L.marker([0, 0], {}).addTo(map_094a1d2cc67a698de1583e83f72ba778);
         var icon = L.AwesomeMarkers.icon(
             {"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
         );
@@ -226,7 +246,7 @@
                 }
                 
                 if (foco) {
-                    map_f63c9a42b387b7620b9c3dbf34b83eee.setView(novaPosicao, map_f63c9a42b387b7620b9c3dbf34b83eee.getZoom());
+                    map_094a1d2cc67a698de1583e83f72ba778.setView(novaPosicao, map_094a1d2cc67a698de1583e83f72ba778.getZoom());
                 }
                 
             }
@@ -248,7 +268,7 @@
             marcadorDinamico.setRotationAngle(angulo);
             
             adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
-            map_f63c9a42b387b7620b9c3dbf34b83eee.setView(novaPosicao, map_f63c9a42b387b7620b9c3dbf34b83eee.getZoom());*/
+            map_094a1d2cc67a698de1583e83f72ba778.setView(novaPosicao, map_094a1d2cc67a698de1583e83f72ba778.getZoom());*/
         });
         
         function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {

AgroBase/AgroBase/bin/x64/Debug/Python/Output/mapa_interativo.html

@@ -17,7 +17,7 @@
             <meta name="viewport" content="width=device-width,
                 initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
             <style>
-                #map_c4ba79c1d98f5ccc1d268b643fbb7abb {
+                #map_de13db2dddfa8d44afb0c6dc4a7b2662 {
                     position: relative;
                     width: 100.0%;
                     height: 100.0%;
@@ -54,14 +54,14 @@
 <body>
     
     
-            <div class="folium-map" id="map_c4ba79c1d98f5ccc1d268b643fbb7abb" ></div>
+            <div class="folium-map" id="map_de13db2dddfa8d44afb0c6dc4a7b2662" ></div>
         
 </body>
 <script>
     
     
-            var map_c4ba79c1d98f5ccc1d268b643fbb7abb = L.map(
+            var map_de13db2dddfa8d44afb0c6dc4a7b2662 = L.map(
-                "map_c4ba79c1d98f5ccc1d268b643fbb7abb",
+                "map_de13db2dddfa8d44afb0c6dc4a7b2662",
                 {
                     center: [-22.17265214058333, -47.39519380741667],
                     crs: L.CRS.EPSG3857,
@@ -78,7 +78,7 @@
 
         
     
-            var tile_layer_4debaf03661fab35c7f05f9165d16413 = L.tileLayer(
+            var tile_layer_0d4b83a132c0a3275ab44552f768a22b = L.tileLayer(
                 "https://tile.openstreetmap.org/{z}/{x}/{y}.png",
                 {
   "minZoom": 0,
@@ -95,7 +95,7 @@
             );
         
     
-            tile_layer_4debaf03661fab35c7f05f9165d16413.addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+            tile_layer_0d4b83a132c0a3275ab44552f768a22b.addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
         
     
 
@@ -111,7 +111,7 @@
                 }*/
             });
     }
-    function geo_json_9f5502753d57522086d0216c6c3a7f99_onEachFeature(feature, layer) {
+    function geo_json_77371c1eaf1e6144b4deabe3ab1e0927_onEachFeature(feature, layer) {
 
             layer.on({
             
@@ -148,23 +148,23 @@
         }*/
     });
         };
-        var geo_json_9f5502753d57522086d0216c6c3a7f99 = L.geoJson(null, {
+        var geo_json_77371c1eaf1e6144b4deabe3ab1e0927 = L.geoJson(null, {
-                onEachFeature: geo_json_9f5502753d57522086d0216c6c3a7f99_onEachFeature,
+                onEachFeature: geo_json_77371c1eaf1e6144b4deabe3ab1e0927_onEachFeature,
             
             ...{
 }
         });
 
-        function geo_json_9f5502753d57522086d0216c6c3a7f99_add (data) {
+        function geo_json_77371c1eaf1e6144b4deabe3ab1e0927_add (data) {
-            geo_json_9f5502753d57522086d0216c6c3a7f99
+            geo_json_77371c1eaf1e6144b4deabe3ab1e0927
                 .addData(data);
         }
-            geo_json_9f5502753d57522086d0216c6c3a7f99_add({"features": [{"geometry": {"coordinates": [[-47.3952092235, -22.172566764333332], [-47.395214685166664, -22.172591648666668], [-47.395217691333336, -22.1726149115], [-47.39522120783333, -22.172638106166666], [-47.395224592333335, -22.172662264666666], [-47.39522753683333, -22.172683503833333], [-47.395231278833336, -22.172709659], [-47.3952341445, -22.1727307235]], "id": "0", "type": "LineString"}, "id": 0, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "1", "Length": 18.438211678884382, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395219567666665, -22.172731676], [-47.3952166705, -22.172709694166667], [-47.39521295683333, -22.1726860135], [-47.39520964983333, -22.172662382166667], [-47.3952061285, -22.172638539666668], [-47.395203071333334, -22.172615091], [-47.395199642, -22.172589946333332], [-47.39519637716667, -22.172568656333333]], "id": "1", "type": "LineString"}, "id": 1, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "2", "Length": 18.304756653641313, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395182856666665, -22.172571987333335], [-47.395186353666666, -22.172596904666666], [-47.3951894365, -22.1726195275], [-47.39519236183333, -22.172642871333334], [-47.3951957415, -22.1726662635], [-47.3951991415, -22.1726895815], [-47.39520258383333, -22.172713842], [-47.39520522883333, -22.172734196333334]], "id": "2", "type": "LineString"}, "id": 2, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "3", "Length": 18.20408552782309, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951911205, -22.172735722833334], [-47.395187572, -22.172712241833334], [-47.3951841675, -22.1726893895], [-47.395179907166664, -22.1726651245], [-47.3951765075, -22.172642284833334], [-47.395173248666666, -22.1726180985], [-47.395169798666664, -22.172594096166666], [-47.395167092166666, -22.172573705666668]], "id": "3", "type": "LineString"}, "id": 3, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "4", "Length": 18.206275148810604, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395153470333334, -22.172576570833332], [-47.39515635316667, -22.172600550333332], [-47.39515999233333, -22.172624829333333], [-47.395162920833336, -22.172647923333333], [-47.39516644, -22.17267195433333], [-47.395169917833336, -22.1726952245], [-47.3951736845, -22.172721525666667], [-47.395175992333336, -22.172737516833333]], "id": "4", "type": "LineString"}, "id": 4, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "5", "Length": 18.06713908258954, "Name": "CidadeJardimCerto"}, "type": "Feature"}], "type": "FeatureCollection"});
+            geo_json_77371c1eaf1e6144b4deabe3ab1e0927_add({"features": [{"geometry": {"coordinates": [[-47.3952092235, -22.172566764333332], [-47.395214685166664, -22.172591648666668], [-47.395217691333336, -22.1726149115], [-47.39522120783333, -22.172638106166666], [-47.395224592333335, -22.172662264666666], [-47.39522753683333, -22.172683503833333], [-47.395231278833336, -22.172709659], [-47.3952341445, -22.1727307235]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "1", "Length": 18.438211678884382, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395219567666665, -22.172731676], [-47.3952166705, -22.172709694166667], [-47.39521295683333, -22.1726860135], [-47.39520964983333, -22.172662382166667], [-47.3952061285, -22.172638539666668], [-47.395203071333334, -22.172615091], [-47.395199642, -22.172589946333332], [-47.39519637716667, -22.172568656333333]], "id": null, "type": "LineString"}, "id": 1, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "2", "Length": 18.304756653641313, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395182856666665, -22.172571987333335], [-47.395186353666666, -22.172596904666666], [-47.3951894365, -22.1726195275], [-47.39519236183333, -22.172642871333334], [-47.3951957415, -22.1726662635], [-47.3951991415, -22.1726895815], [-47.39520258383333, -22.172713842], [-47.39520522883333, -22.172734196333334]], "id": null, "type": "LineString"}, "id": 2, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "3", "Length": 18.20408552782309, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951911205, -22.172735722833334], [-47.395187572, -22.172712241833334], [-47.3951841675, -22.1726893895], [-47.395179907166664, -22.1726651245], [-47.3951765075, -22.172642284833334], [-47.395173248666666, -22.1726180985], [-47.395169798666664, -22.172594096166666], [-47.395167092166666, -22.172573705666668]], "id": null, "type": "LineString"}, "id": 3, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "4", "Length": 18.206275148810604, "Name": "CidadeJardimCerto"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395153470333334, -22.172576570833332], [-47.39515635316667, -22.172600550333332], [-47.39515999233333, -22.172624829333333], [-47.395162920833336, -22.172647923333333], [-47.39516644, -22.17267195433333], [-47.395169917833336, -22.1726952245], [-47.3951736845, -22.172721525666667], [-47.395175992333336, -22.172737516833333]], "id": null, "type": "LineString"}, "id": 4, "properties": {"Dist1": 18.438211678884382, "Dist2": 0.0, "Id": "5", "Length": 18.06713908258954, "Name": "CidadeJardimCerto"}, "type": "Feature"}], "type": "FeatureCollection"});
-        geo_json_9f5502753d57522086d0216c6c3a7f99.setStyle(function(feature) {return feature.properties.style;});
+        geo_json_77371c1eaf1e6144b4deabe3ab1e0927.setStyle(function(feature) {return feature.properties.style;});
 
         
     
-            geo_json_9f5502753d57522086d0216c6c3a7f99.addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+            geo_json_77371c1eaf1e6144b4deabe3ab1e0927.addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
         
 </script>
 
@@ -185,7 +185,7 @@
         }
         trajeto_json_add({"features": []});
 
-        trajeto_json.addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+        trajeto_json.addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
 
         function adicionarGeometria(novaGeometria) {
             trajeto_json.addData(novaGeometria);
@@ -243,7 +243,7 @@
         }
         trajeto_dinamico_json_add({"features": []});
 
-        trajeto_dinamico_json.addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+        trajeto_dinamico_json.addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
 
         function adicionarGeometriaDinamica(novaGeometria) {
             trajeto_dinamico_json.addData(novaGeometria);
@@ -296,9 +296,9 @@
         
         var marcadorEquipamento = L.marker([0, 0], {
             icon: customIcon
-        }).addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+        }).addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
         
-        var marcadorBase = L.marker([0, 0], {}).addTo(map_c4ba79c1d98f5ccc1d268b643fbb7abb);
+        var marcadorBase = L.marker([0, 0], {}).addTo(map_de13db2dddfa8d44afb0c6dc4a7b2662);
         var icon = L.AwesomeMarkers.icon(
             {"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
         );
@@ -380,7 +380,7 @@
                 }
                 
                 if (foco) {
-                    map_c4ba79c1d98f5ccc1d268b643fbb7abb.setView(novaPosicao, map_c4ba79c1d98f5ccc1d268b643fbb7abb.getZoom());
+                    map_de13db2dddfa8d44afb0c6dc4a7b2662.setView(novaPosicao, map_de13db2dddfa8d44afb0c6dc4a7b2662.getZoom());
                 }
                 
             }
@@ -397,7 +397,7 @@
         function atualizarSelecaoRuas(selecionadas) {
             selecionadas = JSON.parse(selecionadas);
             RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
-            geo_json_9f5502753d57522086d0216c6c3a7f99.eachLayer(function (layer) {
+            geo_json_77371c1eaf1e6144b4deabe3ab1e0927.eachLayer(function (layer) {
                 if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
                     layer.setStyle({ color: 'blue' });
                 } else {

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

@@ -81,7 +81,7 @@ class ControladorMPC:
             self._cl_xy = cl
             self._seg_p0 = self._seg_v = self._seg_L = self._seg_t_hat = self._seg_n_hat = None
             self._s_nodes = np.zeros((cl.shape[0],), float)
-            self._mg_nodes = np.array([p.get("distanciaMargem", 0.7) for p in self.pontos_info], float)
+            self._mg_nodes = np.array([(p.get("distanciaMargem", 0.7) / 0.8) for p in self.pontos_info], float)
             return
 
         p0 = cl[:-1]
@@ -95,7 +95,7 @@ class ControladorMPC:
         s_nodes = np.zeros((cl.shape[0],), float)
         s_nodes[1:] = np.cumsum(L)
 
-        mg_nodes = np.array([p.get("distanciaMargem", 0.7) for p in self.pontos_info], float)
+        mg_nodes = np.array([(p.get("distanciaMargem", 0.7) / 0.8) for p in self.pontos_info], float)
 
         self._cl_xy = cl
         self._seg_p0 = p0
@@ -1086,7 +1086,7 @@ class ControladorMPC:
                 #mostrar_log(f"idx_proximo_ponto_C#: {idx_proximo_ponto_real}, idx_ponto_alvo_C#: {idx_ponto_alvo}, idx_proximo_ponto_Python: {idx_alvo_correcao}")
 
                 # Loop de K comandos (receding); cada comando simula dt_pred com v_sim
-                #print("COMENCANDO")
+                print("COMECANDO...")
                 for passo in range(self.qtd_comandos_sucessivos):
                     if ms_left() <= 0:
                         break
@@ -1227,6 +1227,7 @@ class ControladorMPC:
                     else:
                         idx_cmd = 0 if len(_comandos) == 1 else 1
                         tipo_final, angulo_final = _comandos[idx_cmd]
+                        debug_custo[f"{tipo_final.value}_{np.degrees(angulo_final):.2f}"]["melhor"] = True
                     simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(melhor["trajetoria"]) or [])
                     simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta))
                 else:
@@ -2229,17 +2230,13 @@ class ControladorMPC:
                 # 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)
 
-                # erro de orientação: referência = theta_path (não o bearing ao alvo)
+                # erro de orientação
-                # (manter 'peso_erro_orientacao_caminho' se você usa em pesos dinâmicos)
-                peso_erro_orientacao_pp = 1.0 # 0.6 if (status_carro == StatusCarroMapa.CaminhandoRua and dentro_corredor) else 0.4
                 orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim)
                 orient_sim = (orient_sim + np.pi) % (2 * np.pi)
-
+                erro_ori_sim = self.gps_handler.erro_angular(orient_sim, theta_sim, theta_path, 0.5)
-                # erros
+                #erro_ori_sim = abs(self._wrap_pi(orient_sim - theta_sim))
-                #erro_ori_sim = self.gps_handler.erro_angular(orient_sim, theta_sim, theta_path, peso_erro_orientacao_pp)
+                #erro_ori_sim = abs(self._wrap_pi(theta_path - theta_sim))
-                erro_ori_sim = abs(self._wrap_pi(orient_sim - theta_sim))
+                o_norm = np.clip(erro_ori_sim / np.pi, 0.0, 1.0)
-                o_norm = np.clip(abs(erro_ori_sim) / np.pi, 0.0, 1.0)
-                #erro_ori_sim = self._wrap_pi(theta_path - theta_sim)
 
                 # pesos dinâmicos (mantido)
                 pesos = self._calcular_pesos_movimento(contexto, erro_ori_sim)
@@ -2277,10 +2274,15 @@ class ControladorMPC:
 
                 # --- PATCH D: custo lateral com deadband + quadrático + normalização
                 dead = 0.0                            # 5–12 cm
-                half = max(float(margem), 0.20)       # meia-largura mínima
+                half = max(float(margem) / 2.0, 0.5)       # meia-largura mínima
                 e_eff = max(0.0, abs(e_lat) - dead)
-                l_norm = np.clip(e_eff / half, 0.0, 1.0) ** 2
+                r = e_eff/half
-                #custo_lateral = (((e_eff / half) ** 2) * erro_pos) * peso_lateral
+                if r <= 1.0:
+                    alpha = 0.25          # 0 = só quadrático; 1 = só linear
+                    l_norm = alpha*r + (1-alpha)*r**2
+                else:
+                    gamma = 1.0
+                    l_norm = 1.0 + gamma*(r - 1.0)    # ex.: gamma = 1.0 (ajuste)
                 custo_lateral = l_norm * peso_lateral
 
                 # --- PATCH E: amortecimento por derivada do erro lateral (m/s)
@@ -2288,8 +2290,13 @@ class ControladorMPC:
                     de = (e_lat - e_lat_prev_local) / max(sub_dt, 1e-6)  # m/s
                     peso_lat_der = 0.1 * float(peso_lateral)            # ganho pequeno
                     custo_lat_der = (de ** 2) * peso_lat_der
+
+                    dlat = (abs(e_lat) - abs(e_lat_prev_local)) / max(sub_dt,1e-6)
+                    pen_away = max(0.0, dlat/half) * (0.2 * peso_lateral)
                 else:
                     custo_lat_der = 0.0
+                    pen_away = 0.0
+                custo_lateral += pen_away
                 e_lat_prev_local = e_lat
 
                 # --- PATCH F: anti-chatter (histerese) perto do centro
@@ -2371,7 +2378,8 @@ class ControladorMPC:
                     "custo_flip": somas[20],
                     "custo_visual_worker": somas[21],
                     "custo_map": somas[22],
-                    "custo_total": finais[23]
+                    "custo_total": finais[23],
+                    "melhor": False
                 }
                 #print(custo_visual_worker)