Compare commits
2 Commits
876b24c5fc
...
2fe3fa4f0e
| Author | SHA1 | Date |
|---|---|---|
|
|
2fe3fa4f0e | |
|
|
72211b06d3 |
|
|
@ -136,13 +136,37 @@ namespace AgroBase.Forms.IHM.Operacao
|
||||||
chkRegistrarPosProcessamento.FlatAppearance.BorderColor = ativo ? HmiTheme.Success : HmiTheme.Border;
|
chkRegistrarPosProcessamento.FlatAppearance.BorderColor = ativo ? HmiTheme.Success : HmiTheme.Border;
|
||||||
}
|
}
|
||||||
|
|
||||||
private void DefinirOperacaoParametrizando(OperacaoModel operacao, bool recarregarMapa = true)
|
private void DefinirOperacaoParametrizando(
|
||||||
|
OperacaoModel operacao,
|
||||||
|
bool recarregarMapa = true,
|
||||||
|
bool registrarComoFonteAutoritativa = true)
|
||||||
{
|
{
|
||||||
operacaoParametrizando = operacao;
|
operacaoParametrizando = operacao;
|
||||||
|
|
||||||
_operacaoFonteCarregada = operacao;
|
/*
|
||||||
_parametrosFonteCarregados = operacao?.Parametros;
|
* IMPORTANTE:
|
||||||
_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);
|
||||||
|
|
@ -242,7 +266,13 @@ namespace AgroBase.Forms.IHM.Operacao
|
||||||
novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados;
|
novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados;
|
||||||
}
|
}
|
||||||
|
|
||||||
DefinirOperacaoParametrizando(novaOperacao, recarregarMapa: false);
|
// É um rascunho local. Não substitui os marcadores da última
|
||||||
|
// 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 = 8.0;
|
private const double FolgaMaximaReferencia = 9.5;
|
||||||
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 = true,
|
MovAuxilioSonar = false,
|
||||||
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 = true,
|
MovAuxilioSonar = false,
|
||||||
MpcHorizonte = 4.0,
|
MpcHorizonte = 4.0,
|
||||||
AnteciparManobraCorredorM = 0,
|
AnteciparManobraCorredorM = 0,
|
||||||
|
|
||||||
|
|
@ -1308,6 +1308,19 @@ 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)
|
||||||
|
|
@ -1346,7 +1359,34 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao();
|
novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao();
|
||||||
|
|
||||||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(novaOp);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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);
|
||||||
|
|
||||||
|
|
@ -1400,7 +1440,15 @@ namespace AgroBase.Models
|
||||||
{ T_Code.Atu, new Dictionary<string, bool>() { } },
|
{ T_Code.Atu, new Dictionary<string, bool>() { } },
|
||||||
};
|
};
|
||||||
|
|
||||||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(op);
|
||||||
|
|
||||||
|
RedisService.AtualizarCampos(
|
||||||
|
CtxKey.DadosOperacao,
|
||||||
|
("configurado", !exigeMapaCritico),
|
||||||
|
("mapa_barreira_critica", false),
|
||||||
|
("mapa_confirmado", false)
|
||||||
|
);
|
||||||
|
|
||||||
WeedWorkerService.ReiniciarLeituraAnalise();
|
WeedWorkerService.ReiniciarLeituraAnalise();
|
||||||
VisualWorkerService.ReiniciarLeituraAnalise();
|
VisualWorkerService.ReiniciarLeituraAnalise();
|
||||||
|
|
||||||
|
|
@ -1416,8 +1464,13 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
op.GPSTrajetoria = new List<GPSModel>();
|
op.GPSTrajetoria = new List<GPSModel>();
|
||||||
|
|
||||||
op.Trajetoria?.ProjetarTrajetoriaFixa();
|
if (exigeMapaCritico)
|
||||||
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;
|
||||||
|
|
@ -1428,20 +1481,47 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
op.IniciarTimerLogsOperacao();
|
op.IniciarTimerLogsOperacao();
|
||||||
|
|
||||||
// Barreira crítica: a operação só pode seguir para a fase de
|
if (exigeMapaCritico)
|
||||||
// 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)
|
|
||||||
{
|
{
|
||||||
Variaveis.MostrarLog(
|
// Barreira crítica somente para modos que realmente dependem
|
||||||
"[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada."
|
// 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(
|
||||||
|
"[OperacaoModel.IniciarOperacao] " +
|
||||||
|
"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; } = 2.65;
|
public static double KuDirecional { get; } = 0.35; // 2.65
|
||||||
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0;
|
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0; // °/s
|
||||||
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,9 +58,24 @@ 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>
|
||||||
|
|
@ -369,6 +384,15 @@ 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;
|
||||||
|
|
@ -574,6 +598,16 @@ 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)
|
||||||
{
|
{
|
||||||
|
|
@ -1785,25 +1819,188 @@ 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 ||
|
||||||
* Uma solução RTK não pode receber correções simultâneas de duas
|
bytesRead <= 0 ||
|
||||||
* bases/referências diferentes. Se NTRIP estiver efetivamente
|
bytesRead > correcao.Length ||
|
||||||
* conectado, ele é a fonte ativa e RTCM via MQTT é ignorado.
|
bytesRead > RtcmMaxPayloadBytes)
|
||||||
*
|
|
||||||
* 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);
|
||||||
|
|
||||||
|
|
@ -1814,8 +2011,25 @@ 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);
|
||||||
|
|
||||||
|
|
@ -1985,8 +2199,12 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
public static async Task IniciarNtripAsync()
|
public static async Task IniciarNtripAsync()
|
||||||
{
|
{
|
||||||
if (!Variaveis.IsAgroBase || !CorrecaoRTK_Ntrip)
|
if (!Variaveis.IsAgroBase ||
|
||||||
|
!CorrecaoRTK_Ntrip ||
|
||||||
|
!DeveAtivarNtripComoFallback())
|
||||||
|
{
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
lock (_workerLock)
|
lock (_workerLock)
|
||||||
{
|
{
|
||||||
|
|
@ -1998,6 +2216,11 @@ 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
|
||||||
|
|
@ -2085,7 +2308,7 @@ namespace AgroBase.Services
|
||||||
MountpointManual = NtripMountpoint,
|
MountpointManual = NtripMountpoint,
|
||||||
Username = NtripUsername,
|
Username = NtripUsername,
|
||||||
Password = NtripPassword,
|
Password = NtripPassword,
|
||||||
UsarNtripV2 = true,
|
UsarNtripV2 = false,
|
||||||
Comunitario = false,
|
Comunitario = false,
|
||||||
ExigeUsuario = true,
|
ExigeUsuario = true,
|
||||||
ExigeSenha = true,
|
ExigeSenha = true,
|
||||||
|
|
@ -2101,7 +2324,7 @@ namespace AgroBase.Services
|
||||||
MountpointManual = NtripRtk2goMountpoint,
|
MountpointManual = NtripRtk2goMountpoint,
|
||||||
Username = NtripRtk2goUsername,
|
Username = NtripRtk2goUsername,
|
||||||
Password = NtripRtk2goPassword,
|
Password = NtripRtk2goPassword,
|
||||||
UsarNtripV2 = true,
|
UsarNtripV2 = false,
|
||||||
Comunitario = true,
|
Comunitario = true,
|
||||||
ExigeUsuario = true,
|
ExigeUsuario = true,
|
||||||
ExigeSenha = false,
|
ExigeSenha = false,
|
||||||
|
|
@ -2213,6 +2436,26 @@ 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.
|
||||||
|
|
@ -2256,7 +2499,8 @@ namespace AgroBase.Services
|
||||||
{
|
{
|
||||||
if (
|
if (
|
||||||
cancellationToken.IsCancellationRequested ||
|
cancellationToken.IsCancellationRequested ||
|
||||||
!CorrecaoRTK_Ntrip)
|
!CorrecaoRTK_Ntrip ||
|
||||||
|
RtcmLocalEstavelParaPreferencia())
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -2309,7 +2553,8 @@ namespace AgroBase.Services
|
||||||
{
|
{
|
||||||
if (
|
if (
|
||||||
cancellationToken.IsCancellationRequested ||
|
cancellationToken.IsCancellationRequested ||
|
||||||
!CorrecaoRTK_Ntrip)
|
!CorrecaoRTK_Ntrip ||
|
||||||
|
RtcmLocalEstavelParaPreferencia())
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -2375,9 +2620,14 @@ namespace AgroBase.Services
|
||||||
cancellationToken
|
cancellationToken
|
||||||
).ConfigureAwait(false);
|
).ConfigureAwait(false);
|
||||||
|
|
||||||
|
if (RtcmLocalEstavelParaPreferencia())
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Retorno normal só ocorre quando a sessão foi
|
* Retorno normal também pode ocorrer quando a
|
||||||
* encerrada por cancelamento/desativação.
|
* base local reaparece e reassume a preferência.
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
catch (OperationCanceledException)
|
catch (OperationCanceledException)
|
||||||
|
|
@ -2427,7 +2677,8 @@ 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] " +
|
||||||
|
|
@ -2445,6 +2696,11 @@ namespace AgroBase.Services
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (RtcmLocalEstavelParaPreferencia())
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
Interlocked.Increment(
|
Interlocked.Increment(
|
||||||
ref _ntripReconnects
|
ref _ntripReconnects
|
||||||
);
|
);
|
||||||
|
|
@ -3882,6 +4138,13 @@ 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
|
||||||
|
|
@ -3966,6 +4229,15 @@ 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)
|
||||||
|
|
@ -4025,6 +4297,13 @@ 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.
|
||||||
|
|
@ -4657,7 +4936,10 @@ namespace AgroBase.Services
|
||||||
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
||||||
UltimaLeitura.Inicializado = Iniciado;
|
UltimaLeitura.Inicializado = Iniciado;
|
||||||
|
|
||||||
if (CorrecaoRTK_Ntrip && GetAgeMs(Interlocked.Read(ref _lastRtcmWrittenMono)) > TempoMin_Ntrip * 1000.0)
|
// NTRIP é contingência: só tenta iniciar quando a base local MQTT
|
||||||
|
// 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();
|
||||||
}
|
}
|
||||||
|
|
@ -5195,8 +5477,15 @@ 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);
|
||||||
|
|
@ -5623,6 +5912,24 @@ 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
|
||||||
|
|
@ -5984,6 +6291,11 @@ 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,6 +450,34 @@ 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
|
||||||
|
|
@ -1420,8 +1448,16 @@ 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(self, x, y, theta, idx_base, velocidade, contexto):
|
def _referencia_seguidor_dubins(
|
||||||
"""V12: Pure Pursuit deterministico sobre a geometria Dubins.
|
self, x, y, theta, idx_base, velocidade, contexto, status_carro
|
||||||
|
):
|
||||||
|
"""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
|
||||||
|
|
@ -1445,21 +1481,42 @@ 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, StatusCarroMapa.Manobrando.value
|
x, y, idx_base, status_projecao
|
||||||
)
|
)
|
||||||
|
|
||||||
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 = (
|
|
||||||
float(self._dubins_pp_lookahead_base_m)
|
if pp_saida:
|
||||||
+ float(self._dubins_pp_lookahead_velocidade_s) * v
|
look = (
|
||||||
)
|
float(self._dubins_saida_pp_lookahead_base_m)
|
||||||
look = float(np.clip(
|
+ float(self._dubins_saida_pp_lookahead_velocidade_s) * v
|
||||||
look,
|
)
|
||||||
float(self._dubins_pp_lookahead_min_m),
|
look = float(np.clip(
|
||||||
float(self._dubins_pp_lookahead_max_m),
|
look,
|
||||||
))
|
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)
|
||||||
|
|
@ -1517,10 +1574,14 @@ 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(self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl):
|
def _simular_preview_seguidor_dubins(
|
||||||
"""Preview leve usando exatamente o mesmo Pure Pursuit da V12."""
|
self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl, status_carro
|
||||||
|
):
|
||||||
|
"""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)
|
||||||
|
|
@ -1538,7 +1599,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
|
xs, ys, ths, idx_sim, v, contexto, status_carro
|
||||||
)
|
)
|
||||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
||||||
TipoMovimentoDirecional.MovimentoArco, ang
|
TipoMovimentoDirecional.MovimentoArco, ang
|
||||||
|
|
@ -1678,7 +1739,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
|
x, y, theta, idx_alvo_correcao, velocidade, contexto, status_carro
|
||||||
)
|
)
|
||||||
|
|
||||||
pp_antecipado = _status_in(
|
pp_antecipado = _status_in(
|
||||||
|
|
@ -1696,7 +1757,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,
|
self.tempo_execucao_local, status_carro,
|
||||||
)
|
)
|
||||||
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))
|
||||||
|
|
@ -1734,10 +1795,12 @@ class ControladorMPC:
|
||||||
"debug_custo": {},
|
"debug_custo": {},
|
||||||
"debug_seguidor_dubins": {
|
"debug_seguidor_dubins": {
|
||||||
"modo": (
|
"modo": (
|
||||||
"pure_pursuit_4ws_antecipado"
|
"pure_pursuit_4ws_saindo_rua"
|
||||||
if pp_antecipado
|
if pp_antecipado
|
||||||
else "pure_pursuit_4ws"
|
else "pure_pursuit_4ws_manobrando"
|
||||||
),
|
),
|
||||||
|
"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