Merge branch 'op_control' of http://zendioninc.com.br:3000/Zendion/agrobot_base into op_control
This commit is contained in:
Diego Freitas
commit
1297a898d5
|
|
@ -617,46 +617,136 @@ namespace AgroBase.Services
|
|||
|
||||
private static void ProcessarGNTHS(string sentenca)
|
||||
{
|
||||
//Console.WriteLine(sentenca);
|
||||
// Remove o caractere de início '$' e divide os campos
|
||||
var campos = sentenca.TrimStart('$').Split(',');
|
||||
|
||||
if (campos.Length < 2)
|
||||
{
|
||||
Variaveis.MostrarLog("[GPSService.ProcessarGNTHS] Sentença incompleta.");
|
||||
if (string.IsNullOrWhiteSpace(sentenca))
|
||||
return;
|
||||
}
|
||||
|
||||
if (campos.Length > 2 && string.IsNullOrEmpty(campos[1]))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// Parsing dos campos
|
||||
double headingTrue = double.Parse(campos[1].Replace(".", ",")); // Campo <Heading>
|
||||
string status = campos[2].Split('*')[0]; // Campo <Status>
|
||||
string checksum = campos[2].Split('*')[1]; // Campo <Checksum>
|
||||
/*
|
||||
* Exemplos:
|
||||
*
|
||||
* Válido:
|
||||
* $GPTHS,123.4567,A*XX
|
||||
*
|
||||
* Inválido:
|
||||
* $GPTHS,,V*0E
|
||||
*/
|
||||
|
||||
PenultimaLeitura.TimestampOri = UltimaLeitura.TimestampOri.Clone();
|
||||
PenultimaLeitura.Momento = UltimaLeitura.Momento;
|
||||
PenultimaLeitura.OrientacaoReal = UltimaLeitura.OrientacaoReal;
|
||||
PenultimaLeitura.TipoOrientacao = UltimaLeitura.TipoOrientacao;
|
||||
string conteudo = sentenca.Trim();
|
||||
|
||||
if (conteudo.StartsWith("$"))
|
||||
conteudo = conteudo.Substring(1);
|
||||
|
||||
string[] partesChecksum = conteudo.Split(
|
||||
new[] { '*' },
|
||||
2
|
||||
);
|
||||
|
||||
string corpo = partesChecksum[0];
|
||||
|
||||
string[] campos = corpo.Split(',');
|
||||
|
||||
// [0] = GPTHS/GNTHS
|
||||
// [1] = heading
|
||||
// [2] = status A/V
|
||||
if (campos.Length < 3)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[GPSService.ProcessarGNTHS] Sentença incompleta: {sentenca}"
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
string headingTexto = (campos[1] ?? string.Empty).Trim();
|
||||
|
||||
string statusRecebido = (campos[2] ?? string.Empty)
|
||||
.Trim()
|
||||
.ToUpperInvariant();
|
||||
|
||||
bool headingNumerico = double.TryParse(
|
||||
headingTexto,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.InvariantCulture,
|
||||
out double headingTrue
|
||||
);
|
||||
|
||||
bool headingValido =
|
||||
statusRecebido == "A" &&
|
||||
headingNumerico &&
|
||||
!double.IsNaN(headingTrue) &&
|
||||
!double.IsInfinity(headingTrue);
|
||||
|
||||
/*
|
||||
* Toda sentença recebida gera uma nova leitura,
|
||||
* mesmo quando o heading está inválido.
|
||||
*
|
||||
* Dessa forma, a frequência continua sendo calculada
|
||||
* pela chegada das sentenças THS.
|
||||
*/
|
||||
|
||||
PenultimaLeitura.TimestampOri.valor =
|
||||
UltimaLeitura.TimestampOri.valor;
|
||||
|
||||
PenultimaLeitura.Momento =
|
||||
UltimaLeitura.Momento;
|
||||
|
||||
PenultimaLeitura.OrientacaoReal =
|
||||
UltimaLeitura.OrientacaoReal;
|
||||
|
||||
PenultimaLeitura.TipoOrientacao =
|
||||
UltimaLeitura.TipoOrientacao;
|
||||
|
||||
UltimaLeitura.TimestampOri.valor =
|
||||
Stopwatch.GetTimestamp() /
|
||||
(double)Stopwatch.Frequency;
|
||||
|
||||
UltimaLeitura.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
|
||||
UltimaLeitura.Momento = DateTime.Now;
|
||||
UltimaLeitura.OrientacaoReal = InverterHeading ? GPSUtils.NormalizarAngulo(headingTrue - 180.0) : headingTrue;
|
||||
UltimaLeitura.TipoOrientacao = status;
|
||||
|
||||
if (!headingValido)
|
||||
{
|
||||
/*
|
||||
* Comunicação está funcionando, mas o UM982
|
||||
* não possui solução válida de heading.
|
||||
*/
|
||||
UltimaLeitura.OrientacaoReal = 9999;
|
||||
|
||||
UltimaLeitura.TipoOrientacao = "V";
|
||||
|
||||
/*
|
||||
* Não aplicar posição corrigida nem atualizar
|
||||
* o ângulo usado pelo controle do rover.
|
||||
*
|
||||
* O valor 9999 existe apenas para telemetria
|
||||
* e diagnóstico de saúde.
|
||||
*/
|
||||
return;
|
||||
}
|
||||
|
||||
headingTrue = GPSUtils.NormalizarAngulo(
|
||||
headingTrue
|
||||
);
|
||||
|
||||
UltimaLeitura.OrientacaoReal =
|
||||
InverterHeading
|
||||
? GPSUtils.NormalizarAngulo(
|
||||
headingTrue - 180.0
|
||||
)
|
||||
: headingTrue;
|
||||
|
||||
UltimaLeitura.TipoOrientacao = "A";
|
||||
|
||||
AplicarPosicaoCorrigida();
|
||||
|
||||
// ✅ Atualiza o ângulo unificado no GPSService
|
||||
// Atualiza o ângulo usado pelo restante do sistema
|
||||
// somente quando a solução é válida.
|
||||
DefinirAnguloCarro();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Variaveis.MostrarLog($"[GPSService.ProcessarGNTHS] Erro ao processar a sentença: {ex.Message}");
|
||||
Variaveis.MostrarLog(
|
||||
$"[GPSService.ProcessarGNTHS] " +
|
||||
$"Erro ao processar a sentença '{sentenca}': {ex.Message}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1320,7 +1410,10 @@ namespace AgroBase.Services
|
|||
{
|
||||
try
|
||||
{
|
||||
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Simulando ? historicoPosicao.Peek() : UltimaLeitura;
|
||||
GPSModel posicaoAtual =
|
||||
Variaveis.OperacaoEmAndamento.Simulando
|
||||
? historicoPosicao.Peek()
|
||||
: UltimaLeitura;
|
||||
|
||||
bool rtkValido =
|
||||
posicaoAtual.IdadeCorrecao >= 0 &&
|
||||
|
|
@ -1331,31 +1424,91 @@ namespace AgroBase.Services
|
|||
posicaoAtual.QualidadeFix == TiposCorrecaoGPS.DGPS
|
||||
);
|
||||
|
||||
// ---------------------------------------------------------
|
||||
// ESTADO DO HEADING DUAL-ANTENNA
|
||||
// ---------------------------------------------------------
|
||||
|
||||
string statusOrientacao =
|
||||
(posicaoAtual.TipoOrientacao ?? string.Empty)
|
||||
.Trim()
|
||||
.ToUpperInvariant();
|
||||
|
||||
double valorOrientacao =
|
||||
posicaoAtual.OrientacaoReal;
|
||||
|
||||
bool valorOrientacaoNumerico =
|
||||
!double.IsNaN(valorOrientacao) &&
|
||||
!double.IsInfinity(valorOrientacao);
|
||||
|
||||
bool valorOrientacaoDentroDaFaixa =
|
||||
valorOrientacaoNumerico &&
|
||||
valorOrientacao >= 0.0 &&
|
||||
valorOrientacao < 360.0;
|
||||
|
||||
/*
|
||||
* A solução só é utilizável quando:
|
||||
*
|
||||
* 1. O UM982 informou status "A";
|
||||
* 2. O valor é numérico;
|
||||
* 3. O valor está na faixa válida de heading.
|
||||
*
|
||||
* Assim, a sentinela 9999 nunca será considerada válida.
|
||||
*/
|
||||
bool orientacaoValida =
|
||||
statusOrientacao == "A" &&
|
||||
valorOrientacaoDentroDaFaixa;
|
||||
|
||||
RedisService.AtualizarCampos(
|
||||
RedisService.ModKey(T_Code.Gps),
|
||||
|
||||
("conectado", Iniciado),
|
||||
("freq_base", TaxaAmostragemHz),
|
||||
|
||||
// Posição e orientação utilizada pelo carro
|
||||
("lat", posicaoAtual.Latitude),
|
||||
("lon", posicaoAtual.Longitude),
|
||||
("theta", posicaoAtual.AnguloCarroDefinido),
|
||||
|
||||
// Estado da solução GNSS
|
||||
("fix", (int)posicaoAtual.QualidadeFix),
|
||||
("rtk", rtkValido),
|
||||
("hAcc", posicaoAtual.PrecisaoCm),
|
||||
("nSatelites", posicaoAtual.NumeroSatelites),
|
||||
("age", posicaoAtual.IdadeCorrecao),
|
||||
|
||||
// Frequências
|
||||
("freq.posicao", posicaoAtual.TimestampPos.frequencia),
|
||||
("freq.orientacao", posicaoAtual.TimestampOri.frequencia),
|
||||
|
||||
// Latências
|
||||
("latency.posicao", posicaoAtual.TimestampPos.dt),
|
||||
("latency.orientacao", posicaoAtual.TimestampOri.dt),
|
||||
|
||||
// Timestamps monotônicos
|
||||
("timestamp.posicao", posicaoAtual.TimestampPos.valor),
|
||||
("timestamp.orientacao", posicaoAtual.TimestampOri.valor),
|
||||
("age", posicaoAtual.IdadeCorrecao),
|
||||
|
||||
// Estado específico do heading dual-antenna
|
||||
("orientacao.status", statusOrientacao),
|
||||
("orientacao.valida", orientacaoValida),
|
||||
("orientacao.valor", valorOrientacao),
|
||||
|
||||
("heartbeat", posicaoAtual.Heartbeat)
|
||||
);
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
Variaveis.MostrarLog($"[GPSService.AtualizarDadosRedis] Erro ao salvar dados do GPS no Redis: {e.Message}");
|
||||
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(T_Code.Gps, StatusModulo.Falha, 0, $"Erro ao salvar dados do GPS no Redis: {e.Message}");
|
||||
Variaveis.MostrarLog(
|
||||
$"[GPSService.AtualizarDadosRedis] " +
|
||||
$"Erro ao salvar dados do GPS no Redis: {e.Message}"
|
||||
);
|
||||
|
||||
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(
|
||||
T_Code.Gps,
|
||||
StatusModulo.Falha,
|
||||
0,
|
||||
$"Erro ao salvar dados do GPS no Redis: {e.Message}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ class ModuloGPS(ModuloDiagnosticoBase):
|
|||
def atualizar_saude(self):
|
||||
try:
|
||||
now = time.perf_counter()
|
||||
modulo = ContextoGlobalRedis.get_modulo(self.t_code)
|
||||
modulo = ContextoGlobalRedis.get_modulo(self.t_code) or {}
|
||||
FREQ_BASE = modulo.get("freq_base", 1)
|
||||
FREQ_MIN = FREQ_BASE * 0.2
|
||||
SAUDE_MIN_ALERTA = 80
|
||||
|
|
@ -31,18 +31,68 @@ class ModuloGPS(ModuloDiagnosticoBase):
|
|||
age = modulo.get("age", -1)
|
||||
ntrip = modulo.get("correcao_ntrip", False)
|
||||
|
||||
orientacao = modulo.get("orientacao") or {}
|
||||
|
||||
status_ori = str(
|
||||
orientacao.get("status", "")
|
||||
).strip().upper()
|
||||
|
||||
valor_ori_bruto = orientacao.get("valor", 9999.0)
|
||||
valida_redis = orientacao.get("valida", False)
|
||||
|
||||
# Proteção caso o Redis devolva booleano como texto.
|
||||
if isinstance(valida_redis, str):
|
||||
valida_redis = valida_redis.strip().lower() in (
|
||||
"true",
|
||||
"1",
|
||||
"yes",
|
||||
"sim",
|
||||
)
|
||||
else:
|
||||
valida_redis = bool(valida_redis)
|
||||
|
||||
try:
|
||||
valor_ori = float(valor_ori_bruto)
|
||||
except (TypeError, ValueError):
|
||||
valor_ori = 9999.0
|
||||
|
||||
valor_ori_valido = (
|
||||
math.isfinite(valor_ori)
|
||||
and 0.0 <= valor_ori < 360.0
|
||||
)
|
||||
|
||||
orientacao_recebendo = (
|
||||
freq_ori > FREQ_MIN
|
||||
and not fail_ori
|
||||
)
|
||||
|
||||
posicao_recebendo = (
|
||||
freq_pos > FREQ_MIN
|
||||
and not fail_pos
|
||||
)
|
||||
|
||||
orientacao_valida = (
|
||||
orientacao_recebendo
|
||||
and valida_redis
|
||||
and status_ori == "A"
|
||||
and valor_ori_valido
|
||||
)
|
||||
|
||||
saude = 100
|
||||
motivos = []
|
||||
|
||||
if not conectado:
|
||||
motivos.append("desconectado")
|
||||
motivos.append("GNSS desconectado")
|
||||
saude = 0
|
||||
elif freq_pos <= FREQ_MIN or fail_pos:
|
||||
motivos.append("Sem resposta de coordenadas")
|
||||
|
||||
elif not posicao_recebendo:
|
||||
motivos.append("Sem resposta de coordenadas GGA")
|
||||
saude = 0
|
||||
elif freq_ori <= FREQ_MIN or fail_ori:
|
||||
motivos.append("Sem resposta de orientacao")
|
||||
|
||||
elif not orientacao_recebendo:
|
||||
motivos.append("Sem resposta da sentença de orientação THS")
|
||||
saude = 0
|
||||
|
||||
else:
|
||||
f_pos = min(freq_pos, FREQ_BASE)
|
||||
erro_pos = (FREQ_BASE - f_pos) / FREQ_BASE
|
||||
|
|
@ -57,6 +107,30 @@ class ModuloGPS(ModuloDiagnosticoBase):
|
|||
saude -= abs(p_freq_ori)
|
||||
if abs(p_freq_ori) > 0:
|
||||
motivos.append(f"Frequência de orientacao baixa: {f_ori:.2f} Hz (penalidade {p_freq_ori}%)")
|
||||
|
||||
# A sentença está chegando, mas a solução pode estar inválida.
|
||||
if not orientacao_valida:
|
||||
P_ORIENTACAO_INVALIDA = 45
|
||||
saude -= P_ORIENTACAO_INVALIDA
|
||||
|
||||
if status_ori == "V":
|
||||
motivos.append(
|
||||
"Sentença THS recebida, mas solução de heading inválida "
|
||||
f"(status V, penalidade {P_ORIENTACAO_INVALIDA}%)"
|
||||
)
|
||||
|
||||
elif not valor_ori_valido:
|
||||
motivos.append(
|
||||
"Sentença THS recebida com valor de heading inválido "
|
||||
f"({valor_ori}, penalidade {P_ORIENTACAO_INVALIDA}%)"
|
||||
)
|
||||
|
||||
else:
|
||||
motivos.append(
|
||||
"Sentença THS recebida, mas orientação não foi marcada "
|
||||
f"como válida (penalidade {P_ORIENTACAO_INVALIDA}%)"
|
||||
)
|
||||
|
||||
# 🔹 Fix
|
||||
|
||||
p_fix = self._penalidade_fix(fix)
|
||||
|
|
@ -80,15 +154,22 @@ class ModuloGPS(ModuloDiagnosticoBase):
|
|||
saude -= p_hacc
|
||||
motivos.append(f"hAcc {'nao definido' if hAcc == 0 else 'alto'}: {hAcc:.2f} cm (penalidade {p_hacc}%)")
|
||||
|
||||
saude = max(saude, 0)
|
||||
saude = max(round(saude, 1), 0)
|
||||
|
||||
status = StatusModulo.OPERANTE
|
||||
if not conectado:
|
||||
status = StatusModulo.DESCONECTADO
|
||||
|
||||
elif not posicao_recebendo or not orientacao_recebendo:
|
||||
status = StatusModulo.FALHA
|
||||
|
||||
elif saude <= 0:
|
||||
status = StatusModulo.FALHA
|
||||
|
||||
elif saude < SAUDE_MIN_ALERTA:
|
||||
status = StatusModulo.ALERTA
|
||||
|
||||
else:
|
||||
status = StatusModulo.OPERANTE
|
||||
|
||||
saude_geral = {
|
||||
"conectado": conectado,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,8 @@
|
|||
{
|
||||
// Use IntelliSense to learn about possible attributes.
|
||||
// Hover to view descriptions of existing attributes.
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue