unificando angulo antena e movimento
This commit is contained in:
Diego Freitas
parent
a2bd8cc83f
commit
8993003b31
Binary file not shown.
Binary file not shown.
|
|
@ -1,6 +1,8 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using AgroBase.Services.Operadores;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
|
|
@ -498,8 +500,21 @@ namespace AgroBase.Forms
|
|||
|
||||
private void cmbStatusCorredor_SelectedIndexChanged(object sender, EventArgs e)
|
||||
{
|
||||
var json = RedisService.Get(CtxKey.DadosVisualWorker);
|
||||
if (string.IsNullOrWhiteSpace(json)) return;
|
||||
JObject root;
|
||||
try
|
||||
{
|
||||
root = JObject.Parse(json);
|
||||
}
|
||||
catch
|
||||
{
|
||||
return;
|
||||
}
|
||||
long tsUnix = root["ts_analise"]?.Value<long>() ?? 0;
|
||||
RedisService.AtualizarCampos(
|
||||
CtxKey.DadosVisualWorker,
|
||||
("ts_analise", tsUnix + 1),
|
||||
("segmentacao.status_corredor", cmbStatusCorredor.SelectedIndex)
|
||||
);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -451,7 +451,7 @@ namespace AgroBase.Models
|
|||
|
||||
double resultado = Math.Atan2(y, x) * 180.0 / Math.PI;
|
||||
|
||||
return GPSUtils.NormalizarAngulo(resultado);
|
||||
return NormalizarAngulo(resultado);
|
||||
}
|
||||
|
||||
public static (GPSModel ponto, int index) PontoMaisProximoTrechoComIndice(GPSModel pontoRef, List<GPSModel> trecho)
|
||||
|
|
|
|||
|
|
@ -546,6 +546,7 @@ namespace AgroBase.Models.Operadores
|
|||
public double erro_angular { get; set; }
|
||||
public double erro_lateral_pct { get; set; }
|
||||
public StatusCarroMapa status_corredor { get; set; }
|
||||
public StatusCarroMapa status_corredor_anterior { get; set; }
|
||||
public List<double[]> centros_corredor { get; set; }
|
||||
|
||||
public VisualWorkerMessageSegmentacaoSemanticaModel Clone()
|
||||
|
|
@ -557,6 +558,7 @@ namespace AgroBase.Models.Operadores
|
|||
erro_angular = erro_angular,
|
||||
erro_lateral_pct = erro_lateral_pct,
|
||||
status_corredor = status_corredor,
|
||||
status_corredor_anterior = status_corredor_anterior,
|
||||
centros_corredor = new List<double[]>(centros_corredor ?? new List<double[]>())
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
using AgroBase.Models.Operadores;
|
||||
using AgroBase.Services;
|
||||
using AgroBase.Services.Operadores;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using static AgroBase.Models.Enums;
|
||||
using static OpenTK.Graphics.OpenGL.GL;
|
||||
|
||||
namespace AgroBase.Models
|
||||
{
|
||||
|
|
@ -628,6 +630,7 @@ namespace AgroBase.Models
|
|||
{
|
||||
if (CorredorAtual == null) return;
|
||||
|
||||
bool abrirCurva = false;
|
||||
double limiarDistanciaMargem = 20.0;
|
||||
double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante;
|
||||
double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, CorredorAtual.Pontos.LastOrDefault().Posicao);
|
||||
|
|
@ -635,10 +638,12 @@ namespace AgroBase.Models
|
|||
StatusModulo OpVisualStatus = Variaveis.OperacaoEmAndamento.Sensoriamento.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
|
||||
var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
|
||||
|
||||
if (OpVisualStatus == StatusModulo.Operante) //OpVisual.Iniciado &&
|
||||
if (OpVisualStatus == StatusModulo.Operante || Variaveis.OperacaoEmAndamento.Simulando) //OpVisual.Iniciado &&
|
||||
{
|
||||
// Contagem de tempo por status em segundos
|
||||
if (CorredorAtual.TempoPorStatus == null) CorredorAtual.TempoPorStatus = new Dictionary<StatusCarroMapa, double>();
|
||||
StatusCarroMapa statusCarro = DadosSegmentacao.status_corredor;
|
||||
StatusCarroMapa statusCarroAnt = DadosSegmentacao.status_corredor_anterior;
|
||||
if (!CorredorAtual.TempoPorStatus.TryGetValue(statusCarro, out var status))
|
||||
{
|
||||
CorredorAtual.TempoPorStatus.Add(statusCarro, 0);
|
||||
|
|
@ -650,18 +655,43 @@ namespace AgroBase.Models
|
|||
CorredorAtual.UltimoTempoStatus = DateTime.Now;
|
||||
double TempoTotalContagem = CorredorAtual.TempoPorStatus.Sum(x => x.Value);
|
||||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.CaminhandoRua, out double TempoCaminhandoRua);
|
||||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.CaminhandoRua, out double TempoDirecionando);
|
||||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.Direcionando, out double TempoDirecionando);
|
||||
double PercentualTempoCaminhandoRua = TempoCaminhandoRua / TempoTotalContagem;
|
||||
double PercentualTempoDirecionando = TempoDirecionando / TempoTotalContagem;
|
||||
|
||||
if (
|
||||
PercentualTempoCaminhandoRua > 0.4 && // Esteve a pelo menos 40% do tempo do corredor com cana dos dois lados
|
||||
PercentualTempoDirecionando < 0.5 && // Esteve ate no maximo 50% do tempo fora do corredor
|
||||
distanciaRestanteCorredor < limiarDistanciaMargem && // Esta a pelo menos x metros do final do corredor
|
||||
distanciaUltimoPontoCorredor < limiarDistanciaMargem && // Esta a pelo menos x metros do ultimo ponto do corredor
|
||||
PercentualTempoCaminhandoRua > 0.4 && // Esteve a pelo menos 60% do tempo do corredor com cana dos dois lados
|
||||
PercentualTempoDirecionando < 0.5 && // Esteve ate no maximo 50% do tempo fora do corredor
|
||||
statusCarro == StatusCarroMapa.Direcionando // Carro esta fora do corredor
|
||||
statusCarro == StatusCarroMapa.Direcionando && // Carro esta fora do corredor
|
||||
statusCarroAnt != statusCarro
|
||||
|
||||
|| (Variaveis.OperacaoEmAndamento.Simulando && distanciaRestanteCorredor < limiarDistanciaMargem && distanciaUltimoPontoCorredor < limiarDistanciaMargem && statusCarro == StatusCarroMapa.Direcionando && statusCarroAnt != statusCarro)
|
||||
)
|
||||
{
|
||||
PontoTrajetoriaModel pontoEntrada = null;
|
||||
if (abrirCurva && !CorredorAtual.Ultimo)
|
||||
{
|
||||
double anguloProjetar = GPSUtils.CalcularOrientacao(PontoAtual.Posicao, ProximoPonto.Posicao);
|
||||
double anguloAcrescentar = CorredorAtual.idxRuaDireita % 2 == 0 && CorredorAtual.Pontos.First().Direcao == DirecaoCarroRua.Ida ? 45 : -45;
|
||||
anguloProjetar += anguloAcrescentar;
|
||||
|
||||
double percentualAcrescimoPontoAberturaCurva = 1.1;
|
||||
GPSModel posicao = GPSUtils.ProjetarPontoDeslocado(PontoAtual.Posicao, DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva, anguloProjetar);
|
||||
pontoEntrada = new PontoTrajetoriaModel(TipoPontoRua.LigacaoEntrada)
|
||||
{
|
||||
Visitado = false,
|
||||
idxCorredor = CorredorAtual.Idx + 1,
|
||||
idxPonto = 0,
|
||||
idxPontoCorredor = 0,
|
||||
Direcao = PontoAtual.Direcao,
|
||||
LarguraCorredor = PontoAtual.LarguraCorredor,
|
||||
Orientacao = PontoAtual.Orientacao,
|
||||
Posicao = posicao
|
||||
};
|
||||
}
|
||||
|
||||
int pontosMarcar = 0;
|
||||
foreach (var Ponto in CorredorAtual.Pontos.Where(x => !x.Visitado))
|
||||
{
|
||||
|
|
@ -681,8 +711,25 @@ namespace AgroBase.Models
|
|||
}
|
||||
AtualizarIndicesJanela();
|
||||
AtualizarTrajetoriaJanela();
|
||||
|
||||
DefinirPontoAtual();
|
||||
if (abrirCurva && pontoEntrada != null)
|
||||
{
|
||||
pontoEntrada.idxPonto = PontoAtual.idxPonto + 1;
|
||||
CorredorAtual.Pontos.Insert(pontosMarcar, pontoEntrada);
|
||||
_TrajetoriaFixa.Insert(pontoEntrada.idxPonto, pontoEntrada);
|
||||
DefinirPontoAtual();
|
||||
DefinirProximoPonto();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RedisService.AtualizarCampos(
|
||||
CtxKey.DadosVisualWorker,
|
||||
("segmentacao.status_corredor_anterior", (int)statusCarro)
|
||||
);
|
||||
if (VisualWorkerService.DadosLeitura.Analises.segmentacao != null)
|
||||
VisualWorkerService.DadosLeitura.Analises.segmentacao.status_corredor_anterior = statusCarro;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1470,7 +1517,7 @@ namespace AgroBase.Models
|
|||
Ultimo = Ultimo,
|
||||
FatorLarguraCorredor = FatorLarguraCorredor,
|
||||
UltimoTempoStatus = UltimoTempoStatus,
|
||||
TempoPorStatus = new Dictionary<StatusCarroMapa, double>(TempoPorStatus)
|
||||
TempoPorStatus = new Dictionary<StatusCarroMapa, double>(TempoPorStatus ?? new Dictionary<StatusCarroMapa, double>())
|
||||
};
|
||||
if (clonarPontos)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ namespace AgroBase.Services
|
|||
|
||||
public static GPSModel UltimaLeitura = new GPSModel();
|
||||
public static GPSModel PenultimaLeitura = new GPSModel();
|
||||
public static List<GPSModel> UltimasLeituras = new List<GPSModel>();
|
||||
public static List<string> Logs = new List<string>();
|
||||
|
||||
public static int TaxaAmostragemHz { get; set; } = 5;
|
||||
|
|
@ -949,7 +950,10 @@ namespace AgroBase.Services
|
|||
return;
|
||||
}
|
||||
|
||||
DefinirAnguloCarroGPS();
|
||||
UltimasLeituras.Add(UltimaLeitura);
|
||||
if (UltimasLeituras.Count > TaxaAmostragemHz) UltimasLeituras.Remove(UltimasLeituras.First());
|
||||
|
||||
DefinirOrientacaoMovimento();
|
||||
|
||||
AtualizaDadosRedis();
|
||||
|
||||
|
|
@ -1069,64 +1073,64 @@ namespace AgroBase.Services
|
|||
});
|
||||
}
|
||||
|
||||
private static void DefinirAnguloCarroGPS()
|
||||
private static void DefinirOrientacaoMovimento()
|
||||
{
|
||||
int PontosConsiderarAngulo = 2;
|
||||
double Angulo = 0.0;
|
||||
double DistAngulo = 0.0;
|
||||
|
||||
try
|
||||
if (UltimasLeituras.Count < 2)
|
||||
{
|
||||
var GPSTrajetoria = Variaveis.OperacaoEmAndamento.GPSTrajetoria;
|
||||
double ang = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
|
||||
double d = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
|
||||
|
||||
// ✅ Se a trajetória estiver vazia, não há o que calcular
|
||||
if (GPSTrajetoria == null || GPSTrajetoria.Count == 0)
|
||||
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
||||
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
||||
|
||||
UltimaLeitura.OrientacaoMovimento = ang;
|
||||
UltimaLeitura.Distancia = d; // aqui fica sendo o deslocamento dessa “janela” mínima
|
||||
}
|
||||
else
|
||||
{
|
||||
// Média circular ponderada pela distância de cada segmento da janela
|
||||
double sumX = 0.0, sumY = 0.0;
|
||||
double distAcum = 0.0;
|
||||
|
||||
for (int i = 0; i < UltimasLeituras.Count - 1; i++)
|
||||
{
|
||||
Angulo = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
|
||||
DistAngulo = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
|
||||
var a = UltimasLeituras[i];
|
||||
var b = UltimasLeituras[i + 1];
|
||||
|
||||
double angSeg = GPSUtils.CalcularOrientacao(a, b); // em graus
|
||||
double dSeg = GPSUtils.DistanciaEntrePontos(a, b); // em metros
|
||||
if (dSeg <= 0) continue; // ignora degrau zero
|
||||
|
||||
double rad = angSeg * Math.PI / 180.0;
|
||||
sumX += Math.Cos(rad) * dSeg; // peso = distância
|
||||
sumY += Math.Sin(rad) * dSeg;
|
||||
distAcum += dSeg;
|
||||
}
|
||||
|
||||
// Se tudo foi zero (parado), caia para heading instantâneo
|
||||
double anguloMovimento;
|
||||
if (distAcum <= 0)
|
||||
{
|
||||
anguloMovimento = UltimaLeitura.OrientacaoReal; // heading como fallback parado
|
||||
}
|
||||
else
|
||||
{
|
||||
// ✅ Obtém os últimos pontos, respeitando o número mínimo de leituras
|
||||
var pontos = GPSTrajetoria
|
||||
.OrderByDescending(x => x.Momento)
|
||||
.ThenByDescending(x => x.DataHora)
|
||||
.Take(Math.Min(PontosConsiderarAngulo, GPSTrajetoria.Count))
|
||||
.ToList();
|
||||
|
||||
// ✅ Se houver apenas um ponto, adicionamos a última leitura do GPS
|
||||
if (pontos.Count == 1)
|
||||
{
|
||||
pontos.Insert(0, UltimaLeitura);
|
||||
}
|
||||
|
||||
// ✅ Ordena os pontos do mais antigo para o mais recente (somente uma vez)
|
||||
pontos.Reverse();
|
||||
|
||||
// ✅ Calcula o ângulo médio e a distância total
|
||||
double somaAngulo = 0;
|
||||
for (int i = 0; i < pontos.Count - 1; i++)
|
||||
{
|
||||
somaAngulo += GPSUtils.CalcularOrientacao(pontos[i], pontos[i + 1]);
|
||||
DistAngulo += GPSUtils.DistanciaEntrePontos(pontos[i], pontos[i + 1]);
|
||||
}
|
||||
double mediaAngulo = somaAngulo / (pontos.Count - 1);
|
||||
|
||||
// ✅ Corrige valores inválidos
|
||||
Angulo = double.IsNaN(mediaAngulo) ? 0.0 : mediaAngulo;
|
||||
anguloMovimento = Math.Atan2(sumY, sumX) * 180.0 / Math.PI;
|
||||
anguloMovimento = GPSUtils.NormalizarAngulo(anguloMovimento);
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
Angulo = 0.0;
|
||||
DistAngulo = 0.0;
|
||||
}
|
||||
|
||||
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
||||
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
||||
// Distância linear entre a 1ª e a última da janela (boa para "confiabilidade")
|
||||
var first = UltimasLeituras[0];
|
||||
var last = UltimasLeituras[UltimasLeituras.Count - 1];
|
||||
double distLinear = GPSUtils.DistanciaEntrePontos(first, last);
|
||||
|
||||
UltimaLeitura.OrientacaoMovimento = Angulo;
|
||||
UltimaLeitura.Distancia = DistAngulo;
|
||||
// Atualiza campos
|
||||
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
|
||||
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
|
||||
|
||||
UltimaLeitura.OrientacaoMovimento = anguloMovimento;
|
||||
UltimaLeitura.Distancia = distLinear; // use a linear como sinal de "Δpos confiável" para a fusão
|
||||
}
|
||||
|
||||
DefinirAnguloCarro();
|
||||
}
|
||||
|
|
@ -1152,14 +1156,16 @@ namespace AgroBase.Services
|
|||
// O GPS está disponível
|
||||
//anguloFinal = UltimaLeitura.OrientacaoReal;
|
||||
|
||||
double anguloGPS = UltimaLeitura.OrientacaoReal;
|
||||
double anguloGPSMovimento = UltimaLeitura.OrientacaoMovimento;
|
||||
double distAnguloGPS = UltimaLeitura.Distancia;
|
||||
double heading = UltimaLeitura.OrientacaoReal;
|
||||
double angMov = UltimaLeitura.OrientacaoMovimento;
|
||||
double distJan = UltimaLeitura.Distancia;
|
||||
double velPercent = Variaveis.OperacaoEmAndamento.Simulando ? Variaveis.OperacaoEmAndamento.Controle.RPM_SP : Variaveis.OperacaoEmAndamento.Sensoriamento.Movimentacao?.RPMMedio ?? 0;
|
||||
bool rtkFix = UltimaLeitura.QualidadeFix == TiposCorrecaoGPS.RTKFixo;
|
||||
double angAnterior = UltimaLeitura.AnguloCarroDefinido;
|
||||
|
||||
//anguloFinal = CalcularAnguloUnificado(anguloGPS, UltimaLeitura.QualidadeFix, anguloGPSMovimento, distAnguloGPS);
|
||||
//anguloFinal = GPSUtils.MisturarAngulosPorDistancia(anguloGPS, anguloGPSMovimento, distAnguloGPS, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras, 0.03, 0.3);
|
||||
|
||||
anguloFinal = anguloGPS;
|
||||
anguloFinal = FundirHeadingComMovimento(heading, angMov, distJan, velPercent, rtkFix, angFundidoAnterior: angAnterior, alfaLowPass: 0.25);
|
||||
}
|
||||
else if (imuIniciado)
|
||||
{
|
||||
|
|
@ -1175,88 +1181,72 @@ namespace AgroBase.Services
|
|||
UltimaLeitura.AnguloCarroDefinido = GPSUtils.NormalizarAngulo(anguloFinal + HeadingOffsetSimulador + HeadingOffsetCorrecao);
|
||||
}
|
||||
|
||||
public static double CalcularAnguloUnificado(double orientacaoReal, TiposCorrecaoGPS precisaoGPS, double anguloMovimento, double distanciaMovimento, double? anguloWT901C = null)
|
||||
public static double FundirHeadingComMovimento(
|
||||
double headingDeg, // OrientacaoReal (antena/corpo)
|
||||
double angMovDeg, // OrientacaoMovimento (calculada acima)
|
||||
double distLinearJanela, // UltimaLeitura.Distancia (Δpos linear entre 1ª e última amostra)
|
||||
double velPercent = 0.0, // se tiver velocidade filtrada, passe aqui; senão 0
|
||||
bool rtkFix = true, // se tiver essa info
|
||||
double angFundidoAnterior = double.NaN, // para low-pass; passe double.NaN para desabilitar
|
||||
double alfaLowPass = 0.25 // 0→sem low-pass; 0.2–0.35 costuma ser bom
|
||||
)
|
||||
{
|
||||
// Pesos para cada fonte de dados
|
||||
double pesoGPS = 0.0;
|
||||
double pesoWT901C = 0.0;
|
||||
double pesoMovimento = 0.0;
|
||||
// Parâmetros sintonizados para 5 Hz
|
||||
double distMin = 0.03; // ~3 cm → considera "parado"
|
||||
double distFull = 0.25; // ~25 cm → deslocamento confiável
|
||||
double pesoMinHeading = 0.30; // mantém um "fio" do heading mesmo em alta confiança no movimento
|
||||
double fatorConfMovSemFix = 0.80; // penaliza confiança no movimento se não for RTK FIX
|
||||
|
||||
// Definir pesos com base na precisão do GPS
|
||||
switch (precisaoGPS)
|
||||
// Confiança em "movimento" por distância da janela (0..1)
|
||||
double fd = Smoothstep(0, 1, Norm(distLinearJanela, distMin, distFull));
|
||||
// Confiança por velocidade (0..1)
|
||||
double fv = velPercent / 100.0;
|
||||
|
||||
double confMov = Math.Max(fd, fv);
|
||||
if (!rtkFix) confMov *= fatorConfMovSemFix;
|
||||
|
||||
// Peso do heading = 1 - confMov, mas preservando contribuição mínima proporcional
|
||||
double pesoHeading = 1.0 - confMov;
|
||||
double minHeading = pesoMinHeading * confMov;
|
||||
if (pesoHeading < minHeading) pesoHeading = minHeading;
|
||||
if (pesoHeading > 1.0) pesoHeading = 1.0;
|
||||
|
||||
double angMisto = MisturarAngulosCircular(headingDeg, angMovDeg, pesoHeading);
|
||||
|
||||
if (!double.IsNaN(angFundidoAnterior) && alfaLowPass > 0)
|
||||
angMisto = LowPassAngle(angFundidoAnterior, angMisto, alfaLowPass);
|
||||
|
||||
return GPSUtils.NormalizarAngulo(angMisto);
|
||||
|
||||
// ---------- helpers locais ----------
|
||||
double Norm(double v, double lo, double hi)
|
||||
{
|
||||
/*case TiposCorrecaoGPS.RTKFixo:
|
||||
pesoGPS = 1.0;
|
||||
pesoWT901C = 0.0;
|
||||
pesoMovimento = 0.0;
|
||||
break;
|
||||
|
||||
case TiposCorrecaoGPS.RTKFlutuante:
|
||||
pesoGPS = 0.8;
|
||||
pesoWT901C = anguloWT901C.HasValue ? 0.2 : 0.0;
|
||||
pesoMovimento = Math.Min(0.2, distanciaMovimento / 10.0);
|
||||
break;*/
|
||||
|
||||
case TiposCorrecaoGPS.RTKFixo:
|
||||
case TiposCorrecaoGPS.RTKFlutuante:
|
||||
// Definir limites para o peso do ângulo do UM982
|
||||
double minPesoMovimento = 0.0; // 0%
|
||||
double maxPesoMovimento = 1.0; // 95%
|
||||
double distanciaMin = 0.05; // Distância média quando o robô anda devagar
|
||||
double distanciaMax = 1.0; // Distância máxima razoável para interpolação
|
||||
|
||||
// Cálculo do peso baseado na distância percorrida
|
||||
pesoMovimento = minPesoMovimento + (maxPesoMovimento - minPesoMovimento) * Math.Min(1.0, Math.Max(0.0, (distanciaMovimento - distanciaMin) / (distanciaMax - distanciaMin)));
|
||||
|
||||
// O restante do peso vai para a orientação real do UM982
|
||||
pesoGPS = 1.0 - pesoMovimento;
|
||||
|
||||
pesoWT901C = 0.0;
|
||||
break;
|
||||
|
||||
case TiposCorrecaoGPS.DGPS:
|
||||
pesoGPS = 0.5;
|
||||
pesoWT901C = anguloWT901C.HasValue ? 0.4 : 0.0;
|
||||
pesoMovimento = Math.Min(0.5, distanciaMovimento / 5.0);
|
||||
break;
|
||||
|
||||
case TiposCorrecaoGPS.Autonomo:
|
||||
default:
|
||||
pesoGPS = 0.3;
|
||||
pesoWT901C = anguloWT901C.HasValue ? 0.5 : 0.0;
|
||||
pesoMovimento = Math.Min(0.8, distanciaMovimento / 3.0);
|
||||
break;
|
||||
if (hi <= lo) return v >= hi ? 1.0 : 0.0;
|
||||
double t = (v - lo) / (hi - lo);
|
||||
if (t < 0) t = 0; else if (t > 1) t = 1;
|
||||
return t;
|
||||
}
|
||||
|
||||
// ⚠️ Ajuste quando WT901C estiver indisponível:
|
||||
if (!anguloWT901C.HasValue)
|
||||
double Smoothstep(double a, double b, double x)
|
||||
{
|
||||
// **Redistribuir o peso do WT901C para GPS e Movimento**
|
||||
pesoGPS += pesoWT901C / 2.0;
|
||||
pesoMovimento += pesoWT901C / 2.0;
|
||||
pesoWT901C = 0.0; // WT901C não influencia se for `null`
|
||||
// aqui x já normalizado 0..1 no uso acima
|
||||
x = FuncoesMatematicas.Clamp(x, 0.0, 1.0);
|
||||
return x * x * (3 - 2 * x);
|
||||
}
|
||||
|
||||
// Normalizar os pesos para garantir que sempre somem 1
|
||||
double somaPesos = pesoGPS + pesoWT901C + pesoMovimento;
|
||||
pesoGPS /= somaPesos;
|
||||
pesoWT901C /= somaPesos;
|
||||
pesoMovimento /= somaPesos;
|
||||
|
||||
// Calcular o ângulo unificado
|
||||
double anguloUnificado = (orientacaoReal * pesoGPS) + (anguloMovimento * pesoMovimento);
|
||||
if (anguloWT901C.HasValue)
|
||||
double MisturarAngulosCircular(double aDeg, double bDeg, double pesoA /*0..1*/)
|
||||
{
|
||||
anguloUnificado += (anguloWT901C.Value * pesoWT901C);
|
||||
double a = aDeg * Math.PI / 180.0;
|
||||
double b = bDeg * Math.PI / 180.0;
|
||||
double x = Math.Cos(a) * pesoA + Math.Cos(b) * (1.0 - pesoA);
|
||||
double y = Math.Sin(a) * pesoA + Math.Sin(b) * (1.0 - pesoA);
|
||||
return Math.Atan2(y, x) * 180.0 / Math.PI;
|
||||
}
|
||||
double LowPassAngle(double atualDeg, double novoDeg, double alfa)
|
||||
{
|
||||
double diff = GPSUtils.NormalizarAngulo(novoDeg - atualDeg);
|
||||
return GPSUtils.NormalizarAngulo(atualDeg + alfa * diff);
|
||||
}
|
||||
|
||||
// Ajustar para espaço circular (0 a 360 graus)
|
||||
anguloUnificado = GPSUtils.NormalizarAngulo(anguloUnificado);
|
||||
|
||||
return anguloUnificado;
|
||||
}
|
||||
|
||||
|
||||
public static void AtualizaDadosRedis()
|
||||
{
|
||||
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Simulando ? historicoPosicao.Peek() : UltimaLeitura;
|
||||
|
|
|
|||
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": "13403636820469100"
|
||||
"next_scheduled_calculation_time": "13403636820469509"
|
||||
}
|
||||
|
|
|
|||
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/22-15:30:19.498 1b7c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/09/22-15:30:19.509 1b7c Recovering log #3
|
||||
2025/09/22-15:30:19.512 1b7c Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/09/23-09:38:49.956 4b78 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/09/23-09:38:49.966 4b78 Recovering log #3
|
||||
2025/09/23-09:38:49.969 4b78 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/22-14:17:21.935 12a0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/09/22-14:17:21.943 12a0 Recovering log #3
|
||||
2025/09/22-14:17:21.946 12a0 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/09/23-09:33:56.629 19b0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/09/23-09:33:56.643 19b0 Recovering log #3
|
||||
2025/09/23-09:33:56.647 19b0 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":"13403124628748627","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":14427},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:67d:1400:cd2a:9909:b778:feef","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":"13403189309995191","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":10486},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:67d:1400:98be:836a:a20c:3874","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":1790097601.177978,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1758561601.177983}],"version":2}
|
||||
{"sts":[{"expiry":1790164822.419645,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1758628822.419655}],"version":2}
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,3 +1,3 @@
|
|||
2025/09/22-15:32:03.868 1b7c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/09/22-15:32:03.869 1b7c Recovering log #3
|
||||
2025/09/22-15:32:03.872 1b7c Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/09/23-09:42:46.689 4b78 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/09/23-09:42:46.691 4b78 Recovering log #3
|
||||
2025/09/23-09:42:46.694 4b78 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/22-15:11:23.486 12a0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/09/22-15:11:23.487 12a0 Recovering log #3
|
||||
2025/09/22-15:11:23.490 12a0 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/09/23-09:38:42.409 19b0 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/09/23-09:38:42.410 19b0 Recovering log #3
|
||||
2025/09/23-09:38:42.413 19b0 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/22-15:30:19.422 14dc Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/09/22-15:30:19.425 14dc Recovering log #7
|
||||
2025/09/22-15:30:19.425 14dc Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
2025/09/23-09:38:49.884 2874 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/09/23-09:38:49.887 2874 Recovering log #7
|
||||
2025/09/23-09:38:49.887 2874 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/22-14:17:21.861 5d88 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/09/22-14:17:21.863 5d88 Recovering log #7
|
||||
2025/09/22-14:17:21.864 5d88 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
2025/09/23-09:33:56.549 1c98 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/09/23-09:33:56.551 1c98 Recovering log #7
|
||||
2025/09/23-09:33:56.551 1c98 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.
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
File diff suppressed because one or more lines are too long
|
|
@ -17,7 +17,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_a930f18588199d9d6021d4c49b85c784 {
|
||||
#map_b5c20ec1e6bf396924dc4485751b4881 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -54,14 +54,14 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_a930f18588199d9d6021d4c49b85c784" ></div>
|
||||
<div class="folium-map" id="map_b5c20ec1e6bf396924dc4485751b4881" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_a930f18588199d9d6021d4c49b85c784 = L.map(
|
||||
"map_a930f18588199d9d6021d4c49b85c784",
|
||||
var map_b5c20ec1e6bf396924dc4485751b4881 = L.map(
|
||||
"map_b5c20ec1e6bf396924dc4485751b4881",
|
||||
{
|
||||
center: [0.0, 0.0],
|
||||
crs: L.CRS.EPSG3857,
|
||||
|
|
@ -96,7 +96,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_a930f18588199d9d6021d4c49b85c784);
|
||||
trajeto_json.addTo(map_b5c20ec1e6bf396924dc4485751b4881);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -159,9 +159,9 @@
|
|||
|
||||
var marcadorEquipamento = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_a930f18588199d9d6021d4c49b85c784);
|
||||
}).addTo(map_b5c20ec1e6bf396924dc4485751b4881);
|
||||
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_a930f18588199d9d6021d4c49b85c784);
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_b5c20ec1e6bf396924dc4485751b4881);
|
||||
var icon = L.AwesomeMarkers.icon(
|
||||
{"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
|
||||
);
|
||||
|
|
@ -226,7 +226,7 @@
|
|||
}
|
||||
|
||||
if (foco) {
|
||||
map_a930f18588199d9d6021d4c49b85c784.setView(novaPosicao, map_a930f18588199d9d6021d4c49b85c784.getZoom());
|
||||
map_b5c20ec1e6bf396924dc4485751b4881.setView(novaPosicao, map_b5c20ec1e6bf396924dc4485751b4881.getZoom());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -248,7 +248,7 @@
|
|||
marcadorDinamico.setRotationAngle(angulo);
|
||||
|
||||
adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
|
||||
map_a930f18588199d9d6021d4c49b85c784.setView(novaPosicao, map_a930f18588199d9d6021d4c49b85c784.getZoom());*/
|
||||
map_b5c20ec1e6bf396924dc4485751b4881.setView(novaPosicao, map_b5c20ec1e6bf396924dc4485751b4881.getZoom());*/
|
||||
});
|
||||
|
||||
function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
Binary file not shown.
|
|
@ -253,7 +253,7 @@ class ControladorMPC:
|
|||
|
||||
def _corrigir_pontos_visitados(self, x, y, pontos_visitados: np.ndarray, idx_atual: int = -1,
|
||||
limite_max_avanco: float = 5.0, # em METROS
|
||||
limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1):
|
||||
limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True):
|
||||
try:
|
||||
p_xy, p_mg, p_s = self._p_xy, self._p_margem, self._p_s
|
||||
N = p_xy.shape[0]
|
||||
|
|
@ -291,7 +291,7 @@ class ControladorMPC:
|
|||
off = int(np.argmax(hits))
|
||||
idx = start_idx + off
|
||||
pontos_visitados[:idx] = True
|
||||
if velocidade > -1:
|
||||
if velocidade > -1 and look_ahead:
|
||||
s_hit = self._p_s[idx]
|
||||
s_goal = s_hit + self.d_look_ahead_m(velocidade, dt)
|
||||
idx_alvo = int(np.searchsorted(self._p_s, s_goal, side="left"))
|
||||
|
|
@ -1027,6 +1027,7 @@ class ControladorMPC:
|
|||
"angulo": candidato["comandos"][-1][1],
|
||||
}
|
||||
|
||||
#look_ahead = passo > 2 and status_carro not in [StatusCarroMapa.Manobrando.value, StatusCarroMapa.Direcionando.value]
|
||||
idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados, velocidade=v_sim, dt=dt_pred)
|
||||
#if passo == 0:
|
||||
# print(f"idx_alvo: {idx_alvo}")
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -592,7 +592,7 @@ class CostmapFuser:
|
|||
row_dist_m, row_scale_x, self.central_cols,
|
||||
use_persistence=True,
|
||||
velocidade_mps=velocidade_ms,
|
||||
a_max_freio=0.4, margem_parada=0.60,
|
||||
a_max_freio=0.2, margem_parada=0.60,
|
||||
N_on=2, N_off=3, blackout_imediato=True,
|
||||
|
||||
det_score_f = det_score,
|
||||
|
|
|
|||
Loading…
Reference in New Issue