Compare commits
No commits in common. "2fe3fa4f0e75ff5b7274aaf9deb800fa25fdc8c8" and "876b24c5fc62b71292a5334577d1eb047ae615a1" have entirely different histories.
2fe3fa4f0e
...
876b24c5fc
|
|
@ -136,37 +136,13 @@ namespace AgroBase.Forms.IHM.Operacao
|
||||||
chkRegistrarPosProcessamento.FlatAppearance.BorderColor = ativo ? HmiTheme.Success : HmiTheme.Border;
|
chkRegistrarPosProcessamento.FlatAppearance.BorderColor = ativo ? HmiTheme.Success : HmiTheme.Border;
|
||||||
}
|
}
|
||||||
|
|
||||||
private void DefinirOperacaoParametrizando(
|
private void DefinirOperacaoParametrizando(OperacaoModel operacao, bool recarregarMapa = true)
|
||||||
OperacaoModel operacao,
|
|
||||||
bool recarregarMapa = true,
|
|
||||||
bool registrarComoFonteAutoritativa = true)
|
|
||||||
{
|
{
|
||||||
operacaoParametrizando = operacao;
|
operacaoParametrizando = operacao;
|
||||||
|
|
||||||
/*
|
_operacaoFonteCarregada = operacao;
|
||||||
* IMPORTANTE:
|
_parametrosFonteCarregados = operacao?.Parametros;
|
||||||
*
|
_controleFonteCarregado = operacao?.Parametros?.Controle;
|
||||||
* A operação exibida na tela nem sempre é a operação autoritativa
|
|
||||||
* recebida da base. Ao trocar o modo no ComboBox, por exemplo,
|
|
||||||
* BtnRedefinir_Click cria uma nova OperacaoModel apenas como
|
|
||||||
* RASCUNHO local de edição.
|
|
||||||
*
|
|
||||||
* Se esse rascunho for registrado em _operacaoFonteCarregada, o
|
|
||||||
* refresh periódico da IHM compara o rascunho com
|
|
||||||
* Variaveis.OperacaoEmAndamento, entende que a fonte mudou e
|
|
||||||
* recarrega imediatamente a operação antiga, desfazendo a seleção
|
|
||||||
* do operador.
|
|
||||||
*
|
|
||||||
* Portanto somente cargas vindas da fonte autoritativa atualizam os
|
|
||||||
* marcadores usados por SincronizarComOperacaoAtualSeNecessario().
|
|
||||||
* Um rascunho local altera somente operacaoParametrizando.
|
|
||||||
*/
|
|
||||||
if (registrarComoFonteAutoritativa)
|
|
||||||
{
|
|
||||||
_operacaoFonteCarregada = operacao;
|
|
||||||
_parametrosFonteCarregados = operacao?.Parametros;
|
|
||||||
_controleFonteCarregado = operacao?.Parametros?.Controle;
|
|
||||||
}
|
|
||||||
|
|
||||||
CarregarDadosGerais();
|
CarregarDadosGerais();
|
||||||
_ucMovimento.DefinirOperacao(operacaoParametrizando);
|
_ucMovimento.DefinirOperacao(operacaoParametrizando);
|
||||||
|
|
@ -266,13 +242,7 @@ namespace AgroBase.Forms.IHM.Operacao
|
||||||
novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados;
|
novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados;
|
||||||
}
|
}
|
||||||
|
|
||||||
// É um rascunho local. Não substitui os marcadores da última
|
DefinirOperacaoParametrizando(novaOperacao, recarregarMapa: false);
|
||||||
// fonte autoritativa, senão o refresh de 1 s restaura o modo antigo.
|
|
||||||
DefinirOperacaoParametrizando(
|
|
||||||
novaOperacao,
|
|
||||||
recarregarMapa: false,
|
|
||||||
registrarComoFonteAutoritativa: false
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -530,7 +530,7 @@ namespace AgroBase.Models.Modules
|
||||||
*/
|
*/
|
||||||
private const double FolgaMinimaReferencia = 0.05;
|
private const double FolgaMinimaReferencia = 0.05;
|
||||||
private const double FolgaAlertaReferencia = 6.0;
|
private const double FolgaAlertaReferencia = 6.0;
|
||||||
private const double FolgaMaximaReferencia = 9.5;
|
private const double FolgaMaximaReferencia = 8.0;
|
||||||
private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120);
|
private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120);
|
||||||
private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30);
|
private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30);
|
||||||
private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30);
|
private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30);
|
||||||
|
|
|
||||||
|
|
@ -677,7 +677,7 @@ namespace AgroBase.Models
|
||||||
ImuDirecionalPreferirArco = false,
|
ImuDirecionalPreferirArco = false,
|
||||||
OakParadaPorObstaculo = false,
|
OakParadaPorObstaculo = false,
|
||||||
DirAuxilioSonar = false,
|
DirAuxilioSonar = false,
|
||||||
MovAuxilioSonar = false,
|
MovAuxilioSonar = true,
|
||||||
MpcHorizonte = 4.0,
|
MpcHorizonte = 4.0,
|
||||||
AnteciparManobraCorredorM = 0.0,
|
AnteciparManobraCorredorM = 0.0,
|
||||||
|
|
||||||
|
|
@ -1065,7 +1065,7 @@ namespace AgroBase.Models
|
||||||
ImuDirecionalPreferirArco = false,
|
ImuDirecionalPreferirArco = false,
|
||||||
OakParadaPorObstaculo = false,
|
OakParadaPorObstaculo = false,
|
||||||
DirAuxilioSonar = false,
|
DirAuxilioSonar = false,
|
||||||
MovAuxilioSonar = false,
|
MovAuxilioSonar = true,
|
||||||
MpcHorizonte = 4.0,
|
MpcHorizonte = 4.0,
|
||||||
AnteciparManobraCorredorM = 0,
|
AnteciparManobraCorredorM = 0,
|
||||||
|
|
||||||
|
|
@ -1308,19 +1308,6 @@ namespace AgroBase.Models
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
private static bool ExigeSincronizacaoMapaCritico(OperacaoModel op)
|
|
||||||
{
|
|
||||||
/*
|
|
||||||
* Operacao Manual nao depende de mapa, trajetoria nem MPC.
|
|
||||||
* A barreira critica de MAPSYNC existe para os modos automaticos
|
|
||||||
* e nao pode bloquear a vida da operacao Manual.
|
|
||||||
*
|
|
||||||
* Mantemos todos os demais modos com o comportamento historico
|
|
||||||
* para evitar regressao em MapaGPS, RetornoBase, TreinamentoIA etc.
|
|
||||||
*/
|
|
||||||
return op?.Parametros?.Modo != ModoOperacao.Manual;
|
|
||||||
}
|
|
||||||
|
|
||||||
public static OperacaoModel CarregarParamerosOperacaoBase(OperacaoParametrosModel parametros)
|
public static OperacaoModel CarregarParamerosOperacaoBase(OperacaoParametrosModel parametros)
|
||||||
{
|
{
|
||||||
if (parametros?.Modo == null)
|
if (parametros?.Modo == null)
|
||||||
|
|
@ -1359,34 +1346,7 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao();
|
novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao();
|
||||||
|
|
||||||
bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(novaOp);
|
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||||
|
|
||||||
/*
|
|
||||||
* Manual fica configurado assim que seus parametros locais sao
|
|
||||||
* carregados. Antes ele recebia configurado=false como MapaGPS,
|
|
||||||
* mas nunca existia um MAPSYNC capaz de promover esse estado para
|
|
||||||
* true. Isso mantinha o status eternamente em Parametrizando.
|
|
||||||
*/
|
|
||||||
RedisService.AtualizarCampos(
|
|
||||||
CtxKey.DadosOperacao,
|
|
||||||
("configurado", !exigeMapaCritico),
|
|
||||||
("mapa_barreira_critica", false),
|
|
||||||
("mapa_confirmado", false)
|
|
||||||
);
|
|
||||||
|
|
||||||
if (!exigeMapaCritico)
|
|
||||||
{
|
|
||||||
HealthWorkerService.AtualizarDadosOperacao(
|
|
||||||
forcar: true,
|
|
||||||
sincronizarMapa: false,
|
|
||||||
configurado: true
|
|
||||||
);
|
|
||||||
|
|
||||||
Variaveis.MostrarLog(
|
|
||||||
"[OperacaoModel.CarregarParamerosOperacaoBase] " +
|
|
||||||
"Operacao Manual configurada sem MAPSYNC."
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
frmInstancial.frmIHM?._frmOperacao?.AtualizarParametrosTela(novaOp);
|
frmInstancial.frmIHM?._frmOperacao?.AtualizarParametrosTela(novaOp);
|
||||||
|
|
||||||
|
|
@ -1440,15 +1400,7 @@ namespace AgroBase.Models
|
||||||
{ T_Code.Atu, new Dictionary<string, bool>() { } },
|
{ T_Code.Atu, new Dictionary<string, bool>() { } },
|
||||||
};
|
};
|
||||||
|
|
||||||
bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(op);
|
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||||
|
|
||||||
RedisService.AtualizarCampos(
|
|
||||||
CtxKey.DadosOperacao,
|
|
||||||
("configurado", !exigeMapaCritico),
|
|
||||||
("mapa_barreira_critica", false),
|
|
||||||
("mapa_confirmado", false)
|
|
||||||
);
|
|
||||||
|
|
||||||
WeedWorkerService.ReiniciarLeituraAnalise();
|
WeedWorkerService.ReiniciarLeituraAnalise();
|
||||||
VisualWorkerService.ReiniciarLeituraAnalise();
|
VisualWorkerService.ReiniciarLeituraAnalise();
|
||||||
|
|
||||||
|
|
@ -1464,13 +1416,8 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
op.GPSTrajetoria = new List<GPSModel>();
|
op.GPSTrajetoria = new List<GPSModel>();
|
||||||
|
|
||||||
if (exigeMapaCritico)
|
op.Trajetoria?.ProjetarTrajetoriaFixa();
|
||||||
{
|
op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false;
|
||||||
op.Trajetoria?.ProjetarTrajetoriaFixa();
|
|
||||||
|
|
||||||
if (op.Trajetoria != null)
|
|
||||||
op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
op.Sensoriamento.UltimoRegistroLog = agora;
|
op.Sensoriamento.UltimoRegistroLog = agora;
|
||||||
op.idxLog = 0;
|
op.idxLog = 0;
|
||||||
|
|
@ -1481,47 +1428,20 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
op.IniciarTimerLogsOperacao();
|
op.IniciarTimerLogsOperacao();
|
||||||
|
|
||||||
if (exigeMapaCritico)
|
// Barreira crítica: a operação só pode seguir para a fase de
|
||||||
|
// execução depois que o Manager/Python confirmar exatamente a
|
||||||
|
// trajetória completa projetada neste OperacaoModel. O método
|
||||||
|
// faz retry enquanto esta operação continuar sendo a atual.
|
||||||
|
bool mapaConfirmado = await HealthWorkerService
|
||||||
|
.SincronizarMapaCriticoAsync(op)
|
||||||
|
.ConfigureAwait(false);
|
||||||
|
|
||||||
|
if (!mapaConfirmado)
|
||||||
{
|
{
|
||||||
// Barreira crítica somente para modos que realmente dependem
|
|
||||||
// da trajetoria/mapa no Manager/Python.
|
|
||||||
bool mapaConfirmado = await HealthWorkerService
|
|
||||||
.SincronizarMapaCriticoAsync(op)
|
|
||||||
.ConfigureAwait(false);
|
|
||||||
|
|
||||||
if (!mapaConfirmado)
|
|
||||||
{
|
|
||||||
Variaveis.MostrarLog(
|
|
||||||
"[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada."
|
|
||||||
);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
/*
|
|
||||||
* Manual nao tem MAPSYNC. Publica explicitamente a operacao
|
|
||||||
* como configurada e limpa qualquer barreira residual.
|
|
||||||
* Assim IniciarOperacao retorna normalmente, liberando tambem
|
|
||||||
* a trava _acaoLateralEmAndamento da IHM.
|
|
||||||
*/
|
|
||||||
RedisService.AtualizarCampos(
|
|
||||||
CtxKey.DadosOperacao,
|
|
||||||
("configurado", true),
|
|
||||||
("mapa_barreira_critica", false),
|
|
||||||
("mapa_confirmado", false)
|
|
||||||
);
|
|
||||||
|
|
||||||
HealthWorkerService.AtualizarDadosOperacao(
|
|
||||||
forcar: true,
|
|
||||||
sincronizarMapa: false,
|
|
||||||
configurado: true
|
|
||||||
);
|
|
||||||
|
|
||||||
Variaveis.MostrarLog(
|
Variaveis.MostrarLog(
|
||||||
"[OperacaoModel.IniciarOperacao] " +
|
"[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada."
|
||||||
"Operacao Manual iniciada sem barreira MAPSYNC."
|
|
||||||
);
|
);
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
await op.RealizarCalibragemInicialAsync(true);
|
await op.RealizarCalibragemInicialAsync(true);
|
||||||
|
|
|
||||||
|
|
@ -1020,8 +1020,8 @@ namespace AgroBase.Models
|
||||||
public static double ComprimentoTrasCm { get; } = 40.0; // 107
|
public static double ComprimentoTrasCm { get; } = 40.0; // 107
|
||||||
public static double DistanciaEntreEixosCm { get; } = 92.0;
|
public static double DistanciaEntreEixosCm { get; } = 92.0;
|
||||||
public static double BitolaRodasCm { get; } = 75.0;
|
public static double BitolaRodasCm { get; } = 75.0;
|
||||||
public static double KuDirecional { get; } = 0.35; // 2.65
|
public static double KuDirecional { get; } = 2.65;
|
||||||
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0; // °/s
|
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0;
|
||||||
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
|
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
|
||||||
public static double LeverArmLateralCm { get; set; } = 0.0; // cm
|
public static double LeverArmLateralCm { get; set; } = 0.0; // cm
|
||||||
public static double LarguraEquipamentoMm
|
public static double LarguraEquipamentoMm
|
||||||
|
|
|
||||||
|
|
@ -58,24 +58,9 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
private static bool InverterHeading = true;
|
private static bool InverterHeading = true;
|
||||||
private static int rtk_timeout = 60;
|
private static int rtk_timeout = 60;
|
||||||
|
private static int TempoMin_Ntrip = 10;
|
||||||
/*
|
|
||||||
* Política de RTCM do rover:
|
|
||||||
* 1) base local via MQTT é sempre a fonte preferencial;
|
|
||||||
* 2) NTRIP é somente contingência quando a base local some;
|
|
||||||
* 3) ao retornar RTCM local de forma estável, o rover faz failback
|
|
||||||
* automaticamente para a base local sem desabilitar a contingência.
|
|
||||||
*/
|
|
||||||
public static int RtcmLocalRecenteMs { get; set; } = 3000;
|
|
||||||
public static int RtcmLocalAusenteParaNtripMs { get; set; } = 6000;
|
|
||||||
public static int RtcmLocalEstavelParaFailbackMs { get; set; } = 2500;
|
|
||||||
|
|
||||||
private static volatile bool LoopRTK_Ntrip = false;
|
private static volatile bool LoopRTK_Ntrip = false;
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// Habilita NTRIP como contingência. Não significa que NTRIP seja
|
|
||||||
/// necessariamente a fonte ativa naquele instante.
|
|
||||||
/// </summary>
|
|
||||||
public static volatile bool CorrecaoRTK_Ntrip = false;
|
public static volatile bool CorrecaoRTK_Ntrip = false;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|
@ -384,15 +369,6 @@ namespace AgroBase.Services
|
||||||
private static long _lastRtcmReceivedMono;
|
private static long _lastRtcmReceivedMono;
|
||||||
private static long _lastRtcmWrittenMono;
|
private static long _lastRtcmWrittenMono;
|
||||||
|
|
||||||
// RTCM por origem. O timestamp MQTT é atualizado mesmo enquanto NTRIP
|
|
||||||
// está conectado, pois ele é o watchdog que permite detectar o retorno
|
|
||||||
// da base local e executar failback automático.
|
|
||||||
private static long _lastMqttRtcmReceivedMono;
|
|
||||||
private static long _lastNtripRtcmReceivedMono;
|
|
||||||
private static long _mqttRtcmHealthySinceMono;
|
|
||||||
private static long _rtcmArbitrationStartMono;
|
|
||||||
private static int _preferenciaLocalAtiva;
|
|
||||||
|
|
||||||
private static long _serialBytesReceived;
|
private static long _serialBytesReceived;
|
||||||
private static long _nmeaValid;
|
private static long _nmeaValid;
|
||||||
private static long _nmeaInvalid;
|
private static long _nmeaInvalid;
|
||||||
|
|
@ -598,16 +574,6 @@ namespace AgroBase.Services
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
novaPorta.Open();
|
novaPorta.Open();
|
||||||
|
|
||||||
// Uma nova sessão serial começa com uma janela limpa de
|
|
||||||
// arbitragem. Se a base local estiver disponível, os
|
|
||||||
// próximos RTCMs MQTT repovoarão imediatamente o watchdog.
|
|
||||||
long agoraMono = Stopwatch.GetTimestamp();
|
|
||||||
Interlocked.Exchange(ref _rtcmArbitrationStartMono, agoraMono);
|
|
||||||
Interlocked.Exchange(ref _lastMqttRtcmReceivedMono, 0);
|
|
||||||
Interlocked.Exchange(ref _lastNtripRtcmReceivedMono, 0);
|
|
||||||
Interlocked.Exchange(ref _mqttRtcmHealthySinceMono, 0);
|
|
||||||
Interlocked.Exchange(ref _preferenciaLocalAtiva, 0);
|
|
||||||
}
|
}
|
||||||
catch (Exception ex)
|
catch (Exception ex)
|
||||||
{
|
{
|
||||||
|
|
@ -1819,188 +1785,25 @@ namespace AgroBase.Services
|
||||||
// RTCM VIA MQTT / FILA DE ESCRITA
|
// RTCM VIA MQTT / FILA DE ESCRITA
|
||||||
// ============================================================
|
// ============================================================
|
||||||
|
|
||||||
private static void RegistrarRtcmLocalRecebido(long agoraMono)
|
|
||||||
{
|
|
||||||
long anterior = Interlocked.Exchange(
|
|
||||||
ref _lastMqttRtcmReceivedMono,
|
|
||||||
agoraMono
|
|
||||||
);
|
|
||||||
|
|
||||||
double gapAnteriorMs = GetAgeMs(anterior);
|
|
||||||
int continuidadeMaxMs = Math.Max(500, RtcmLocalRecenteMs);
|
|
||||||
|
|
||||||
if (anterior <= 0 ||
|
|
||||||
double.IsInfinity(gapAnteriorMs) ||
|
|
||||||
gapAnteriorMs > continuidadeMaxMs)
|
|
||||||
{
|
|
||||||
Interlocked.Exchange(
|
|
||||||
ref _mqttRtcmHealthySinceMono,
|
|
||||||
agoraMono
|
|
||||||
);
|
|
||||||
|
|
||||||
if (anterior <= 0 ||
|
|
||||||
double.IsInfinity(gapAnteriorMs) ||
|
|
||||||
gapAnteriorMs >= Math.Max(
|
|
||||||
1000,
|
|
||||||
RtcmLocalAusenteParaNtripMs
|
|
||||||
))
|
|
||||||
{
|
|
||||||
Interlocked.Exchange(
|
|
||||||
ref _preferenciaLocalAtiva,
|
|
||||||
0
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (Interlocked.Read(ref _mqttRtcmHealthySinceMono) <= 0)
|
|
||||||
{
|
|
||||||
Interlocked.CompareExchange(
|
|
||||||
ref _mqttRtcmHealthySinceMono,
|
|
||||||
anterior,
|
|
||||||
0
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private static bool RtcmLocalRecente()
|
|
||||||
{
|
|
||||||
long ultimo = Interlocked.Read(
|
|
||||||
ref _lastMqttRtcmReceivedMono
|
|
||||||
);
|
|
||||||
|
|
||||||
if (ultimo <= 0)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
return GetAgeMs(ultimo) <=
|
|
||||||
Math.Max(500, RtcmLocalRecenteMs);
|
|
||||||
}
|
|
||||||
|
|
||||||
private static bool RtcmLocalEstavelParaPreferencia()
|
|
||||||
{
|
|
||||||
if (!RtcmLocalRecente())
|
|
||||||
return false;
|
|
||||||
|
|
||||||
long desde = Interlocked.Read(
|
|
||||||
ref _mqttRtcmHealthySinceMono
|
|
||||||
);
|
|
||||||
|
|
||||||
if (desde <= 0)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
return GetAgeMs(desde) >=
|
|
||||||
Math.Max(0, RtcmLocalEstavelParaFailbackMs);
|
|
||||||
}
|
|
||||||
|
|
||||||
private static bool DeveAtivarNtripComoFallback()
|
|
||||||
{
|
|
||||||
if (!CorrecaoRTK_Ntrip)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
// Enquanto a base local estiver chegando, NTRIP nunca toma a frente.
|
|
||||||
if (RtcmLocalRecente())
|
|
||||||
return false;
|
|
||||||
|
|
||||||
long ultimoLocal = Interlocked.Read(
|
|
||||||
ref _lastMqttRtcmReceivedMono
|
|
||||||
);
|
|
||||||
|
|
||||||
int ausenciaMinMs =
|
|
||||||
Math.Max(1000, RtcmLocalAusenteParaNtripMs);
|
|
||||||
|
|
||||||
if (ultimoLocal > 0)
|
|
||||||
{
|
|
||||||
if (GetAgeMs(ultimoLocal) < ausenciaMinMs)
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// No startup dá chance para a base local aparecer antes de abrir
|
|
||||||
// qualquer conexão NTRIP só porque ainda existe internet.
|
|
||||||
long inicio = Interlocked.Read(
|
|
||||||
ref _rtcmArbitrationStartMono
|
|
||||||
);
|
|
||||||
|
|
||||||
if (inicio <= 0)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
if (GetAgeMs(inicio) < ausenciaMinMs)
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
Interlocked.Exchange(ref _preferenciaLocalAtiva, 0);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
private static string ObterFonteRtcmAtiva()
|
|
||||||
{
|
|
||||||
// A base local vence inclusive durante a pequena janela em que a
|
|
||||||
// sessão TCP NTRIP ainda está fechando.
|
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
return "mqtt_local";
|
|
||||||
|
|
||||||
if (Ntrip_Conectado)
|
|
||||||
return "ntrip";
|
|
||||||
|
|
||||||
if (RtcmLocalRecente())
|
|
||||||
return "mqtt_local";
|
|
||||||
|
|
||||||
return "nenhuma";
|
|
||||||
}
|
|
||||||
|
|
||||||
public static void AplicarCorrecaoRTK_Mqtt(byte[] correcao, int bytesRead)
|
public static void AplicarCorrecaoRTK_Mqtt(byte[] correcao, int bytesRead)
|
||||||
{
|
{
|
||||||
if (correcao == null ||
|
/*
|
||||||
bytesRead <= 0 ||
|
* Uma solução RTK não pode receber correções simultâneas de duas
|
||||||
bytesRead > correcao.Length ||
|
* bases/referências diferentes. Se NTRIP estiver efetivamente
|
||||||
bytesRead > RtcmMaxPayloadBytes)
|
* conectado, ele é a fonte ativa e RTCM via MQTT é ignorado.
|
||||||
|
*
|
||||||
|
* Se o NTRIP cair, Ntrip_Conectado volta a false e o MQTT pode
|
||||||
|
* assumir novamente sem exigir reinicialização do serviço.
|
||||||
|
*/
|
||||||
|
if (Ntrip_Conectado)
|
||||||
|
return;
|
||||||
|
|
||||||
|
if (correcao == null || bytesRead <= 0 || bytesRead > correcao.Length || bytesRead > RtcmMaxPayloadBytes)
|
||||||
{
|
{
|
||||||
Interlocked.Increment(ref _rtcmDroppedInvalid);
|
Interlocked.Increment(ref _rtcmDroppedInvalid);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
long agoraMono = Stopwatch.GetTimestamp();
|
|
||||||
RegistrarRtcmLocalRecebido(agoraMono);
|
|
||||||
|
|
||||||
bool localEstavel = RtcmLocalEstavelParaPreferencia();
|
|
||||||
|
|
||||||
if (localEstavel)
|
|
||||||
{
|
|
||||||
bool primeiraConfirmacao =
|
|
||||||
Interlocked.Exchange(
|
|
||||||
ref _preferenciaLocalAtiva,
|
|
||||||
1
|
|
||||||
) == 0;
|
|
||||||
|
|
||||||
bool vinhaDeNtrip =
|
|
||||||
Ntrip_Conectado ||
|
|
||||||
GetAgeMs(Interlocked.Read(
|
|
||||||
ref _lastNtripRtcmReceivedMono
|
|
||||||
)) <= 3000.0;
|
|
||||||
|
|
||||||
// Só é failback quando realmente havia contingência NTRIP em uso.
|
|
||||||
// No startup normal com a base local presente não limpa a fila nem
|
|
||||||
// gera um falso evento de troca de fonte.
|
|
||||||
if (primeiraConfirmacao && vinhaDeNtrip)
|
|
||||||
{
|
|
||||||
lock (_rtcmQueueLock)
|
|
||||||
{
|
|
||||||
_rtcmQueue.Clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
Variaveis.MostrarLog(
|
|
||||||
"[GPSService.RTCM][FAILBACK] Base local MQTT estável; " +
|
|
||||||
"retornando à fonte preferencial local."
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Durante a aquisição da base local, uma sessão NTRIP já estabelecida
|
|
||||||
* continua sendo usada. Assim evitamos trocar por causa de um único
|
|
||||||
* pacote MQTT isolado. Depois do tempo de estabilidade, MQTT assume.
|
|
||||||
*/
|
|
||||||
if (Ntrip_Conectado && !localEstavel)
|
|
||||||
return;
|
|
||||||
|
|
||||||
byte[] copia = new byte[bytesRead];
|
byte[] copia = new byte[bytesRead];
|
||||||
Buffer.BlockCopy(correcao, 0, copia, 0, bytesRead);
|
Buffer.BlockCopy(correcao, 0, copia, 0, bytesRead);
|
||||||
|
|
||||||
|
|
@ -2011,25 +1814,8 @@ namespace AgroBase.Services
|
||||||
{
|
{
|
||||||
EnsureBackgroundWorkersStarted();
|
EnsureBackgroundWorkersStarted();
|
||||||
|
|
||||||
// Nunca mistura a contingência NTRIP com uma base local já estável.
|
|
||||||
// Mesmo que o socket NTRIP leve alguns instantes para perceber o
|
|
||||||
// failback, seus frames deixam de entrar na fila imediatamente.
|
|
||||||
if (source == RtcmSource.Ntrip &&
|
|
||||||
RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
long agora = Stopwatch.GetTimestamp();
|
long agora = Stopwatch.GetTimestamp();
|
||||||
|
|
||||||
if (source == RtcmSource.Ntrip)
|
|
||||||
{
|
|
||||||
Interlocked.Exchange(
|
|
||||||
ref _lastNtripRtcmReceivedMono,
|
|
||||||
agora
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
Interlocked.Increment(ref _rtcmReceived);
|
Interlocked.Increment(ref _rtcmReceived);
|
||||||
Interlocked.Exchange(ref _lastRtcmReceivedMono, agora);
|
Interlocked.Exchange(ref _lastRtcmReceivedMono, agora);
|
||||||
|
|
||||||
|
|
@ -2199,12 +1985,8 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
public static async Task IniciarNtripAsync()
|
public static async Task IniciarNtripAsync()
|
||||||
{
|
{
|
||||||
if (!Variaveis.IsAgroBase ||
|
if (!Variaveis.IsAgroBase || !CorrecaoRTK_Ntrip)
|
||||||
!CorrecaoRTK_Ntrip ||
|
|
||||||
!DeveAtivarNtripComoFallback())
|
|
||||||
{
|
|
||||||
return;
|
return;
|
||||||
}
|
|
||||||
|
|
||||||
lock (_workerLock)
|
lock (_workerLock)
|
||||||
{
|
{
|
||||||
|
|
@ -2216,11 +1998,6 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
_ntripCts = new CancellationTokenSource();
|
_ntripCts = new CancellationTokenSource();
|
||||||
|
|
||||||
Variaveis.MostrarLog(
|
|
||||||
"[GPSService.RTCM][FAILOVER] RTCM local ausente; " +
|
|
||||||
"ativando NTRIP como contingência."
|
|
||||||
);
|
|
||||||
|
|
||||||
_ntripTask = Task.Run(
|
_ntripTask = Task.Run(
|
||||||
() => AplicarCorrecaoRTK_Ntrip(
|
() => AplicarCorrecaoRTK_Ntrip(
|
||||||
_ntripCts.Token
|
_ntripCts.Token
|
||||||
|
|
@ -2308,7 +2085,7 @@ namespace AgroBase.Services
|
||||||
MountpointManual = NtripMountpoint,
|
MountpointManual = NtripMountpoint,
|
||||||
Username = NtripUsername,
|
Username = NtripUsername,
|
||||||
Password = NtripPassword,
|
Password = NtripPassword,
|
||||||
UsarNtripV2 = false,
|
UsarNtripV2 = true,
|
||||||
Comunitario = false,
|
Comunitario = false,
|
||||||
ExigeUsuario = true,
|
ExigeUsuario = true,
|
||||||
ExigeSenha = true,
|
ExigeSenha = true,
|
||||||
|
|
@ -2324,7 +2101,7 @@ namespace AgroBase.Services
|
||||||
MountpointManual = NtripRtk2goMountpoint,
|
MountpointManual = NtripRtk2goMountpoint,
|
||||||
Username = NtripRtk2goUsername,
|
Username = NtripRtk2goUsername,
|
||||||
Password = NtripRtk2goPassword,
|
Password = NtripRtk2goPassword,
|
||||||
UsarNtripV2 = false,
|
UsarNtripV2 = true,
|
||||||
Comunitario = true,
|
Comunitario = true,
|
||||||
ExigeUsuario = true,
|
ExigeUsuario = true,
|
||||||
ExigeSenha = false,
|
ExigeSenha = false,
|
||||||
|
|
@ -2436,26 +2213,6 @@ namespace AgroBase.Services
|
||||||
!cancellationToken.IsCancellationRequested &&
|
!cancellationToken.IsCancellationRequested &&
|
||||||
CorrecaoRTK_Ntrip)
|
CorrecaoRTK_Ntrip)
|
||||||
{
|
{
|
||||||
/*
|
|
||||||
* O worker NTRIP pode permanecer vivo em espera. A decisão
|
|
||||||
* de sair da espera usa a janela completa de ausência local,
|
|
||||||
* e não apenas o limiar curto de "recente". Assim, depois
|
|
||||||
* de um failback, perder 1 ou 2 pacotes MQTT não reacende
|
|
||||||
* NTRIP antes dos 6 s configurados.
|
|
||||||
*/
|
|
||||||
if (!DeveAtivarNtripComoFallback())
|
|
||||||
{
|
|
||||||
Ntrip_Conectado = false;
|
|
||||||
LimparNtripAtivo();
|
|
||||||
|
|
||||||
await Task.Delay(
|
|
||||||
TimeSpan.FromSeconds(1),
|
|
||||||
cancellationToken
|
|
||||||
).ConfigureAwait(false);
|
|
||||||
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Diferente da versão antiga, não encerramos o worker
|
* Diferente da versão antiga, não encerramos o worker
|
||||||
* apenas porque a internet caiu momentaneamente.
|
* apenas porque a internet caiu momentaneamente.
|
||||||
|
|
@ -2499,8 +2256,7 @@ namespace AgroBase.Services
|
||||||
{
|
{
|
||||||
if (
|
if (
|
||||||
cancellationToken.IsCancellationRequested ||
|
cancellationToken.IsCancellationRequested ||
|
||||||
!CorrecaoRTK_Ntrip ||
|
!CorrecaoRTK_Ntrip)
|
||||||
RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -2553,8 +2309,7 @@ namespace AgroBase.Services
|
||||||
{
|
{
|
||||||
if (
|
if (
|
||||||
cancellationToken.IsCancellationRequested ||
|
cancellationToken.IsCancellationRequested ||
|
||||||
!CorrecaoRTK_Ntrip ||
|
!CorrecaoRTK_Ntrip)
|
||||||
RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -2620,14 +2375,9 @@ namespace AgroBase.Services
|
||||||
cancellationToken
|
cancellationToken
|
||||||
).ConfigureAwait(false);
|
).ConfigureAwait(false);
|
||||||
|
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Retorno normal também pode ocorrer quando a
|
* Retorno normal só ocorre quando a sessão foi
|
||||||
* base local reaparece e reassume a preferência.
|
* encerrada por cancelamento/desativação.
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
catch (OperationCanceledException)
|
catch (OperationCanceledException)
|
||||||
|
|
@ -2677,8 +2427,7 @@ namespace AgroBase.Services
|
||||||
if (
|
if (
|
||||||
!Ntrip_Conectado &&
|
!Ntrip_Conectado &&
|
||||||
!cancellationToken.IsCancellationRequested &&
|
!cancellationToken.IsCancellationRequested &&
|
||||||
CorrecaoRTK_Ntrip &&
|
CorrecaoRTK_Ntrip)
|
||||||
!RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
{
|
||||||
Variaveis.MostrarLog(
|
Variaveis.MostrarLog(
|
||||||
"[GPSService.NTRIP][FALLBACK] " +
|
"[GPSService.NTRIP][FALLBACK] " +
|
||||||
|
|
@ -2696,11 +2445,6 @@ namespace AgroBase.Services
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
Interlocked.Increment(
|
Interlocked.Increment(
|
||||||
ref _ntripReconnects
|
ref _ntripReconnects
|
||||||
);
|
);
|
||||||
|
|
@ -4138,13 +3882,6 @@ namespace AgroBase.Services
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
// A base local pode ter voltado enquanto o handshake HTTP
|
|
||||||
// estava em andamento. Nesse caso nem chegamos a assumir NTRIP.
|
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Antes de assumir a nova referência NTRIP, removemos
|
* Antes de assumir a nova referência NTRIP, removemos
|
||||||
* correções ainda pendentes da fonte anterior. Isso evita
|
* correções ainda pendentes da fonte anterior. Isso evita
|
||||||
|
|
@ -4229,15 +3966,6 @@ namespace AgroBase.Services
|
||||||
!cancellationToken.IsCancellationRequested &&
|
!cancellationToken.IsCancellationRequested &&
|
||||||
CorrecaoRTK_Ntrip)
|
CorrecaoRTK_Ntrip)
|
||||||
{
|
{
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
Variaveis.MostrarLog(
|
|
||||||
"[GPSService.RTCM][FAILBACK] Base local MQTT " +
|
|
||||||
"reassumiu; encerrando sessão NTRIP ativa."
|
|
||||||
);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
int bytesRead;
|
int bytesRead;
|
||||||
|
|
||||||
if (!recebeuDados)
|
if (!recebeuDados)
|
||||||
|
|
@ -4297,13 +4025,6 @@ namespace AgroBase.Services
|
||||||
bytesRead
|
bytesRead
|
||||||
);
|
);
|
||||||
|
|
||||||
// Pode ter ocorrido failback enquanto aguardávamos a rede.
|
|
||||||
// Não deixa um frame NTRIP tardio entrar depois da base local.
|
|
||||||
if (RtcmLocalEstavelParaPreferencia())
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Preserva a arquitetura do rover:
|
* Preserva a arquitetura do rover:
|
||||||
* rede nunca escreve diretamente no UM982.
|
* rede nunca escreve diretamente no UM982.
|
||||||
|
|
@ -4936,10 +4657,7 @@ namespace AgroBase.Services
|
||||||
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
||||||
UltimaLeitura.Inicializado = Iniciado;
|
UltimaLeitura.Inicializado = Iniciado;
|
||||||
|
|
||||||
// NTRIP é contingência: só tenta iniciar quando a base local MQTT
|
if (CorrecaoRTK_Ntrip && GetAgeMs(Interlocked.Read(ref _lastRtcmWrittenMono)) > TempoMin_Ntrip * 1000.0)
|
||||||
// está ausente além da janela configurada. A presença de internet,
|
|
||||||
// por si só, nunca mais rouba prioridade da base local.
|
|
||||||
if (DeveAtivarNtripComoFallback())
|
|
||||||
{
|
{
|
||||||
_ = IniciarNtripAsync();
|
_ = IniciarNtripAsync();
|
||||||
}
|
}
|
||||||
|
|
@ -5477,15 +5195,8 @@ namespace AgroBase.Services
|
||||||
("rtcm.queue_depth", metrics.RtcmQueueDepth),
|
("rtcm.queue_depth", metrics.RtcmQueueDepth),
|
||||||
("rtcm.last_rx_age_ms", metrics.LastRtcmReceivedAgeMs),
|
("rtcm.last_rx_age_ms", metrics.LastRtcmReceivedAgeMs),
|
||||||
("rtcm.last_write_age_ms", metrics.LastRtcmWrittenAgeMs),
|
("rtcm.last_write_age_ms", metrics.LastRtcmWrittenAgeMs),
|
||||||
("rtcm.local.last_rx_age_ms", metrics.LastMqttRtcmReceivedAgeMs),
|
|
||||||
("rtcm.local.recente", metrics.RtcmLocalRecente),
|
|
||||||
("rtcm.local.estavel", metrics.RtcmLocalEstavel),
|
|
||||||
("rtcm.fonte_ativa", metrics.RtcmFonteAtiva),
|
|
||||||
("rtcm.drop_stale", metrics.RtcmDroppedStale),
|
("rtcm.drop_stale", metrics.RtcmDroppedStale),
|
||||||
("rtcm.write_errors", metrics.RtcmWriteErrors),
|
("rtcm.write_errors", metrics.RtcmWriteErrors)
|
||||||
("ntrip.connected", metrics.NtripConnected),
|
|
||||||
("ntrip.provider", metrics.NtripProvider ?? string.Empty),
|
|
||||||
("ntrip.mountpoint", metrics.NtripMountpoint ?? string.Empty)
|
|
||||||
);
|
);
|
||||||
|
|
||||||
CorrecaoRTK_Ntrip = RedisService.GetField<bool>(RedisService.ModKey(T_Code.Gps), "correcao_ntrip", false);
|
CorrecaoRTK_Ntrip = RedisService.GetField<bool>(RedisService.ModKey(T_Code.Gps), "correcao_ntrip", false);
|
||||||
|
|
@ -5912,24 +5623,6 @@ namespace AgroBase.Services
|
||||||
)
|
)
|
||||||
),
|
),
|
||||||
|
|
||||||
LastMqttRtcmReceivedAgeMs =
|
|
||||||
GetAgeMs(
|
|
||||||
Interlocked.Read(
|
|
||||||
ref _lastMqttRtcmReceivedMono
|
|
||||||
)
|
|
||||||
),
|
|
||||||
|
|
||||||
LastNtripRtcmReceivedAgeMs =
|
|
||||||
GetAgeMs(
|
|
||||||
Interlocked.Read(
|
|
||||||
ref _lastNtripRtcmReceivedMono
|
|
||||||
)
|
|
||||||
),
|
|
||||||
|
|
||||||
RtcmLocalRecente = RtcmLocalRecente(),
|
|
||||||
RtcmLocalEstavel = RtcmLocalEstavelParaPreferencia(),
|
|
||||||
RtcmFonteAtiva = ObterFonteRtcmAtiva(),
|
|
||||||
|
|
||||||
ConfigurationRuns =
|
ConfigurationRuns =
|
||||||
Interlocked.Read(
|
Interlocked.Read(
|
||||||
ref _configurationRuns
|
ref _configurationRuns
|
||||||
|
|
@ -6291,11 +5984,6 @@ namespace AgroBase.Services
|
||||||
public int RtcmQueueDepth { get; set; }
|
public int RtcmQueueDepth { get; set; }
|
||||||
public double LastRtcmReceivedAgeMs { get; set; }
|
public double LastRtcmReceivedAgeMs { get; set; }
|
||||||
public double LastRtcmWrittenAgeMs { get; set; }
|
public double LastRtcmWrittenAgeMs { get; set; }
|
||||||
public double LastMqttRtcmReceivedAgeMs { get; set; }
|
|
||||||
public double LastNtripRtcmReceivedAgeMs { get; set; }
|
|
||||||
public bool RtcmLocalRecente { get; set; }
|
|
||||||
public bool RtcmLocalEstavel { get; set; }
|
|
||||||
public string RtcmFonteAtiva { get; set; }
|
|
||||||
|
|
||||||
public long ConfigurationRuns { get; set; }
|
public long ConfigurationRuns { get; set; }
|
||||||
public long ConfigurationErrors { get; set; }
|
public long ConfigurationErrors { get; set; }
|
||||||
|
|
|
||||||
|
|
@ -210,10 +210,10 @@ namespace AgroBase.Services.Operadores
|
||||||
switch (id)
|
switch (id)
|
||||||
{
|
{
|
||||||
case "EF":
|
case "EF":
|
||||||
dianteiros.Add(leitura);
|
dianteiros.Add(-leitura);
|
||||||
break;
|
break;
|
||||||
case "DF":
|
case "DF":
|
||||||
dianteiros.Add(-leitura);
|
dianteiros.Add(leitura);
|
||||||
break;
|
break;
|
||||||
case "ET":
|
case "ET":
|
||||||
traseiros.Add(-leitura);
|
traseiros.Add(-leitura);
|
||||||
|
|
|
||||||
|
|
@ -450,34 +450,6 @@ class ControladorMPC:
|
||||||
1.0, min_value=0.25, max_value=2.0,
|
1.0, min_value=0.25, max_value=2.0,
|
||||||
)
|
)
|
||||||
|
|
||||||
# V23 - Pure Pursuit separado para SaindoRua.
|
|
||||||
#
|
|
||||||
# A ferradura em Manobrando ja foi validada com o look-ahead curto acima
|
|
||||||
# e deve manter a janela de projecao curta que evita atalhos entre os
|
|
||||||
# dois ramos do U. SaindoRua e outro problema: ainda estamos no trecho
|
|
||||||
# reto de aproximacao e queremos enxergar mais longe para evitar que
|
|
||||||
# pequenas mudancas de projecao façam o alvo cruzar de um lado para o
|
|
||||||
# outro do rover. Por isso ele recebe parametros proprios e uma projecao
|
|
||||||
# normal de transicao, sem herdar a janela curta de Manobrando.
|
|
||||||
self._dubins_saida_pp_lookahead_base_m = _safe_float(
|
|
||||||
parametros_mpc.get("dubins_saida_pp_lookahead_base_m", 1.30),
|
|
||||||
1.30, min_value=0.60, max_value=3.0,
|
|
||||||
)
|
|
||||||
self._dubins_saida_pp_lookahead_velocidade_s = _safe_float(
|
|
||||||
parametros_mpc.get("dubins_saida_pp_lookahead_velocidade_s", 0.35),
|
|
||||||
0.35, min_value=0.0, max_value=2.0,
|
|
||||||
)
|
|
||||||
self._dubins_saida_pp_lookahead_min_m = _safe_float(
|
|
||||||
parametros_mpc.get("dubins_saida_pp_lookahead_min_m", 1.20),
|
|
||||||
1.20, min_value=0.40, max_value=3.0,
|
|
||||||
)
|
|
||||||
self._dubins_saida_pp_lookahead_max_m = _safe_float(
|
|
||||||
parametros_mpc.get("dubins_saida_pp_lookahead_max_m", 1.80),
|
|
||||||
1.80, min_value=0.60, max_value=4.0,
|
|
||||||
)
|
|
||||||
if self._dubins_saida_pp_lookahead_max_m < self._dubins_saida_pp_lookahead_min_m:
|
|
||||||
self._dubins_saida_pp_lookahead_max_m = self._dubins_saida_pp_lookahead_min_m
|
|
||||||
|
|
||||||
# V22 - amortecimento exclusivo do handoff antecipado em SaindoRua.
|
# V22 - amortecimento exclusivo do handoff antecipado em SaindoRua.
|
||||||
#
|
#
|
||||||
# Nesse estado o rover ja opera a 15% e ainda percorre o trecho reto
|
# Nesse estado o rover ja opera a 15% e ainda percorre o trecho reto
|
||||||
|
|
@ -1448,16 +1420,8 @@ class ControladorMPC:
|
||||||
dh = float(self._wrap_pi(h1 - h0))
|
dh = float(self._wrap_pi(h1 - h0))
|
||||||
return float(dh / max(s1 - s0, 1e-6))
|
return float(dh / max(s1 - s0, 1e-6))
|
||||||
|
|
||||||
def _referencia_seguidor_dubins(
|
def _referencia_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto):
|
||||||
self, x, y, theta, idx_base, velocidade, contexto, status_carro
|
"""V12: Pure Pursuit deterministico sobre a geometria Dubins.
|
||||||
):
|
|
||||||
"""V23: Pure Pursuit 4WS com perfis separados por fase.
|
|
||||||
|
|
||||||
- Manobrando preserva integralmente o PP curto validado na ferradura.
|
|
||||||
- SaindoRua usa look-ahead maior e projecao de transicao, apropriados
|
|
||||||
ao trecho reto que antecede a curva.
|
|
||||||
|
|
||||||
A equacao de curvatura e o ganho continuam compartilhados.
|
|
||||||
|
|
||||||
A V11 usava tangente/heading local + curvatura estimada. Em uma
|
A V11 usava tangente/heading local + curvatura estimada. Em uma
|
||||||
ferradura isso podia aliviar o estercoamento simplesmente porque o
|
ferradura isso podia aliviar o estercoamento simplesmente porque o
|
||||||
|
|
@ -1481,42 +1445,21 @@ class ControladorMPC:
|
||||||
Nao ha arvore MPC, nem promessa de "virar depois": o valor calculado
|
Nao ha arvore MPC, nem promessa de "virar depois": o valor calculado
|
||||||
e o comando executado neste ciclo.
|
e o comando executado neste ciclo.
|
||||||
"""
|
"""
|
||||||
pp_saida = _status_in(status_carro, [StatusCarroMapa.SaindoRua])
|
|
||||||
status_projecao = (
|
|
||||||
StatusCarroMapa.SaindoRua.value
|
|
||||||
if pp_saida
|
|
||||||
else StatusCarroMapa.Manobrando.value
|
|
||||||
)
|
|
||||||
|
|
||||||
e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_para_controle(
|
e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_para_controle(
|
||||||
x, y, idx_base, status_projecao
|
x, y, idx_base, StatusCarroMapa.Manobrando.value
|
||||||
)
|
)
|
||||||
|
|
||||||
s_max = float(self._s_nodes[-1]) if self._s_nodes is not None and len(self._s_nodes) else 0.0
|
s_max = float(self._s_nodes[-1]) if self._s_nodes is not None and len(self._s_nodes) else 0.0
|
||||||
v = max(0.0, float(velocidade))
|
v = max(0.0, float(velocidade))
|
||||||
|
look = (
|
||||||
if pp_saida:
|
float(self._dubins_pp_lookahead_base_m)
|
||||||
look = (
|
+ float(self._dubins_pp_lookahead_velocidade_s) * v
|
||||||
float(self._dubins_saida_pp_lookahead_base_m)
|
)
|
||||||
+ float(self._dubins_saida_pp_lookahead_velocidade_s) * v
|
look = float(np.clip(
|
||||||
)
|
look,
|
||||||
look = float(np.clip(
|
float(self._dubins_pp_lookahead_min_m),
|
||||||
look,
|
float(self._dubins_pp_lookahead_max_m),
|
||||||
float(self._dubins_saida_pp_lookahead_min_m),
|
))
|
||||||
float(self._dubins_saida_pp_lookahead_max_m),
|
|
||||||
))
|
|
||||||
perfil_pp = "saindo_rua"
|
|
||||||
else:
|
|
||||||
look = (
|
|
||||||
float(self._dubins_pp_lookahead_base_m)
|
|
||||||
+ float(self._dubins_pp_lookahead_velocidade_s) * v
|
|
||||||
)
|
|
||||||
look = float(np.clip(
|
|
||||||
look,
|
|
||||||
float(self._dubins_pp_lookahead_min_m),
|
|
||||||
float(self._dubins_pp_lookahead_max_m),
|
|
||||||
))
|
|
||||||
perfil_pp = "manobrando"
|
|
||||||
|
|
||||||
s_alvo = min(s_max, float(s_proj) + look)
|
s_alvo = min(s_max, float(s_proj) + look)
|
||||||
xy_ref, i_ref = self._xy_na_abscissa(s_alvo)
|
xy_ref, i_ref = self._xy_na_abscissa(s_alvo)
|
||||||
|
|
@ -1574,14 +1517,10 @@ class ControladorMPC:
|
||||||
"xy_ref": (float(xy_ref[0]), float(xy_ref[1])),
|
"xy_ref": (float(xy_ref[0]), float(xy_ref[1])),
|
||||||
"s_ref": float(s_alvo),
|
"s_ref": float(s_alvo),
|
||||||
"margem": float(margem),
|
"margem": float(margem),
|
||||||
"perfil_pp": perfil_pp,
|
|
||||||
"status_projecao": int(status_projecao),
|
|
||||||
}
|
}
|
||||||
|
|
||||||
def _simular_preview_seguidor_dubins(
|
def _simular_preview_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl):
|
||||||
self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl, status_carro
|
"""Preview leve usando exatamente o mesmo Pure Pursuit da V12."""
|
||||||
):
|
|
||||||
"""Preview leve usando o mesmo perfil PP da fase real atual."""
|
|
||||||
v = max(float(velocidade), float(getattr(self, "_vmin_planejamento", 0.30)))
|
v = max(float(velocidade), float(getattr(self, "_vmin_planejamento", 0.30)))
|
||||||
dt = max(0.05, min(float(dt_ctrl), 0.50))
|
dt = max(0.05, min(float(dt_ctrl), 0.50))
|
||||||
dist_passo = max(0.05, v * dt)
|
dist_passo = max(0.05, v * dt)
|
||||||
|
|
@ -1599,7 +1538,7 @@ class ControladorMPC:
|
||||||
|
|
||||||
for _ in range(passos):
|
for _ in range(passos):
|
||||||
ang, dbg = self._referencia_seguidor_dubins(
|
ang, dbg = self._referencia_seguidor_dubins(
|
||||||
xs, ys, ths, idx_sim, v, contexto, status_carro
|
xs, ys, ths, idx_sim, v, contexto
|
||||||
)
|
)
|
||||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
||||||
TipoMovimentoDirecional.MovimentoArco, ang
|
TipoMovimentoDirecional.MovimentoArco, ang
|
||||||
|
|
@ -1739,7 +1678,7 @@ class ControladorMPC:
|
||||||
idx_alvo_correcao, pos_lat, pos_lon, pos_theta, pos_latencia, ponto_alvo_real
|
idx_alvo_correcao, pos_lat, pos_lon, pos_theta, pos_latencia, ponto_alvo_real
|
||||||
):
|
):
|
||||||
angulo_bruto_rad, dbg = self._referencia_seguidor_dubins(
|
angulo_bruto_rad, dbg = self._referencia_seguidor_dubins(
|
||||||
x, y, theta, idx_alvo_correcao, velocidade, contexto, status_carro
|
x, y, theta, idx_alvo_correcao, velocidade, contexto
|
||||||
)
|
)
|
||||||
|
|
||||||
pp_antecipado = _status_in(
|
pp_antecipado = _status_in(
|
||||||
|
|
@ -1757,7 +1696,7 @@ class ControladorMPC:
|
||||||
|
|
||||||
preview_xy = self._simular_preview_seguidor_dubins(
|
preview_xy = self._simular_preview_seguidor_dubins(
|
||||||
x, y, theta, idx_alvo_correcao, velocidade, contexto,
|
x, y, theta, idx_alvo_correcao, velocidade, contexto,
|
||||||
self.tempo_execucao_local, status_carro,
|
self.tempo_execucao_local,
|
||||||
)
|
)
|
||||||
simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(preview_xy) or [])
|
simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(preview_xy) or [])
|
||||||
simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta))
|
simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta))
|
||||||
|
|
@ -1795,12 +1734,10 @@ class ControladorMPC:
|
||||||
"debug_custo": {},
|
"debug_custo": {},
|
||||||
"debug_seguidor_dubins": {
|
"debug_seguidor_dubins": {
|
||||||
"modo": (
|
"modo": (
|
||||||
"pure_pursuit_4ws_saindo_rua"
|
"pure_pursuit_4ws_antecipado"
|
||||||
if pp_antecipado
|
if pp_antecipado
|
||||||
else "pure_pursuit_4ws_manobrando"
|
else "pure_pursuit_4ws"
|
||||||
),
|
),
|
||||||
"perfil_pp": dbg.get("perfil_pp", "desconhecido"),
|
|
||||||
"status_projecao": dbg.get("status_projecao"),
|
|
||||||
"lookahead_m": float(dbg["lookahead_m"]),
|
"lookahead_m": float(dbg["lookahead_m"]),
|
||||||
"distancia_alvo_m": float(dbg["distancia_alvo_m"]),
|
"distancia_alvo_m": float(dbg["distancia_alvo_m"]),
|
||||||
"alpha_graus": float(math.degrees(dbg["alpha"])),
|
"alpha_graus": float(math.degrees(dbg["alpha"])),
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue