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;
|
||||
}
|
||||
|
||||
private void DefinirOperacaoParametrizando(
|
||||
OperacaoModel operacao,
|
||||
bool recarregarMapa = true,
|
||||
bool registrarComoFonteAutoritativa = true)
|
||||
private void DefinirOperacaoParametrizando(OperacaoModel operacao, bool recarregarMapa = true)
|
||||
{
|
||||
operacaoParametrizando = operacao;
|
||||
|
||||
/*
|
||||
* IMPORTANTE:
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
_operacaoFonteCarregada = operacao;
|
||||
_parametrosFonteCarregados = operacao?.Parametros;
|
||||
_controleFonteCarregado = operacao?.Parametros?.Controle;
|
||||
|
||||
CarregarDadosGerais();
|
||||
_ucMovimento.DefinirOperacao(operacaoParametrizando);
|
||||
|
|
@ -266,13 +242,7 @@ namespace AgroBase.Forms.IHM.Operacao
|
|||
novaOperacao.Parametros.Controle.RegistrarDadosPosProcessamento = registrarDados;
|
||||
}
|
||||
|
||||
// É 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
|
||||
);
|
||||
DefinirOperacaoParametrizando(novaOperacao, recarregarMapa: false);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -530,7 +530,7 @@ namespace AgroBase.Models.Modules
|
|||
*/
|
||||
private const double FolgaMinimaReferencia = 0.05;
|
||||
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 TimeoutSegundaBorda = TimeSpan.FromSeconds(30);
|
||||
private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30);
|
||||
|
|
|
|||
|
|
@ -677,7 +677,7 @@ namespace AgroBase.Models
|
|||
ImuDirecionalPreferirArco = false,
|
||||
OakParadaPorObstaculo = false,
|
||||
DirAuxilioSonar = false,
|
||||
MovAuxilioSonar = false,
|
||||
MovAuxilioSonar = true,
|
||||
MpcHorizonte = 4.0,
|
||||
AnteciparManobraCorredorM = 0.0,
|
||||
|
||||
|
|
@ -1065,7 +1065,7 @@ namespace AgroBase.Models
|
|||
ImuDirecionalPreferirArco = false,
|
||||
OakParadaPorObstaculo = false,
|
||||
DirAuxilioSonar = false,
|
||||
MovAuxilioSonar = false,
|
||||
MovAuxilioSonar = true,
|
||||
MpcHorizonte = 4.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)
|
||||
{
|
||||
if (parametros?.Modo == null)
|
||||
|
|
@ -1359,34 +1346,7 @@ namespace AgroBase.Models
|
|||
|
||||
novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao();
|
||||
|
||||
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."
|
||||
);
|
||||
}
|
||||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||
|
||||
frmInstancial.frmIHM?._frmOperacao?.AtualizarParametrosTela(novaOp);
|
||||
|
||||
|
|
@ -1440,15 +1400,7 @@ namespace AgroBase.Models
|
|||
{ T_Code.Atu, new Dictionary<string, bool>() { } },
|
||||
};
|
||||
|
||||
bool exigeMapaCritico = ExigeSincronizacaoMapaCritico(op);
|
||||
|
||||
RedisService.AtualizarCampos(
|
||||
CtxKey.DadosOperacao,
|
||||
("configurado", !exigeMapaCritico),
|
||||
("mapa_barreira_critica", false),
|
||||
("mapa_confirmado", false)
|
||||
);
|
||||
|
||||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||
WeedWorkerService.ReiniciarLeituraAnalise();
|
||||
VisualWorkerService.ReiniciarLeituraAnalise();
|
||||
|
||||
|
|
@ -1464,13 +1416,8 @@ namespace AgroBase.Models
|
|||
|
||||
op.GPSTrajetoria = new List<GPSModel>();
|
||||
|
||||
if (exigeMapaCritico)
|
||||
{
|
||||
op.Trajetoria?.ProjetarTrajetoriaFixa();
|
||||
|
||||
if (op.Trajetoria != null)
|
||||
op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false;
|
||||
}
|
||||
op.Trajetoria?.ProjetarTrajetoriaFixa();
|
||||
op.Trajetoria.VerificacaoInicialMeioRuaConcluida = false;
|
||||
|
||||
op.Sensoriamento.UltimoRegistroLog = agora;
|
||||
op.idxLog = 0;
|
||||
|
|
@ -1481,47 +1428,20 @@ namespace AgroBase.Models
|
|||
|
||||
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(
|
||||
"[OperacaoModel.IniciarOperacao] " +
|
||||
"Operacao Manual iniciada sem barreira MAPSYNC."
|
||||
"[OperacaoModel.IniciarOperacao] Mapa MPC não confirmado. Operação permanece bloqueada."
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
await op.RealizarCalibragemInicialAsync(true);
|
||||
|
|
|
|||
|
|
@ -1020,8 +1020,8 @@ namespace AgroBase.Models
|
|||
public static double ComprimentoTrasCm { get; } = 40.0; // 107
|
||||
public static double DistanciaEntreEixosCm { get; } = 92.0;
|
||||
public static double BitolaRodasCm { get; } = 75.0;
|
||||
public static double KuDirecional { get; } = 0.35; // 2.65
|
||||
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0; // °/s
|
||||
public static double KuDirecional { get; } = 2.65;
|
||||
public static double VelocidadeAngularDirecionalGrausSegundo { get; } = 50.0;
|
||||
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
|
||||
public static double LeverArmLateralCm { get; set; } = 0.0; // cm
|
||||
public static double LarguraEquipamentoMm
|
||||
|
|
|
|||
|
|
@ -58,24 +58,9 @@ namespace AgroBase.Services
|
|||
|
||||
private static bool InverterHeading = true;
|
||||
private static int rtk_timeout = 60;
|
||||
|
||||
/*
|
||||
* 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 int TempoMin_Ntrip = 10;
|
||||
|
||||
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;
|
||||
|
||||
/// <summary>
|
||||
|
|
@ -384,15 +369,6 @@ namespace AgroBase.Services
|
|||
private static long _lastRtcmReceivedMono;
|
||||
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 _nmeaValid;
|
||||
private static long _nmeaInvalid;
|
||||
|
|
@ -598,16 +574,6 @@ namespace AgroBase.Services
|
|||
try
|
||||
{
|
||||
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)
|
||||
{
|
||||
|
|
@ -1819,188 +1785,25 @@ namespace AgroBase.Services
|
|||
// 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)
|
||||
{
|
||||
if (correcao == null ||
|
||||
bytesRead <= 0 ||
|
||||
bytesRead > correcao.Length ||
|
||||
bytesRead > RtcmMaxPayloadBytes)
|
||||
/*
|
||||
* Uma solução RTK não pode receber correções simultâneas de duas
|
||||
* bases/referências diferentes. Se NTRIP estiver efetivamente
|
||||
* 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);
|
||||
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];
|
||||
Buffer.BlockCopy(correcao, 0, copia, 0, bytesRead);
|
||||
|
||||
|
|
@ -2011,25 +1814,8 @@ namespace AgroBase.Services
|
|||
{
|
||||
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();
|
||||
|
||||
if (source == RtcmSource.Ntrip)
|
||||
{
|
||||
Interlocked.Exchange(
|
||||
ref _lastNtripRtcmReceivedMono,
|
||||
agora
|
||||
);
|
||||
}
|
||||
|
||||
Interlocked.Increment(ref _rtcmReceived);
|
||||
Interlocked.Exchange(ref _lastRtcmReceivedMono, agora);
|
||||
|
||||
|
|
@ -2199,12 +1985,8 @@ namespace AgroBase.Services
|
|||
|
||||
public static async Task IniciarNtripAsync()
|
||||
{
|
||||
if (!Variaveis.IsAgroBase ||
|
||||
!CorrecaoRTK_Ntrip ||
|
||||
!DeveAtivarNtripComoFallback())
|
||||
{
|
||||
if (!Variaveis.IsAgroBase || !CorrecaoRTK_Ntrip)
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_workerLock)
|
||||
{
|
||||
|
|
@ -2216,11 +1998,6 @@ namespace AgroBase.Services
|
|||
|
||||
_ntripCts = new CancellationTokenSource();
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService.RTCM][FAILOVER] RTCM local ausente; " +
|
||||
"ativando NTRIP como contingência."
|
||||
);
|
||||
|
||||
_ntripTask = Task.Run(
|
||||
() => AplicarCorrecaoRTK_Ntrip(
|
||||
_ntripCts.Token
|
||||
|
|
@ -2308,7 +2085,7 @@ namespace AgroBase.Services
|
|||
MountpointManual = NtripMountpoint,
|
||||
Username = NtripUsername,
|
||||
Password = NtripPassword,
|
||||
UsarNtripV2 = false,
|
||||
UsarNtripV2 = true,
|
||||
Comunitario = false,
|
||||
ExigeUsuario = true,
|
||||
ExigeSenha = true,
|
||||
|
|
@ -2324,7 +2101,7 @@ namespace AgroBase.Services
|
|||
MountpointManual = NtripRtk2goMountpoint,
|
||||
Username = NtripRtk2goUsername,
|
||||
Password = NtripRtk2goPassword,
|
||||
UsarNtripV2 = false,
|
||||
UsarNtripV2 = true,
|
||||
Comunitario = true,
|
||||
ExigeUsuario = true,
|
||||
ExigeSenha = false,
|
||||
|
|
@ -2436,26 +2213,6 @@ namespace AgroBase.Services
|
|||
!cancellationToken.IsCancellationRequested &&
|
||||
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
|
||||
* apenas porque a internet caiu momentaneamente.
|
||||
|
|
@ -2499,8 +2256,7 @@ namespace AgroBase.Services
|
|||
{
|
||||
if (
|
||||
cancellationToken.IsCancellationRequested ||
|
||||
!CorrecaoRTK_Ntrip ||
|
||||
RtcmLocalEstavelParaPreferencia())
|
||||
!CorrecaoRTK_Ntrip)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
|
@ -2553,8 +2309,7 @@ namespace AgroBase.Services
|
|||
{
|
||||
if (
|
||||
cancellationToken.IsCancellationRequested ||
|
||||
!CorrecaoRTK_Ntrip ||
|
||||
RtcmLocalEstavelParaPreferencia())
|
||||
!CorrecaoRTK_Ntrip)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
|
@ -2620,14 +2375,9 @@ namespace AgroBase.Services
|
|||
cancellationToken
|
||||
).ConfigureAwait(false);
|
||||
|
||||
if (RtcmLocalEstavelParaPreferencia())
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Retorno normal também pode ocorrer quando a
|
||||
* base local reaparece e reassume a preferência.
|
||||
* Retorno normal só ocorre quando a sessão foi
|
||||
* encerrada por cancelamento/desativação.
|
||||
*/
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
|
|
@ -2677,8 +2427,7 @@ namespace AgroBase.Services
|
|||
if (
|
||||
!Ntrip_Conectado &&
|
||||
!cancellationToken.IsCancellationRequested &&
|
||||
CorrecaoRTK_Ntrip &&
|
||||
!RtcmLocalEstavelParaPreferencia())
|
||||
CorrecaoRTK_Ntrip)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService.NTRIP][FALLBACK] " +
|
||||
|
|
@ -2696,11 +2445,6 @@ namespace AgroBase.Services
|
|||
break;
|
||||
}
|
||||
|
||||
if (RtcmLocalEstavelParaPreferencia())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
Interlocked.Increment(
|
||||
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
|
||||
* correções ainda pendentes da fonte anterior. Isso evita
|
||||
|
|
@ -4229,15 +3966,6 @@ namespace AgroBase.Services
|
|||
!cancellationToken.IsCancellationRequested &&
|
||||
CorrecaoRTK_Ntrip)
|
||||
{
|
||||
if (RtcmLocalEstavelParaPreferencia())
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService.RTCM][FAILBACK] Base local MQTT " +
|
||||
"reassumiu; encerrando sessão NTRIP ativa."
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
int bytesRead;
|
||||
|
||||
if (!recebeuDados)
|
||||
|
|
@ -4297,13 +4025,6 @@ namespace AgroBase.Services
|
|||
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:
|
||||
* rede nunca escreve diretamente no UM982.
|
||||
|
|
@ -4936,10 +4657,7 @@ namespace AgroBase.Services
|
|||
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
|
||||
UltimaLeitura.Inicializado = Iniciado;
|
||||
|
||||
// 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())
|
||||
if (CorrecaoRTK_Ntrip && GetAgeMs(Interlocked.Read(ref _lastRtcmWrittenMono)) > TempoMin_Ntrip * 1000.0)
|
||||
{
|
||||
_ = IniciarNtripAsync();
|
||||
}
|
||||
|
|
@ -5477,15 +5195,8 @@ namespace AgroBase.Services
|
|||
("rtcm.queue_depth", metrics.RtcmQueueDepth),
|
||||
("rtcm.last_rx_age_ms", metrics.LastRtcmReceivedAgeMs),
|
||||
("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.write_errors", metrics.RtcmWriteErrors),
|
||||
("ntrip.connected", metrics.NtripConnected),
|
||||
("ntrip.provider", metrics.NtripProvider ?? string.Empty),
|
||||
("ntrip.mountpoint", metrics.NtripMountpoint ?? string.Empty)
|
||||
("rtcm.write_errors", metrics.RtcmWriteErrors)
|
||||
);
|
||||
|
||||
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 =
|
||||
Interlocked.Read(
|
||||
ref _configurationRuns
|
||||
|
|
@ -6291,11 +5984,6 @@ namespace AgroBase.Services
|
|||
public int RtcmQueueDepth { get; set; }
|
||||
public double LastRtcmReceivedAgeMs { 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 ConfigurationErrors { get; set; }
|
||||
|
|
|
|||
|
|
@ -210,10 +210,10 @@ namespace AgroBase.Services.Operadores
|
|||
switch (id)
|
||||
{
|
||||
case "EF":
|
||||
dianteiros.Add(leitura);
|
||||
dianteiros.Add(-leitura);
|
||||
break;
|
||||
case "DF":
|
||||
dianteiros.Add(-leitura);
|
||||
dianteiros.Add(leitura);
|
||||
break;
|
||||
case "ET":
|
||||
traseiros.Add(-leitura);
|
||||
|
|
|
|||
|
|
@ -450,34 +450,6 @@ class ControladorMPC:
|
|||
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.
|
||||
#
|
||||
# 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))
|
||||
return float(dh / max(s1 - s0, 1e-6))
|
||||
|
||||
def _referencia_seguidor_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.
|
||||
def _referencia_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto):
|
||||
"""V12: Pure Pursuit deterministico sobre a geometria Dubins.
|
||||
|
||||
A V11 usava tangente/heading local + curvatura estimada. Em uma
|
||||
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
|
||||
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(
|
||||
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
|
||||
v = max(0.0, float(velocidade))
|
||||
|
||||
if pp_saida:
|
||||
look = (
|
||||
float(self._dubins_saida_pp_lookahead_base_m)
|
||||
+ float(self._dubins_saida_pp_lookahead_velocidade_s) * v
|
||||
)
|
||||
look = float(np.clip(
|
||||
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"
|
||||
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),
|
||||
))
|
||||
|
||||
s_alvo = min(s_max, float(s_proj) + look)
|
||||
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])),
|
||||
"s_ref": float(s_alvo),
|
||||
"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, status_carro
|
||||
):
|
||||
"""Preview leve usando o mesmo perfil PP da fase real atual."""
|
||||
def _simular_preview_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl):
|
||||
"""Preview leve usando exatamente o mesmo Pure Pursuit da V12."""
|
||||
v = max(float(velocidade), float(getattr(self, "_vmin_planejamento", 0.30)))
|
||||
dt = max(0.05, min(float(dt_ctrl), 0.50))
|
||||
dist_passo = max(0.05, v * dt)
|
||||
|
|
@ -1599,7 +1538,7 @@ class ControladorMPC:
|
|||
|
||||
for _ in range(passos):
|
||||
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(
|
||||
TipoMovimentoDirecional.MovimentoArco, ang
|
||||
|
|
@ -1739,7 +1678,7 @@ class ControladorMPC:
|
|||
idx_alvo_correcao, pos_lat, pos_lon, pos_theta, pos_latencia, ponto_alvo_real
|
||||
):
|
||||
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(
|
||||
|
|
@ -1757,7 +1696,7 @@ class ControladorMPC:
|
|||
|
||||
preview_xy = self._simular_preview_seguidor_dubins(
|
||||
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.insert(0, (pos_lat, pos_lon, pos_theta))
|
||||
|
|
@ -1795,12 +1734,10 @@ class ControladorMPC:
|
|||
"debug_custo": {},
|
||||
"debug_seguidor_dubins": {
|
||||
"modo": (
|
||||
"pure_pursuit_4ws_saindo_rua"
|
||||
"pure_pursuit_4ws_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"]),
|
||||
"distancia_alvo_m": float(dbg["distancia_alvo_m"]),
|
||||
"alpha_graus": float(math.degrees(dbg["alpha"])),
|
||||
|
|
|
|||
Loading…
Reference in New Issue