aplicacao do lever arm
This commit is contained in:
Diego Freitas
parent
914f838482
commit
9b6dbf7890
Binary file not shown.
Binary file not shown.
|
|
@ -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(
|
||||
$"ang {angleTxt}° #{r.rank} Δ={r.margin:0.000} {yLabel}={yVal:0.0000}");
|
||||
var angNode = tipoNode.Nodes.Add($"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($"alvo: x={m.x_alvo:0.3} y={m.y_alvo:0.3}");
|
||||
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.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($"eori={m.erro_orientacao:0.3} eorin={m.erro_orientacao_n:0.4}");
|
||||
errs.Nodes.Add($"elat={m.erro_lateral:0.3} elatn={m.erro_lateral_n:0.4}");
|
||||
errs.Nodes.Add($"ere={m.erro_re:0.3}");
|
||||
errs.Nodes.Add($"Posição={m.erro_posicao:0.00000} Normalizado={m.erro_posicao_n:0.00000}");
|
||||
errs.Nodes.Add($"Orientação={m.erro_orientacao:0.00000} Normalizado={m.erro_orientacao_n:0.00000}");
|
||||
errs.Nodes.Add($"Suavização={m.erro_orientacao:0.00000}");
|
||||
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($"csua={m.custo_suavizacao:0.4} cre={m.custo_re:0.4}");
|
||||
costs.Nodes.Add($"clat={m.custo_lateral:0.4} clatd={m.custo_lateral_d:0.4}");
|
||||
costs.Nodes.Add($"cvw={m.custo_visual_worker:0.4}");
|
||||
costs.Nodes.Add($"cmap={m.custo_map:0.4} ctot={m.custo_total:0.4}");
|
||||
costs.Nodes.Add($"Posição={m.custo_posicao:0.00000}");
|
||||
costs.Nodes.Add($"Orientação={m.custo_orientacao:0.00000}");
|
||||
costs.Nodes.Add($"Suavização={m.custo_suavizacao:0.00000}");
|
||||
costs.Nodes.Add($"Ré={m.custo_re:0.00000}");
|
||||
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($"suav={m.peso_suavizacao:0.2} re={m.peso_re:0.2} ideal={m.peso_ideal:0.2}");
|
||||
pesos.Nodes.Add($"lat={m.peso_lateral:0.2}");
|
||||
pesos.Nodes.Add($"Posição={m.peso_posicao:0.00}");
|
||||
pesos.Nodes.Add($"Orientação={m.peso_orientacao:0.00}");
|
||||
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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -29,5 +29,5 @@
|
|||
"top_topics_and_observing_domains": [ ]
|
||||
} ],
|
||||
"hex_encoded_hmac_key": "40F346D3248C3AFDF2BEE1FE496DBD32F7CED6E5AE98B881ABC421AA7E7B5642",
|
||||
"next_scheduled_calculation_time": "13403636820470311"
|
||||
"next_scheduled_calculation_time": "13403636820470678"
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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/25-16:03:01.473 6fbc 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.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/26-14:06:14.074 3ac0 Recovering log #3
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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/25-15:43:45.810 6e20 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.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/26-14:03:00.029 39e4 Recovering log #3
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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"}}}
|
||||
|
|
@ -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}
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -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/25-16:03:05.669 6fbc 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.573 3ac0 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.574 3ac0 Recovering log #3
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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/25-15:44:11.256 6e20 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.387 39e4 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.389 39e4 Recovering log #3
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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/25-16:03:01.396 423c 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.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/26-14:06:13.993 27dc Recovering log #7
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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/25-15:43:45.736 4d14 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.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/26-14:02:59.944 4510 Recovering log #7
|
||||
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
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1 +1 @@
|
|||
140.0.3485.81
|
||||
140.0.3485.94
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -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]]}}]}
|
||||
|
|
@ -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(
|
||||
"map_f63c9a42b387b7620b9c3dbf34b83eee",
|
||||
var map_094a1d2cc67a698de1583e83f72ba778 = L.map(
|
||||
"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) {
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
"map_c4ba79c1d98f5ccc1d268b643fbb7abb",
|
||||
var map_de13db2dddfa8d44afb0c6dc4a7b2662 = L.map(
|
||||
"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, {
|
||||
onEachFeature: geo_json_9f5502753d57522086d0216c6c3a7f99_onEachFeature,
|
||||
var geo_json_77371c1eaf1e6144b4deabe3ab1e0927 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_77371c1eaf1e6144b4deabe3ab1e0927_onEachFeature,
|
||||
|
||||
...{
|
||||
}
|
||||
});
|
||||
|
||||
function geo_json_9f5502753d57522086d0216c6c3a7f99_add (data) {
|
||||
geo_json_9f5502753d57522086d0216c6c3a7f99
|
||||
function geo_json_77371c1eaf1e6144b4deabe3ab1e0927_add (data) {
|
||||
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_9f5502753d57522086d0216c6c3a7f99.setStyle(function(feature) {return feature.properties.style;});
|
||||
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_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 {
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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)
|
||||
# (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
|
||||
# erro de orientação
|
||||
orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim)
|
||||
orient_sim = (orient_sim + np.pi) % (2 * np.pi)
|
||||
|
||||
# erros
|
||||
#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(orient_sim - theta_sim))
|
||||
o_norm = np.clip(abs(erro_ori_sim) / np.pi, 0.0, 1.0)
|
||||
#erro_ori_sim = self._wrap_pi(theta_path - theta_sim)
|
||||
erro_ori_sim = self.gps_handler.erro_angular(orient_sim, theta_sim, theta_path, 0.5)
|
||||
#erro_ori_sim = abs(self._wrap_pi(orient_sim - theta_sim))
|
||||
#erro_ori_sim = abs(self._wrap_pi(theta_path - theta_sim))
|
||||
o_norm = np.clip(erro_ori_sim / np.pi, 0.0, 1.0)
|
||||
|
||||
# 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
|
||||
#custo_lateral = (((e_eff / half) ** 2) * erro_pos) * peso_lateral
|
||||
r = e_eff/half
|
||||
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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue