From a5c377195f4bf29fe5237fd674e64c67dce8b920 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Tue, 16 Jun 2026 13:58:17 -0300 Subject: [PATCH] ajustado saude do gps --- AgroBase/AgroBase/Services/GPSService.cs | 213 +++++++++++++++--- .../workers/health_worker/modulos/gps.py | 97 +++++++- .../Sensoriamento_v23/.theia/launch.json | 8 + 3 files changed, 280 insertions(+), 38 deletions(-) create mode 100644 Firmware/Sensoriamento/Sensoriamento_v23/.theia/launch.json diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index daca5d080..e2d5e19b9 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -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 - string status = campos[2].Split('*')[0]; // Campo - string checksum = campos[2].Split('*')[1]; // Campo + /* + * 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}" + ); } } diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/gps.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/gps.py index dca1a1f48..b7680c3fc 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/gps.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/gps.py @@ -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, diff --git a/Firmware/Sensoriamento/Sensoriamento_v23/.theia/launch.json b/Firmware/Sensoriamento/Sensoriamento_v23/.theia/launch.json new file mode 100644 index 000000000..9a49ac9ff --- /dev/null +++ b/Firmware/Sensoriamento/Sensoriamento_v23/.theia/launch.json @@ -0,0 +1,8 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [ + + ] +}