diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index 1c490c2be..394d77796 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -1763,7 +1763,7 @@ namespace AgroBase.Forms.Operacoes } // agrupar por tipo - var porTipo = dict.Where(x => x.Key.Contains("_")).GroupBy(kv => CandMpcKey.ParseKey(kv.Key).Tipo); + var porTipo = dict.Where(x => x.Key.Contains("_") && !x.Key.Contains("d")).GroupBy(kv => CandMpcKey.ParseKey(kv.Key).Tipo); foreach (var g in porTipo.OrderBy(gr => gr.Key.ToString())) { var tipoNode = tv.Nodes.Add(g.Key.ToString()); diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 8e40bdffc..cf26eb4cb 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -22,7 +22,7 @@ namespace AgroBase.Models #region PARAMETROS public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva - public double DistanciaProjecaoRua { get; set; } = (VariaveisEquipamento.DistanciaEntreEixos / 100.0 / 2.0) + 2.0; // Distancia para projetar o primeiro ponto para fora do corredor + public double DistanciaProjecaoRua { get; set; } = (VariaveisEquipamento.DistanciaEntreEixos / 100.0 / 2.0) + 3.0; // Distancia para projetar o primeiro ponto para fora do corredor public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores @@ -207,24 +207,44 @@ namespace AgroBase.Models int idxNovoCorredor = PontoAtual?.idxCorredor ?? 0; - if ((CorredorAtual?.Idx ?? 0) != idxNovoCorredor) + if (idxNovoCorredor < 0 || idxNovoCorredor >= _Corredores.Count) { - CorredorAtual?.AtualizarDados(); - - //AtualizarDadosAutonomiaCorredor(idxCorredor: idxNovoCorredor); + return; } bool mudouCorredor = (CorredorAtual?.Idx ?? -1) != idxNovoCorredor; - if (mudouCorredor || !AutonomiaCorredor.Liberado) + + if (mudouCorredor) { - AtualizarDadosAutonomiaCorredor(idxCorredor: idxNovoCorredor); + /* + * Fecha os dados acumulados do corredor anterior + * antes de tentar assumir o próximo. + */ + CorredorAtual?.AtualizarDados(); } - if (AutonomiaCorredor.Liberado) - { - CorredorAtual = _Corredores[idxNovoCorredor]; - CorredorAtual.AtualizarDados(); - } + /* + * Sempre chama. + * + * Antes da entrada: + * recalcula a autonomia em tempo real. + * + * Depois da confirmação: + * AutonomiaCorredorModel devolve o snapshot congelado + * rapidamente, sem recalcular. + */ + AtualizarDadosAutonomiaCorredor(idxCorredor: idxNovoCorredor); + + /* + * Não assume o novo corredor se a autonomia + * ainda não autorizou a entrada. + */ + if (!AutonomiaCorredor.Liberado) + return; + + CorredorAtual = _Corredores[idxNovoCorredor]; + + CorredorAtual.AtualizarDados(); } public DirecaoCarroRua DirecaoCaminho { @@ -767,6 +787,76 @@ namespace AgroBase.Models return -1; } + private void ConfirmarEntradaAutonomiaCorredor() + { + if (RetornandoBase) + return; + + if (CorredorAtual == null) + return; + + if (AutonomiaCorredor == null) + return; + + if (!AutonomiaCorredor.Iniciado) + return; + + if (!AutonomiaCorredor.Liberado) + return; + + /* + * Proteção importante: + * + * PontoAtual e CorredorAtual precisam representar + * o mesmo corredor. Isso evita confirmar o corredor + * antigo durante uma transição bloqueada. + */ + if (PontoAtual == null || PontoAtual.idxCorredor != CorredorAtual.Idx) + { + return; + } + + /* + * EntrandoRua: + * confirmação normal na borda de entrada. + * + * CaminhandoRua: + * fallback para início no meio da rua, + * recuperação de trajetória ou salto de estado. + */ + bool entradaFisicaConfirmada = + StatusAtual == StatusCarroMapa.EntrandoRua || + StatusAtual == StatusCarroMapa.CaminhandoRua; + + if (!entradaFisicaConfirmada) + return; + + int idxCorredor = CorredorAtual.Idx; + + if (AutonomiaCorredor.CorredorEstaConfirmado(idxCorredor)) + { + return; + } + + bool confirmou = AutonomiaCorredor.ConfirmarEntradaCorredor(idxCorredor); + + if (!confirmou) + return; + + Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + "[AUTONOMIA] Entrada no corredor confirmada. " + + $"corredor={idxCorredor + 1}, " + + $"status={StatusAtual}, " + + $"bat_segura={AutonomiaCorredor.DistanciaSeguraBateria_m:F1}m, " + + $"herb_seguro={AutonomiaCorredor.DistanciaSeguraHerbicida_m:F1}m, " + + $"bat_folga={AutonomiaCorredor.FolgaBateria_m:F1}m, " + + $"herb_folga={AutonomiaCorredor.FolgaHerbicida_m:F1}m" + ); + } + private void AtualizarDadosTrajetoria() { @@ -857,6 +947,7 @@ namespace AgroBase.Models AtualizarDistanciasLaterais(); AtualizarNaMargemDoCorredor(); AtualizarStatusAtual(); + ConfirmarEntradaAutonomiaCorredor(); AtualizarManobrandoEntreRuas(); AtualizarPercentualTrajetoria(); AtualizarTempoEstimado(); @@ -2206,7 +2297,7 @@ namespace AgroBase.Models anguloAcrescentar *= -1; } double percentualAcrescimoPontoAberturaCurva = 0.8; - GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : (DistanciaProjecaoRua * 2.0), anguloProjetar2); + GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva), anguloProjetar2); PontoTrajetoriaModel PontoFinal = new PontoTrajetoriaModel(TipoPontoRua.LigacaoSaida) { @@ -3454,14 +3545,94 @@ namespace AgroBase.Models public class AutonomiaCorredorModel { - // ========================== - // Configurações - // ========================== - private readonly TimeSpan _tempoParaTravar = TimeSpan.FromSeconds(10); // X segundos em crítico -> trava + [Flags] + private enum MotivoTrava + { + Nenhum = 0, + Bateria = 1, + Herbicida = 2, + } - // ========================== + private sealed class SnapshotCorredor + { + public int Indice { get; set; } + public bool Liberado { get; set; } + public AutonomiaCorredorStatus Status { get; set; } + + public string Motivo { get; set; } = ""; + public List Motivos { get; set; } = new List(); + public List Avisos { get; set; } = new List(); + + public double DistanciaCorredor_m { get; set; } + public double DistanciaRequeridaBateria_m { get; set; } + public double DistanciaRequeridaHerbicida_m { get; set; } + + public double DistanciaSeguraBateria_m { get; set; } + public double DistanciaSeguraHerbicida_m { get; set; } + + public double FolgaBateria_m { get; set; } + public double FolgaHerbicida_m { get; set; } + + public bool DadosBateriaValidos { get; set; } + public bool DadosHerbicidaValidos { get; set; } + + public bool BateriaSuficiente { get; set; } + public bool HerbicidaSuficiente { get; set; } + + public bool BateriaOverrideAplicado { get; set; } + public bool HerbicidaOverrideAplicado { get; set; } + + public string FonteBateria { get; set; } = ""; + public string FonteAutonomiaBateria { get; set; } = ""; + public string FonteAutonomiaHerbicida { get; set; } = ""; + + public double PercentualBateria { get; set; } + public double PercentualReservatorio { get; set; } + + public bool PulverizacaoExigida { get; set; } + public bool ConfirmadoParaEntrada { get; set; } + + public DateTime TimestampUtc { get; set; } + } + + // ============================================================ + // Configuração + // ============================================================ + + private readonly TimeSpan _tempoParaTravar; + + /* + * Os contadores de bateria e herbicida já possuem: + * - reserva mínima; + * - margem de segurança; + * - referências conservadoras por metro. + * + * Portanto, os extras ficam em zero por padrão para evitar + * aplicar margem dupla escondida. + */ + public double DistanciaExtraBateria_m { get; set; } + public double DistanciaExtraHerbicida_m { get; set; } + + /* + * Evita oscilações por erro numérico de poucos centímetros. + * Não substitui a margem dos contadores. + */ + public double ToleranciaComparacao_m { get; set; } + + public double TempoParaTravar_s => _tempoParaTravar.TotalSeconds; + + public AutonomiaCorredorModel(double tempoParaTravar_s = 10.0, double distanciaExtraBateria_m = 0.0, double distanciaExtraHerbicida_m = 0.0, double toleranciaComparacao_m = 0.50) + { + _tempoParaTravar = TimeSpan.FromSeconds(Math.Max(0, tempoParaTravar_s)); + DistanciaExtraBateria_m = Math.Max(0, distanciaExtraBateria_m); + DistanciaExtraHerbicida_m = Math.Max(0, distanciaExtraHerbicida_m); + ToleranciaComparacao_m = Math.Max(0, toleranciaComparacao_m); + } + + // ============================================================ // Estado público - // ========================== + // ============================================================ + [JsonProperty] public bool Iniciado { get; private set; } @@ -3471,350 +3642,1445 @@ namespace AgroBase.Models [JsonProperty] public bool Liberado { get; private set; } = true; + [JsonProperty] + public int IndiceCorredor { get; private set; } = -1; + [JsonProperty] public double DistanciaCorredor_m { get; private set; } + [JsonProperty] + public double DistanciaRequeridaBateria_m { get; private set; } + + [JsonProperty] + public double DistanciaRequeridaHerbicida_m { get; private set; } + [JsonProperty] public double DistanciaSeguraBateria_m { get; private set; } [JsonProperty] public double DistanciaSeguraHerbicida_m { get; private set; } + [JsonProperty] + public double FolgaBateria_m { get; private set; } + + [JsonProperty] + public double FolgaHerbicida_m { get; private set; } + + [JsonProperty] + public bool DadosBateriaValidos { get; private set; } + + [JsonProperty] + public bool DadosHerbicidaValidos { get; private set; } + + [JsonProperty] + public bool BateriaSuficiente { get; private set; } + + [JsonProperty] + public bool HerbicidaSuficiente { get; private set; } + + [JsonProperty] + public bool PulverizacaoExigida { get; private set; } + + [JsonProperty] + public bool BateriaOverrideAplicado { get; private set; } + + [JsonProperty] + public bool HerbicidaOverrideAplicado { get; private set; } + + [JsonProperty] + public string FonteBateria { get; private set; } = ""; + + [JsonProperty] + public string FonteAutonomiaBateria { get; private set; } = ""; + + [JsonProperty] + public string FonteAutonomiaHerbicida { get; private set; } = ""; + + [JsonProperty] + public double PercentualBateria { get; private set; } + + [JsonProperty] + public double PercentualReservatorio { get; private set; } + + [JsonProperty] + public bool ConfirmadoParaEntrada { get; private set; } + [JsonProperty] public string Motivo { get; private set; } = ""; [JsonProperty] public List Motivos { get; private set; } = new List(); - // ========================== - // Flags de conveniência - // ========================== - public bool BateriaSuficiente => DistanciaSeguraBateria_m >= DistanciaCorredor_m; - public bool HerbicidaSuficiente => DistanciaSeguraHerbicida_m >= DistanciaCorredor_m; + [JsonProperty] + public List Avisos { get; private set; } = new List(); - // ========================== - // Controle de histórico por corredor - // ========================== - private Dictionary CorredoresLiberados { get; set; } = new Dictionary(); + // ============================================================ + // Estado interno por corredor + // ============================================================ + + /* + * Histórico é atualizado continuamente. + * Um corredor só é congelado quando ConfirmarEntradaCorredor() + * é chamado no ponto real de entrada. + */ + private readonly Dictionary _historicoCorredores = new Dictionary(); + + private readonly HashSet _corredoresConfirmados = new HashSet(); + + /* + * Latch específico por motivo. + * Uma trava de bateria não pode ser reconhecida pelo botão + * de reservatório, e vice-versa. + */ + private readonly Dictionary _travasPorCorredor = new Dictionary(); - // ========================== - // Trava (latch) por corredor - // ========================== - private readonly HashSet _corredoresTravados = new HashSet(); private readonly Dictionary _inicioCriticoUtc = new Dictionary(); - // ========================== + /* + * Overrides assumidos permanecem válidos somente para o corredor + * em que foram concedidos. + */ + private readonly Dictionary _overridesPorCorredor = new Dictionary(); + + /* + * Os bools recebidos por AtualizarDados são tratados como sinais. + * Apenas a borda false -> true gera um novo pulso de autorização. + * Isso evita que uma flag esquecida em true libere corredores futuros. + */ + private bool _ultimoSinalLiberacaoBateria; + private bool _ultimoSinalLiberacaoReservatorio; + + private int _ultimoIndiceAvaliado = -1; + + private readonly Dictionary _ultimaAssinaturaLog = new Dictionary(); + + // ============================================================ // Ciclo de vida - // ========================== + // ============================================================ + public void Iniciar() { Reiniciar(); - if (Variaveis.OperacaoEmAndamento.Parametros.ControleAutomatico) - Iniciado = true; + + Iniciado = Variaveis.OperacaoEmAndamento?.Parametros?.ControleAutomatico ?? false; + + /* + * Fora do automático, esta classe não impede movimento. + */ + Liberado = !Iniciado; } public void Parar() { Iniciado = false; - // opcional: não limpar travas aqui, pois pode querer manter estado até o operador decidir. - // se preferir limpar, descomente: - // _corredoresTravados.Clear(); - // _inicioCriticoUtc.Clear(); + Liberado = true; + Status = AutonomiaCorredorStatus.Desconhecido; } - // ========================== + // ============================================================ // Atualização principal - // ========================== + // ============================================================ + public void AtualizarDados(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado) { - // Se não iniciado, não bloqueia nada + bool pulsoLiberacaoBateria = bateria_liberada && !_ultimoSinalLiberacaoBateria; + + bool pulsoLiberacaoReservatorio = reservatorio_liberado && !_ultimoSinalLiberacaoReservatorio; + + _ultimoSinalLiberacaoBateria = bateria_liberada; + + _ultimoSinalLiberacaoReservatorio = reservatorio_liberado; + if (!Iniciado) { Liberado = true; - Motivos = new List(); Status = AutonomiaCorredorStatus.Desconhecido; + Motivo = "Autonomia de corredor inativa."; + Motivos = new List(); + Avisos = new List(); return; } var op = Variaveis.OperacaoEmAndamento; - // Valida índice do corredor - if ((op.Trajetoria?._Corredores?.Count ?? 0) <= idxCorredor) + if (op == null) { - Liberado = false; - Motivos = new List() - { - $"Trajetória não possui corredores definidos. {idxCorredor}/{op.Trajetoria?._Corredores?.Count}" - }; - Status = AutonomiaCorredorStatus.Critico; + AplicarFalhaEstrutural( + idxCorredor, + "Operação em andamento indisponível." + ); return; } - // Se já registrou esse corredor como liberado e travado/feito, não recalcula - if (CorredoresLiberados.ContainsKey(idxCorredor) && CorredoresLiberados[idxCorredor].Liberado) + int quantidadeCorredores = op.Trajetoria?._Corredores?.Count ?? 0; + + if (idxCorredor < 0 || idxCorredor >= quantidadeCorredores) + { + AplicarFalhaEstrutural( + idxCorredor, + $"Índice de corredor inválido: " + + $"{idxCorredor}/{quantidadeCorredores}." + ); return; + } - var motivosBloqueio = new List(); - Motivos = new List(); + /* + * Depois que a trajetória confirma a entrada real, a autorização + * daquele corredor é congelada. Até essa confirmação, continua + * sendo reavaliada em tempo real. + */ + if ( + _corredoresConfirmados.Contains(idxCorredor) && + _historicoCorredores.TryGetValue( + idxCorredor, + out SnapshotCorredor confirmado + ) + ) + { + AplicarSnapshot(confirmado); + return; + } - // Atalho: interpretar que o operador “clicou em liberar” (pelo menos um sinal veio) - bool solicitouLiberacaoHumana = bateria_liberada || reservatorio_liberado; - - // Carrega distâncias e contadores double distanciaCorredor = op.Trajetoria._Corredores[idxCorredor]?.DistanciaTotal ?? 0; - var _Bateria = op.DispSen?.Dados?.ContadorCarga; - var _Reservatorio = op.DispAtu?.Dados?.ContadorCarga; + if (!ValorFinito(distanciaCorredor) || distanciaCorredor <= 0) + { + AplicarFalhaEstrutural( + idxCorredor, + $"Corredor {idxCorredor + 1} possui " + + $"distância inválida: {distanciaCorredor:F2} m." + ); + return; + } + + IndiceCorredor = idxCorredor; + _ultimoIndiceAvaliado = idxCorredor; DistanciaCorredor_m = distanciaCorredor; - DistanciaSeguraBateria_m = _Bateria?.DistanciaEstimadaRestante_m ?? 0; - DistanciaSeguraHerbicida_m = _Reservatorio?.DistanciaEstimadaRestantePulverizando_m ?? 0; - bool bateriaOk = BateriaSuficiente && (_Bateria?.Iniciado ?? false); - bool herbOk = HerbicidaSuficiente && (_Reservatorio?.Iniciado ?? false); + DistanciaRequeridaBateria_m = Math.Max(0, distanciaCorredor + DistanciaExtraBateria_m); + + DistanciaRequeridaHerbicida_m = Math.Max(0, distanciaCorredor + DistanciaExtraHerbicida_m); + + var bateria = op.DispSen?.Dados?.ContadorCarga; + + var reservatorio = op.DispAtu?.Dados?.ContadorCarga; + + AtualizarDiagnosticoBateria(bateria); + + AtualizarDiagnosticoHerbicida(reservatorio); bool pulverizadorAutomatico = op.Parametros?.Controle?.PulverizadorAutomatico ?? false; bool atuMandatorio = - op.Parametros?.ModulosMandatorios?.Any(x => - x.Dispositivo == T_Code.Atu && - x.Mandatorio && - x.Utilizar - ) ?? false; + op + .Parametros? + .ModulosMandatorios? + .Any( + x => + x.Dispositivo == T_Code.Atu && + x.Mandatorio && + x.Utilizar + ) + ?? false; - bool exigirPulverizacaoTotal = pulverizadorAutomatico && atuMandatorio; + PulverizacaoExigida = + pulverizadorAutomatico && + atuMandatorio; - // ========================== - // 1) Calcula status "base" (sem considerar trava) - // ========================== - AutonomiaCorredorStatus statusBase; - string motivoBase = ""; - string motivoBat = "Bateria " + (bateriaOk ? "suficiente" : "insuficiente") + - $". Distância segura: {DistanciaSeguraBateria_m:F1} m, corredor: {distanciaCorredor:F1} m"; - string motivoHerb = "Herbicida " + (herbOk ? "suficiente" : "insuficiente") + - $" para pulverizar o corredor. Distância segura pulverizando: {DistanciaSeguraHerbicida_m:F1} m, corredor: {distanciaCorredor:F1} m"; + BateriaSuficiente = + DadosBateriaValidos && + DistanciaSeguraBateria_m + + ToleranciaComparacao_m >= + DistanciaRequeridaBateria_m; - // Regra: bateria insuficiente é sempre crítica (não atravessa com segurança) - bool criticoBateria = !bateriaOk; + HerbicidaSuficiente = + DadosHerbicidaValidos && + DistanciaSeguraHerbicida_m + + ToleranciaComparacao_m >= + DistanciaRequeridaHerbicida_m; - // Regra: herbicida insuficiente só vira crítico se pulverização total é mandatória - bool criticoHerbicida = !herbOk && exigirPulverizacaoTotal; + FolgaBateria_m = + DistanciaSeguraBateria_m - + DistanciaRequeridaBateria_m; - if (criticoBateria || criticoHerbicida) + FolgaHerbicida_m = + DistanciaSeguraHerbicida_m - + DistanciaRequeridaHerbicida_m; + + AtualizarAvisos( + bateria, + reservatorio + ); + + MotivoTrava criticosAtuais = + MotivoTrava.Nenhum; + + var motivosCriticos = + new List(); + + if (!BateriaSuficiente) { - statusBase = AutonomiaCorredorStatus.Critico; + criticosAtuais |= + MotivoTrava.Bateria; - if (criticoBateria) - { - motivosBloqueio.Add("Bateria insuficiente para concluir o corredor com segurança."); - motivoBase += motivoBat; - } + motivosCriticos.Add( + MontarMotivoBateria() + ); + } - if (criticoHerbicida) + if ( + PulverizacaoExigida && + !HerbicidaSuficiente + ) + { + criticosAtuais |= + MotivoTrava.Herbicida; + + motivosCriticos.Add( + MontarMotivoHerbicida() + ); + } + + /* + * Guarda qualquer override já assumido neste corredor. + */ + MotivoTrava overrides = + ObterFlags( + _overridesPorCorredor, + idxCorredor + ); + + if ( + pulsoLiberacaoBateria && + ( + criticosAtuais.HasFlag( + MotivoTrava.Bateria + ) || + ObterFlags( + _travasPorCorredor, + idxCorredor + ).HasFlag( + MotivoTrava.Bateria + ) + ) + ) + { + overrides |= + MotivoTrava.Bateria; + } + + if ( + pulsoLiberacaoReservatorio && + ( + criticosAtuais.HasFlag( + MotivoTrava.Herbicida + ) || + ObterFlags( + _travasPorCorredor, + idxCorredor + ).HasFlag( + MotivoTrava.Herbicida + ) + ) + ) + { + overrides |= + MotivoTrava.Herbicida; + } + + if (overrides != MotivoTrava.Nenhum) + { + _overridesPorCorredor[idxCorredor] = + overrides; + } + + BateriaOverrideAplicado = + overrides.HasFlag( + MotivoTrava.Bateria + ); + + HerbicidaOverrideAplicado = + overrides.HasFlag( + MotivoTrava.Herbicida + ); + + AtualizarLatch( + idxCorredor, + criticosAtuais + ); + + MotivoTrava travas = + ObterFlags( + _travasPorCorredor, + idxCorredor + ); + + bool bateriaResolvida = + !criticosAtuais.HasFlag( + MotivoTrava.Bateria + ) || + BateriaOverrideAplicado; + + bool herbicidaResolvido = + !criticosAtuais.HasFlag( + MotivoTrava.Herbicida + ) || + HerbicidaOverrideAplicado; + + bool criticosAtuaisResolvidos = + bateriaResolvida && + herbicidaResolvido; + + /* + * Quando a condição já se recuperou, uma trava persistente ainda + * exige reconhecimento específico do motivo que a originou. + */ + bool travaBateriaReconhecida = + !travas.HasFlag( + MotivoTrava.Bateria + ) || + BateriaOverrideAplicado; + + bool travaHerbicidaReconhecida = + !travas.HasFlag( + MotivoTrava.Herbicida + ) || + HerbicidaOverrideAplicado; + + bool travasReconhecidas = + travaBateriaReconhecida && + travaHerbicidaReconhecida; + + AutonomiaCorredorStatus statusFinal; + bool liberadoFinal; + string motivoFinal; + var motivosBloqueio = + new List(); + + if ( + criticosAtuais != MotivoTrava.Nenhum && + !criticosAtuaisResolvidos + ) + { + bool travado = + travas != MotivoTrava.Nenhum; + + statusFinal = + travado + ? AutonomiaCorredorStatus + .AguardandoLiberacaoManual + : AutonomiaCorredorStatus + .Critico; + + liberadoFinal = false; + + motivosBloqueio.AddRange( + motivosCriticos + ); + + motivoFinal = + $"Corredor {idxCorredor + 1}: " + + ( + travado + ? "BLOQUEADO após condição crítica persistente. " + : "autonomia crítica. " + ) + + string.Join( + "; ", + motivosCriticos + ); + + if (travado) { - motivosBloqueio.Add("Herbicida insuficiente para pulverizar o corredor completo."); - motivoBase += (string.IsNullOrEmpty(motivoBase) ? "" : "; ") + motivoHerb; + motivoFinal += + " Aguardando liberação específica " + + "do operador."; } } + else if ( + travas != MotivoTrava.Nenhum && + !travasReconhecidas + ) + { + statusFinal = + AutonomiaCorredorStatus + .AguardandoLiberacaoManual; + + liberadoFinal = false; + + motivosBloqueio.Add( + $"Corredor {idxCorredor + 1}: " + + "a autonomia se recuperou, mas existe " + + "uma trava persistente aguardando " + + "reconhecimento do operador." + ); + + motivoFinal = + motivosBloqueio[0]; + } else { - if (!herbOk && !exigirPulverizacaoTotal) + if ( + travas != MotivoTrava.Nenhum && + travasReconhecidas + ) { - statusBase = AutonomiaCorredorStatus.OkSomenteTransito; - motivoBase = motivoHerb; + DestravarCorredor( + idxCorredor + ); + } + + bool houveOverride = + BateriaOverrideAplicado || + HerbicidaOverrideAplicado; + + if (houveOverride) + { + statusFinal = + AutonomiaCorredorStatus + .OkSupervisionado; + + motivoFinal = + MontarMotivoSupervisionado( + idxCorredor + ); + } + else if ( + pulverizadorAutomatico && + HerbicidaSuficiente + ) + { + statusFinal = + AutonomiaCorredorStatus + .OkCompleto; + + motivoFinal = + $"Corredor {idxCorredor + 1}: " + + "bateria e herbicida suficientes."; } else { - statusBase = AutonomiaCorredorStatus.OkCompleto; + statusFinal = + AutonomiaCorredorStatus + .OkSomenteTransito; + + motivoFinal = + $"Corredor {idxCorredor + 1}: " + + "bateria suficiente para atravessar. " + + ( + pulverizadorAutomatico + ? "Pulverização completa não é mandatória ou não está garantida." + : "Pulverizador automático desativado." + ); } + + liberadoFinal = true; } - // ========================== - // 2) Atualiza timers de crítico e ativa trava se necessário - // ========================== - bool estaCriticoAgora = (statusBase == AutonomiaCorredorStatus.Critico); - - if (estaCriticoAgora) - { - if (!_inicioCriticoUtc.ContainsKey(idxCorredor)) - _inicioCriticoUtc[idxCorredor] = DateTime.UtcNow; - - var tempoCritico = DateTime.UtcNow - _inicioCriticoUtc[idxCorredor]; - - if (tempoCritico >= _tempoParaTravar) - _corredoresTravados.Add(idxCorredor); - } - else - { - // Saiu do crítico -> reseta timer (mas não destrava automaticamente se já travou) - if (_inicioCriticoUtc.ContainsKey(idxCorredor)) - _inicioCriticoUtc.Remove(idxCorredor); - } - - // ========================== - // 3) Aplicação da trava + liberação humana - // ========================== - void DestravarCorredoresAte(int _idxCorredor) - { - for (int i = 0; i <= _idxCorredor; i++) - { - _corredoresTravados.Remove(i); - _inicioCriticoUtc.Remove(i); - } - } - - bool corredorTravado = _corredoresTravados.Contains(idxCorredor); - - AutonomiaCorredorStatus statusFinal = statusBase; - bool liberadoFinal = false; - string motivoFinal = ""; - - if (corredorTravado) - { - // Se travou, não libera sozinho nunca - if (!solicitouLiberacaoHumana) - { - statusFinal = AutonomiaCorredorStatus.AguardandoLiberacaoManual; - liberadoFinal = false; - - // Motivo ajuda o operador a entender o porquê - string detalhes = string.IsNullOrEmpty(motivoBase) - ? "Autonomia insuficiente detectada anteriormente." - : motivoBase; - - motivoFinal = $"Corredor {idxCorredor + 1}: BLOQUEADO. {detalhes} " + - $"Aguardando liberação do operador pela base."; - motivosBloqueio.Add($"Corredor {idxCorredor + 1}: BLOQUEADO. Aguardando solução ou liberação manual do operador."); - } - else - { - // Operador solicitou liberação - if (statusBase != AutonomiaCorredorStatus.Critico) - { - // ✅ Se agora está OK, libera como OK normal (não supervisionado) - statusFinal = statusBase; // OkCompleto ou OkSomenteTransito - liberadoFinal = true; - - motivoFinal = $"Corredor {idxCorredor + 1}: LIBERADO pelo operador (parâmetros OK). " + - (string.IsNullOrEmpty(motivoBase) ? "Liberado" : motivoBase); - - // Destrava o corredor - DestravarCorredoresAte(idxCorredor); - } - else - { - motivosBloqueio.Clear(); - - // ❌ Ainda está crítico, mas operador insistiu -> OkSupervisionado (override) - statusFinal = AutonomiaCorredorStatus.OkSupervisionado; - liberadoFinal = true; - - motivoFinal = $"Corredor {idxCorredor + 1}: LIBERAÇÃO MANUAL (override). " + - (string.IsNullOrEmpty(motivoBase) ? "Autonomia insuficiente, mas liberado." : motivoBase); - motivosBloqueio.Add($"Corredor {idxCorredor + 1}: LIBERAÇÃO MANUAL. Autonomia insuficiente, mas houve liberação supervisionada."); - - // Destrava o corredor (o operador assumiu) - DestravarCorredoresAte(idxCorredor); - } - } - } - else - { - // Sem trava: comportamento raiz - statusFinal = statusBase; - - liberadoFinal = - statusFinal == AutonomiaCorredorStatus.OkCompleto || - statusFinal == AutonomiaCorredorStatus.OkSomenteTransito; - - motivoFinal = $"Corredor {idxCorredor + 1}: " + - (liberadoFinal - ? (string.IsNullOrEmpty(motivoBase) ? "Liberado" : motivoBase) - : (string.IsNullOrEmpty(motivoBase) ? "Bloqueado" : motivoBase)); - } - - // ========================== - // 4) Persiste/Loga resultado como você já fazia (mantendo raiz) - // ========================== Status = statusFinal; + Liberado = liberadoFinal; + Motivo = motivoFinal; - Motivos = motivosBloqueio - .Where(x => !string.IsNullOrWhiteSpace(x)) - .Distinct() - .ToList(); + Motivos = + motivosBloqueio + .Where( + x => + !string.IsNullOrWhiteSpace(x) + ) + .Distinct() + .ToList(); - var res = new AutonomiaCorredorModel() - { - Iniciado = this.Iniciado, - Liberado = liberadoFinal, - Motivo = motivoFinal, - Motivos = Motivos, - Status = statusFinal, - DistanciaCorredor_m = DistanciaCorredor_m, - DistanciaSeguraBateria_m = DistanciaSeguraBateria_m, - DistanciaSeguraHerbicida_m = DistanciaSeguraHerbicida_m, - }; + ConfirmadoParaEntrada = false; - if (CorredoresLiberados.ContainsKey(idxCorredor)) - { - // Se estava bloqueado e agora liberou, registra mudança - if (CorredoresLiberados[idxCorredor].Liberado == false && liberadoFinal) - { - CorredoresLiberados[idxCorredor] = res; - op.Sensoriamento.InserirLog(T_Code.Trj, liberadoFinal ? StatusModulo.Operante : StatusModulo.Alerta, liberadoFinal ? 100 : 0, $"Corredor {idxCorredor + 1} liberado pelo operador"); - } - else - { - // Mantém o mais recente motivo/status para UI (opcional) - CorredoresLiberados[idxCorredor] = res; - } - } - else - { - CorredoresLiberados.Add(idxCorredor, res); - op.Sensoriamento.InserirLog(T_Code.Trj, liberadoFinal ? StatusModulo.Operante : StatusModulo.Alerta, liberadoFinal ? 100 : 0, $"{motivoFinal}"); - } + SnapshotCorredor snapshot = + CriarSnapshot( + idxCorredor + ); - // Atualiza estado exposto - Liberado = res.Liberado; - Motivo = res.Motivo; - Motivos = res.Motivos; + _historicoCorredores[idxCorredor] = + snapshot; + + LogarSeMudou( + op, + snapshot + ); } - // ========================== - // Reiniciar - // ========================== + // ============================================================ + // Diagnóstico dos contadores revisados + // ============================================================ + + private void AtualizarDiagnosticoBateria(CoulombCounterService bateria) + { + DistanciaSeguraBateria_m = 0; + DadosBateriaValidos = false; + + FonteBateria = ""; + FonteAutonomiaBateria = ""; + PercentualBateria = 0; + + if (bateria == null) + return; + + try + { + DistanciaSeguraBateria_m = + NumeroNaoNegativo( + (double) + bateria + .DistanciaEstimadaRestante_m + ); + + FonteBateria = + Convert.ToString( + bateria.FonteAtiva + ) + ?? ""; + + FonteAutonomiaBateria = + Convert.ToString( + bateria.FonteAutonomia + ) + ?? ""; + + PercentualBateria = + Clamp( + (double) + bateria.PercentualAtual, + 0, + 100 + ); + + double whPorMetroReferencia = + NumeroNaoNegativo( + (double) + bateria.WhPorMetroReferencia + ); + + DadosBateriaValidos = + (bool)bateria.Iniciado && + !string.IsNullOrWhiteSpace( + FonteBateria + ) && + !string.Equals( + FonteBateria, + "nenhuma", + StringComparison.OrdinalIgnoreCase + ) && + whPorMetroReferencia > 0 && + ValorFinito( + DistanciaSeguraBateria_m + ); + } + catch + { + DadosBateriaValidos = false; + } + } + + private void AtualizarDiagnosticoHerbicida(HerbicideCounterService reservatorio) + { + DistanciaSeguraHerbicida_m = 0; + DadosHerbicidaValidos = false; + + FonteAutonomiaHerbicida = ""; + PercentualReservatorio = 0; + + if (reservatorio == null) + return; + + try + { + DistanciaSeguraHerbicida_m = + NumeroNaoNegativo( + (double) + reservatorio + .DistanciaEstimadaRestantePulverizando_m + ); + + FonteAutonomiaHerbicida = + Convert.ToString( + reservatorio + .FonteAutonomiaOperacional + ) + ?? ""; + + PercentualReservatorio = + Clamp( + (double) + reservatorio.PercentualAtual, + 0, + 100 + ); + + double lPorMetroReferencia = + NumeroNaoNegativo( + (double) + reservatorio + .LPorMetroReferenciaOperacional + ); + + DadosHerbicidaValidos = + (bool)reservatorio.Iniciado && + (bool)reservatorio + .PulverizadorLiberado && + lPorMetroReferencia > 0 && + ValorFinito( + DistanciaSeguraHerbicida_m + ); + } + catch + { + DadosHerbicidaValidos = false; + } + } + + private void AtualizarAvisos(CoulombCounterService bateria, HerbicideCounterService reservatorio) + { + var avisos = + new List(); + + if (!DadosBateriaValidos) + { + avisos.Add( + "Autonomia de bateria indisponível ou inválida." + ); + } + else + { + try + { + if (!(bool)bateria.FontesCoerentes) + { + avisos.Add( + $"Daly e AB36V divergentes: " + + $"ΔV={(double)bateria.DiferencaTensaoFontes_V:F2} V, " + + $"ΔI={(double)bateria.DiferencaCorrenteFontes_A:F2} A." + ); + } + + if (!(bool)bateria.AutonomiaComHistorico) + { + avisos.Add( + $"Bateria ainda usando referência inicial: " + + $"{FonteAutonomiaBateria}." + ); + } + } + catch + { + // Campos diagnósticos opcionais. + } + } + + if ( + PulverizacaoExigida && + !DadosHerbicidaValidos + ) + { + avisos.Add( + "Autonomia operacional de herbicida indisponível ou inválida." + ); + } + else if (DadosHerbicidaValidos) + { + try + { + if ( + !(bool)reservatorio + .AutonomiaOperacionalComHistorico + ) + { + avisos.Add( + $"Herbicida ainda usando referência inicial: " + + $"{FonteAutonomiaHerbicida}." + ); + } + + if ( + !(bool)reservatorio + .LeituraReservatorioConfiavel + ) + { + avisos.Add( + "Leitura instantânea do reservatório rejeitada; " + + "mantendo último volume filtrado." + ); + } + } + catch + { + // Campos diagnósticos opcionais. + } + } + + Avisos = + avisos + .Where( + x => + !string.IsNullOrWhiteSpace(x) + ) + .Distinct() + .ToList(); + } + + // ============================================================ + // Latch e overrides + // ============================================================ + + private void AtualizarLatch(int idxCorredor, MotivoTrava criticosAtuais) + { + if (criticosAtuais == MotivoTrava.Nenhum) + { + _inicioCriticoUtc.Remove( + idxCorredor + ); + return; + } + + if ( + !_inicioCriticoUtc.ContainsKey( + idxCorredor + ) + ) + { + _inicioCriticoUtc[idxCorredor] = + DateTime.UtcNow; + } + + TimeSpan tempoCritico = + DateTime.UtcNow - + _inicioCriticoUtc[idxCorredor]; + + if ( + tempoCritico >= + _tempoParaTravar + ) + { + MotivoTrava travas = + ObterFlags( + _travasPorCorredor, + idxCorredor + ); + + _travasPorCorredor[idxCorredor] = + travas | + criticosAtuais; + } + } + + private void DestravarCorredor(int idxCorredor) + { + _travasPorCorredor.Remove( + idxCorredor + ); + + _inicioCriticoUtc.Remove( + idxCorredor + ); + } + + private static MotivoTrava ObterFlags(Dictionary origem, int idxCorredor) + { + return + origem.TryGetValue( + idxCorredor, + out MotivoTrava flags + ) + ? flags + : MotivoTrava.Nenhum; + } + + // ============================================================ + // Confirmação da entrada + // ============================================================ + + /* + * Chamar exatamente quando a trajetória efetivamente inicia a entrada + * no corredor. Antes disso a autonomia continua sendo reavaliada. + * + * Retorna false se o corredor não estiver liberado. + */ + public bool ConfirmarEntradaCorredor(int idxCorredor) + { + if ( + idxCorredor != + _ultimoIndiceAvaliado || + !Liberado || + !_historicoCorredores.TryGetValue( + idxCorredor, + out SnapshotCorredor snapshot + ) + ) + { + return false; + } + + snapshot.ConfirmadoParaEntrada = + true; + + _historicoCorredores[idxCorredor] = + snapshot; + + _corredoresConfirmados.Add( + idxCorredor + ); + + ConfirmadoParaEntrada = true; + + return true; + } + + public bool CorredorEstaConfirmado(int idxCorredor) + { + return + _corredoresConfirmados.Contains( + idxCorredor + ); + } + + // ============================================================ + // Mensagens + // ============================================================ + + private string MontarMotivoBateria() + { + if (!DadosBateriaValidos) + { + return + "Bateria sem autonomia válida para garantir " + + "a conclusão do corredor."; + } + + return + $"Bateria insuficiente: segura=" + + $"{DistanciaSeguraBateria_m:F1} m, " + + $"requerida={DistanciaRequeridaBateria_m:F1} m, " + + $"folga={FolgaBateria_m:F1} m, " + + $"SOC={PercentualBateria:F1}%, " + + $"fonte={FonteBateria}/{FonteAutonomiaBateria}."; + } + + private string MontarMotivoHerbicida() + { + if (!DadosHerbicidaValidos) + { + return + "Herbicida sem autonomia operacional válida " + + "para garantir a pulverização completa."; + } + + return + $"Herbicida insuficiente: seguro=" + + $"{DistanciaSeguraHerbicida_m:F1} m, " + + $"requerido={DistanciaRequeridaHerbicida_m:F1} m, " + + $"folga={FolgaHerbicida_m:F1} m, " + + $"reservatório={PercentualReservatorio:F1}%, " + + $"fonte={FonteAutonomiaHerbicida}."; + } + + private string MontarMotivoSupervisionado(int idxCorredor) + { + var partes = + new List(); + + if (BateriaOverrideAplicado) + { + partes.Add( + "bateria assumida pelo operador" + ); + } + + if (HerbicidaOverrideAplicado) + { + partes.Add( + "herbicida assumido pelo operador" + ); + } + + return + $"Corredor {idxCorredor + 1}: " + + "LIBERAÇÃO SUPERVISIONADA. " + + string.Join( + "; ", + partes + ) + + "."; + } + + // ============================================================ + // Snapshot e logs + // ============================================================ + + private SnapshotCorredor CriarSnapshot(int idxCorredor) + { + return + new SnapshotCorredor + { + Indice = + idxCorredor, + + Liberado = + Liberado, + + Status = + Status, + + Motivo = + Motivo, + + Motivos = + new List( + Motivos ?? + new List() + ), + + Avisos = + new List( + Avisos ?? + new List() + ), + + DistanciaCorredor_m = + DistanciaCorredor_m, + + DistanciaRequeridaBateria_m = + DistanciaRequeridaBateria_m, + + DistanciaRequeridaHerbicida_m = + DistanciaRequeridaHerbicida_m, + + DistanciaSeguraBateria_m = + DistanciaSeguraBateria_m, + + DistanciaSeguraHerbicida_m = + DistanciaSeguraHerbicida_m, + + FolgaBateria_m = + FolgaBateria_m, + + FolgaHerbicida_m = + FolgaHerbicida_m, + + DadosBateriaValidos = + DadosBateriaValidos, + + DadosHerbicidaValidos = + DadosHerbicidaValidos, + + BateriaSuficiente = + BateriaSuficiente, + + HerbicidaSuficiente = + HerbicidaSuficiente, + + BateriaOverrideAplicado = + BateriaOverrideAplicado, + + HerbicidaOverrideAplicado = + HerbicidaOverrideAplicado, + + FonteBateria = + FonteBateria, + + FonteAutonomiaBateria = + FonteAutonomiaBateria, + + FonteAutonomiaHerbicida = + FonteAutonomiaHerbicida, + + PercentualBateria = + PercentualBateria, + + PercentualReservatorio = + PercentualReservatorio, + + PulverizacaoExigida = + PulverizacaoExigida, + + ConfirmadoParaEntrada = + ConfirmadoParaEntrada, + + TimestampUtc = + DateTime.UtcNow, + }; + } + + private void AplicarSnapshot(SnapshotCorredor snapshot) + { + if (snapshot == null) + return; + + IndiceCorredor = + snapshot.Indice; + + Liberado = + snapshot.Liberado; + + Status = + snapshot.Status; + + Motivo = + snapshot.Motivo; + + Motivos = + new List( + snapshot.Motivos ?? + new List() + ); + + Avisos = + new List( + snapshot.Avisos ?? + new List() + ); + + DistanciaCorredor_m = + snapshot.DistanciaCorredor_m; + + DistanciaRequeridaBateria_m = + snapshot.DistanciaRequeridaBateria_m; + + DistanciaRequeridaHerbicida_m = + snapshot.DistanciaRequeridaHerbicida_m; + + DistanciaSeguraBateria_m = + snapshot.DistanciaSeguraBateria_m; + + DistanciaSeguraHerbicida_m = + snapshot.DistanciaSeguraHerbicida_m; + + FolgaBateria_m = + snapshot.FolgaBateria_m; + + FolgaHerbicida_m = + snapshot.FolgaHerbicida_m; + + DadosBateriaValidos = + snapshot.DadosBateriaValidos; + + DadosHerbicidaValidos = + snapshot.DadosHerbicidaValidos; + + BateriaSuficiente = + snapshot.BateriaSuficiente; + + HerbicidaSuficiente = + snapshot.HerbicidaSuficiente; + + BateriaOverrideAplicado = + snapshot.BateriaOverrideAplicado; + + HerbicidaOverrideAplicado = + snapshot.HerbicidaOverrideAplicado; + + FonteBateria = + snapshot.FonteBateria; + + FonteAutonomiaBateria = + snapshot.FonteAutonomiaBateria; + + FonteAutonomiaHerbicida = + snapshot.FonteAutonomiaHerbicida; + + PercentualBateria = + snapshot.PercentualBateria; + + PercentualReservatorio = + snapshot.PercentualReservatorio; + + PulverizacaoExigida = + snapshot.PulverizacaoExigida; + + ConfirmadoParaEntrada = + snapshot.ConfirmadoParaEntrada; + } + + private void LogarSeMudou(OperacaoModel op, SnapshotCorredor snapshot) + { + if ( + op == null || + snapshot == null + ) + { + return; + } + + string assinatura = + $"{snapshot.Status}|" + + $"{snapshot.Liberado}|" + + $"{snapshot.Motivo}"; + + if ( + _ultimaAssinaturaLog.TryGetValue( + snapshot.Indice, + out string anterior + ) && + string.Equals( + anterior, + assinatura, + StringComparison.Ordinal + ) + ) + { + return; + } + + _ultimaAssinaturaLog[snapshot.Indice] = + assinatura; + + try + { + op.Sensoriamento.InserirLog( + T_Code.Trj, + snapshot.Liberado + ? StatusModulo.Operante + : StatusModulo.Alerta, + snapshot.Liberado + ? 100 + : 0, + snapshot.Motivo + ); + } + catch + { + // Log nunca deve derrubar a decisão de autonomia. + } + } + + private void AplicarFalhaEstrutural(int idxCorredor, string motivo) + { + IndiceCorredor = + idxCorredor; + + Liberado = false; + Status = + AutonomiaCorredorStatus.Critico; + + Motivo = + motivo ?? "Falha estrutural na autonomia."; + + Motivos = + new List + { + Motivo + }; + + Avisos = + new List(); + + ConfirmadoParaEntrada = false; + + DistanciaCorredor_m = 0; + DistanciaRequeridaBateria_m = 0; + DistanciaRequeridaHerbicida_m = 0; + + DistanciaSeguraBateria_m = 0; + DistanciaSeguraHerbicida_m = 0; + + FolgaBateria_m = 0; + FolgaHerbicida_m = 0; + + DadosBateriaValidos = false; + DadosHerbicidaValidos = false; + + BateriaSuficiente = false; + HerbicidaSuficiente = false; + } + + // ============================================================ + // Reinício e clone + // ============================================================ + public void Reiniciar() { Iniciado = false; + + Status = + AutonomiaCorredorStatus.Desconhecido; + Liberado = false; - Motivos = new List(); - Motivo = ""; - Status = AutonomiaCorredorStatus.Desconhecido; + + IndiceCorredor = -1; + DistanciaCorredor_m = 0; + DistanciaRequeridaBateria_m = 0; + DistanciaRequeridaHerbicida_m = 0; + DistanciaSeguraBateria_m = 0; DistanciaSeguraHerbicida_m = 0; - CorredoresLiberados.Clear(); - _corredoresTravados.Clear(); + + FolgaBateria_m = 0; + FolgaHerbicida_m = 0; + + DadosBateriaValidos = false; + DadosHerbicidaValidos = false; + + BateriaSuficiente = false; + HerbicidaSuficiente = false; + + PulverizacaoExigida = false; + + BateriaOverrideAplicado = false; + HerbicidaOverrideAplicado = false; + + FonteBateria = ""; + FonteAutonomiaBateria = ""; + FonteAutonomiaHerbicida = ""; + + PercentualBateria = 0; + PercentualReservatorio = 0; + + ConfirmadoParaEntrada = false; + + Motivo = ""; + Motivos = new List(); + Avisos = new List(); + + _historicoCorredores.Clear(); + _corredoresConfirmados.Clear(); + + _travasPorCorredor.Clear(); _inicioCriticoUtc.Clear(); + _overridesPorCorredor.Clear(); + + _ultimaAssinaturaLog.Clear(); + + _ultimoSinalLiberacaoBateria = false; + _ultimoSinalLiberacaoReservatorio = false; + + _ultimoIndiceAvaliado = -1; } - // ========================== - // Clone (mantido) - // ========================== public AutonomiaCorredorModel Clone() { - return new AutonomiaCorredorModel() - { - Iniciado = Iniciado, - Liberado = Liberado, - Motivo = Motivo, - Motivos = new List(Motivos ?? new List()), - Status = Status, - DistanciaCorredor_m = DistanciaCorredor_m, - DistanciaSeguraBateria_m = DistanciaSeguraBateria_m, - DistanciaSeguraHerbicida_m = DistanciaSeguraHerbicida_m, - }; + return + new AutonomiaCorredorModel( + tempoParaTravar_s: + TempoParaTravar_s, + + distanciaExtraBateria_m: + DistanciaExtraBateria_m, + + distanciaExtraHerbicida_m: + DistanciaExtraHerbicida_m, + + toleranciaComparacao_m: + ToleranciaComparacao_m + ) + { + Iniciado = + Iniciado, + + Status = + Status, + + Liberado = + Liberado, + + IndiceCorredor = + IndiceCorredor, + + DistanciaCorredor_m = + DistanciaCorredor_m, + + DistanciaRequeridaBateria_m = + DistanciaRequeridaBateria_m, + + DistanciaRequeridaHerbicida_m = + DistanciaRequeridaHerbicida_m, + + DistanciaSeguraBateria_m = + DistanciaSeguraBateria_m, + + DistanciaSeguraHerbicida_m = + DistanciaSeguraHerbicida_m, + + FolgaBateria_m = + FolgaBateria_m, + + FolgaHerbicida_m = + FolgaHerbicida_m, + + DadosBateriaValidos = + DadosBateriaValidos, + + DadosHerbicidaValidos = + DadosHerbicidaValidos, + + BateriaSuficiente = + BateriaSuficiente, + + HerbicidaSuficiente = + HerbicidaSuficiente, + + PulverizacaoExigida = + PulverizacaoExigida, + + BateriaOverrideAplicado = + BateriaOverrideAplicado, + + HerbicidaOverrideAplicado = + HerbicidaOverrideAplicado, + + FonteBateria = + FonteBateria, + + FonteAutonomiaBateria = + FonteAutonomiaBateria, + + FonteAutonomiaHerbicida = + FonteAutonomiaHerbicida, + + PercentualBateria = + PercentualBateria, + + PercentualReservatorio = + PercentualReservatorio, + + ConfirmadoParaEntrada = + ConfirmadoParaEntrada, + + Motivo = + Motivo, + + Motivos = + new List( + Motivos ?? + new List() + ), + + Avisos = + new List( + Avisos ?? + new List() + ), + }; + } + + // ============================================================ + // Helpers + // ============================================================ + + private static double NumeroNaoNegativo(double valor) + { + if (!ValorFinito(valor)) + return 0; + + return Math.Max(0, valor); + } + + private static bool ValorFinito(double valor) + { + return + !double.IsNaN(valor) && + !double.IsInfinity(valor); + } + + private static double Clamp(double valor, double minimo, double maximo) + { + return Math.Max(minimo, Math.Min(maximo, valor)); } } diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index 83682402c..20e58464d 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -27,7 +27,7 @@ namespace AgroBase.Models { public class Variaveis { - public static readonly bool IniciarWorkers = false; + public static readonly bool IniciarWorkers = true; public static readonly bool Producao = false; public static bool DebugMode { get; set; } = true; public static bool Fechando { get; set; } = false; @@ -873,7 +873,7 @@ namespace AgroBase.Models } public static double TensaoMinimaBateria { get; set; } = 30.0; public static double TensaoMaximaBateria { get; set; } = 42.0; - public static double CorrenteMaximaBateria { get; set; } = 20.0; + public static double CorrenteMaximaBateria { get; set; } = 100.0; public static double PercentualTensaoBateriaMin { get; set; } = 25.0; public static double PercentualReservatorioMin { get; set; } = 8.0; public static double PercentualReservatorioMinCritio { get; set; } = 5.0; @@ -962,8 +962,8 @@ namespace AgroBase.Models { TipoMovimentoDirecional.MovimentoLateral, 90 }, { TipoMovimentoDirecional.Diagnostico, 25 }, }; - public static double AnguloInclinacaoRollMax { get; set; } = 45.0; // Frontal - public static double AnguloInclinacaoPitchMax { get; set; } = 25.0; // Lateral + public static double AnguloInclinacaoRollMax { get; set; } = 30.0; // Frontal 40 graus max + public static double AnguloInclinacaoPitchMax { get; set; } = 22.0; // Lateral public static string VozAlerta { get; set; } = "Microsoft Maria Desktop"; diff --git a/AgroBase/AgroBase/Services/CoulombCounterService.cs b/AgroBase/AgroBase/Services/CoulombCounterService.cs index 4e7340ca8..8bbdbe9a7 100644 --- a/AgroBase/AgroBase/Services/CoulombCounterService.cs +++ b/AgroBase/AgroBase/Services/CoulombCounterService.cs @@ -8,31 +8,164 @@ namespace AgroBase.Services { public class CoulombCounterService { - public CoulombCounterService(double max_Ah, double wh_m_base = 0.1167) + /* + * Convenções: + * + * - Corrente líquida positiva = descarga. + * - Corrente líquida negativa = carga. + * - Energia consumida = descarga bruta integrada. + * - SOC integrado = carga líquida restante, considerando + * descarga e eventual recarga. + * + * A autonomia usada pelo corredor considera: + * - energia útil acima da reserva mínima; + * - consumo operacional por metro, incluindo energia gasta parado; + * - margem de segurança; + * - a fonte mais conservadora entre SOC do BMS e coulomb counting. + */ + + private const double CorrenteZonaMorta_A = 0.05; + private const double PotenciaMinimaTempo_W = 1.0; + + private const double MinDistanciaTaxa_m = 0.50; + private const double MinDistanciaHistorico_m = 20.0; + + private const double JanelaAutonomia_m = 150.0; + private const double JanelaAutonomiaTempo_s = 300.0; + + private const double AlphaHistorico = 0.20; + private const double FatorConservadorHistorico = 1.10; + + private const double DiferencaTensaoAlerta_V = 2.0; + private const double DiferencaCorrenteMinAlerta_A = 3.0; + private const double DiferencaCorrenteRelativaAlerta = 0.35; + + private const double DtMaximoIntegracao_s = 5.0; + + private readonly double _timeoutBms_s; + private readonly double _timeoutSensor_s; + + /* + * Sinal que transforma a corrente bruta em: + * positivo = descarga; negativo = carga. + * + * Daly normalmente envia descarga negativa. + * AB36V normalmente envia descarga positiva. + */ + private readonly double _dalyCorrenteDescargaSinal; + private readonly double _sensorCorrenteDescargaSinal; + + private class FonteEnergiaSnapshot { - CapacidadeMaxima_Ah = max_Ah; - CargaMaxima_C = max_Ah * 3600.0; - WhPorMetroBase = wh_m_base; + public string Nome { get; set; } = ""; + public bool Inicializada { get; set; } + public bool Valida { get; set; } + public string MotivoInvalida { get; set; } = ""; + + public DateTime? Timestamp { get; set; } + public double Idade_s { get; set; } + + public double Tensao_V { get; set; } + public double CorrenteBruta_A { get; set; } + public double CorrenteLiquida_A { get; set; } + + public double Soc_percent { get; set; } + public bool SocValido { get; set; } + + public double Temperatura_C { get; set; } + } + + public CoulombCounterService( + double max_Ah, + double wh_m_base = 0.30, + double tensao_nominal_V = 36.0, + double daly_corrente_descarga_sinal = -1.0, + double sensor_corrente_descarga_sinal = 1.0, + double timeout_bms_s = 3.0, + double timeout_sensor_s = 3.0) + { + CapacidadeMaxima_Ah = Math.Max(0, max_Ah); + CargaMaxima_C = CapacidadeMaxima_Ah * 3600.0; + + TensaoNominal_V = Math.Max(1.0, tensao_nominal_V); + EnergiaMaximaNominal_Wh = + CapacidadeMaxima_Ah * TensaoNominal_V; + + /* + * Na operação analisada, o consumo real ficou aproximadamente + * entre 0,18 e 0,29 Wh/m. O fallback de 0,30 Wh/m começa + * conservador e depois é substituído pelo histórico operacional. + */ + WhPorMetroBase = Math.Max(0, wh_m_base); + + _dalyCorrenteDescargaSinal = + NormalizarSinal(daly_corrente_descarga_sinal, -1.0); + + _sensorCorrenteDescargaSinal = + NormalizarSinal(sensor_corrente_descarga_sinal, 1.0); + + _timeoutBms_s = Math.Max(0.5, timeout_bms_s); + _timeoutSensor_s = Math.Max(0.5, timeout_sensor_s); MargemSegurancaAutonomia = 0.30; ResetarTudo(); } + // ============================================================ + // Estado geral + // ============================================================ + public bool Iniciado { get; private set; } + + /* + * Indica que a fonte ativa escolhida neste ciclo é o Daly. + * Não significa apenas que o serviço Daly foi inicializado. + */ public bool BmsLigado { get; private set; } + + public bool BmsDisponivel { get; private set; } + public bool SensorAb36VDisponivel { get; private set; } + + public string FonteAtiva { get; private set; } = "nenhuma"; + public string MotivoFonteAtiva { get; private set; } = ""; + public bool OperacaoAtiva { get; private set; } public bool OperacaoFinalizada { get; private set; } private bool _estadoInicializado; + private bool _inicioOperacaoPendente; + private DateTime? _ultimaLeituraIntegrada; + private string _ultimaFonteIntegrada = ""; + private string _ultimaFonteLogada = ""; public double CapacidadeMaxima_Ah { get; private set; } private double CargaMaxima_C { get; set; } + public double TensaoNominal_V { get; } + public double EnergiaMaximaNominal_Wh { get; } + + private double _cargaIntegradaAtual_C; + public double CargaAtual_C { get; private set; } public double CargaAtualAmp => CargaAtual_C / 3600.0; + public double SocIntegrado_percent + { + get + { + if (CargaMaxima_C <= 0) + return 0; + + return Clamp( + _cargaIntegradaAtual_C / CargaMaxima_C * 100.0, + 0, + 100 + ); + } + } + private double _socInicialInstancia; private double _socInicialOperacao; @@ -41,17 +174,44 @@ namespace AgroBase.Services private double _consumidoIntegradoTotal_C; private double _consumidoIntegradoOperacao_C; - public double ConsumidoTotal_Ah => ConsumidoTotal_C / 3600.0; - public double ConsumidoOperacao_Ah => ConsumidoOperacao_C / 3600.0; + private double _carregadoIntegradoTotal_C; + private double _carregadoIntegradoOperacao_C; + public double ConsumidoIntegradoTotal_Ah => + _consumidoIntegradoTotal_C / 3600.0; + + public double ConsumidoIntegradoOperacao_Ah => + _consumidoIntegradoOperacao_C / 3600.0; + + public double CarregadoIntegradoTotal_Ah => + _carregadoIntegradoTotal_C / 3600.0; + + public double CarregadoIntegradoOperacao_Ah => + _carregadoIntegradoOperacao_C / 3600.0; + + /* + * Para os campos legados de consumo, usa o maior entre: + * - queda do SOC conservador; + * - descarga integrada. + * + * Isso evita subestimar o consumo se uma das fontes ficar otimista. + */ public double ConsumidoTotal_C { get { - if (BmsLigado) - return Math.Max(0, (_socInicialInstancia - PercentualBateria) / 100.0 * CargaMaxima_C); + double porSoc = + Math.Max( + 0, + (_socInicialInstancia - PercentualAtual) / + 100.0 * + CargaMaxima_C + ); - return _consumidoIntegradoTotal_C; + return Math.Max( + porSoc, + _consumidoIntegradoTotal_C + ); } } @@ -62,23 +222,41 @@ namespace AgroBase.Services if (!OperacaoAtiva && !OperacaoFinalizada) return 0; - if (BmsLigado) - return Math.Max(0, (_socInicialOperacao - PercentualBateria) / 100.0 * CargaMaxima_C); + double porSoc = + Math.Max( + 0, + (_socInicialOperacao - PercentualAtual) / + 100.0 * + CargaMaxima_C + ); - return _consumidoIntegradoOperacao_C; + return Math.Max( + porSoc, + _consumidoIntegradoOperacao_C + ); } } + public double ConsumidoTotal_Ah => + ConsumidoTotal_C / 3600.0; + + public double ConsumidoOperacao_Ah => + ConsumidoOperacao_C / 3600.0; + public double PercentualConsumidoTotal { get { - if (CargaMaxima_C <= 0) return 0; + if (CargaMaxima_C <= 0) + return 0; - if (BmsLigado) - return Clamp(_socInicialInstancia - PercentualBateria, 0, 100); - - return Clamp((_consumidoIntegradoTotal_C / CargaMaxima_C) * 100.0, 0, 100); + return Clamp( + ConsumidoTotal_C / + CargaMaxima_C * + 100.0, + 0, + 100 + ); } } @@ -86,15 +264,21 @@ namespace AgroBase.Services { get { - if (!OperacaoAtiva && !OperacaoFinalizada) + if ( + (!OperacaoAtiva && !OperacaoFinalizada) || + CargaMaxima_C <= 0 + ) + { return 0; + } - if (CargaMaxima_C <= 0) return 0; - - if (BmsLigado) - return Clamp(_socInicialOperacao - PercentualBateria, 0, 100); - - return Clamp((_consumidoIntegradoOperacao_C / CargaMaxima_C) * 100.0, 0, 100); + return Clamp( + ConsumidoOperacao_C / + CargaMaxima_C * + 100.0, + 0, + 100 + ); } } @@ -102,10 +286,16 @@ namespace AgroBase.Services { get { - if (_socInicialOperacao <= 0) return 0; + if (_socInicialOperacao <= 0) + return 0; - double consumido = PercentualConsumidoOperacao; - return Clamp((consumido / _socInicialOperacao) * 100.0, 0, 100); + return Clamp( + PercentualConsumidoOperacao / + _socInicialOperacao * + 100.0, + 0, + 100 + ); } } @@ -113,8 +303,16 @@ namespace AgroBase.Services { get { - if (CargaMaxima_C <= 0) return 0; - return Clamp((_consumidoIntegradoTotal_C / CargaMaxima_C) * 100.0, 0, 100); + if (CargaMaxima_C <= 0) + return 0; + + return Clamp( + _consumidoIntegradoTotal_C / + CargaMaxima_C * + 100.0, + 0, + 100 + ); } } @@ -122,45 +320,94 @@ namespace AgroBase.Services { get { - if (CargaMaxima_C <= 0) return 0; - return Clamp((_consumidoIntegradoOperacao_C / CargaMaxima_C) * 100.0, 0, 100); + if (CargaMaxima_C <= 0) + return 0; + + return Clamp( + _consumidoIntegradoOperacao_C / + CargaMaxima_C * + 100.0, + 0, + 100 + ); } } - public double DiferencaSocBmsVsIntegrado_Total - { - get - { - return PercentualConsumidoTotalIntegrado - PercentualConsumidoTotal; - } - } + /* + * Positivo: o integrador contabilizou mais consumo que a queda + * observada no SOC do Daly. + */ + public double DiferencaSocBmsVsIntegrado_Total { get; private set; } + public double DiferencaSocBmsVsIntegrado_Operacao { get; private set; } - public double PercentualAtual - { - get - { - if (BmsLigado) - return PercentualBateria; - - if (CargaMaxima_C <= 0) return 0; - return Clamp((CargaAtual_C / CargaMaxima_C) * 100.0, 0, 100); - } - } + // ============================================================ + // Leituras ativas + // ============================================================ + /* + * SOC bruto da fonte selecionada: + * - Daly, quando válido; + * - estimativa por tensão do AB36V, quando o Daly está indisponível. + */ public double PercentualBateria { get; private set; } + + /* + * SOC conservador usado pela autonomia: + * - mínimo entre Daly e integrador quando o Daly é válido; + * - integrador quando apenas o AB36V está disponível. + */ + public double PercentualAtual { get; private set; } + public double Temperatura { get; private set; } + public double CorrenteInstantanea { get; private set; } + public double CorrenteLiquida_A { get; private set; } + public double CorrenteDescarga_A { get; private set; } + public double CorrenteCarga_A { get; private set; } + public double TensaoInstantanea { get; private set; } + public double PotenciaInstantanea { get; private set; } public double PotenciaDescarga_W { get; private set; } + public double PotenciaCarga_W { get; private set; } + public double TempoEstimadoMinutos { get; private set; } + + /* + * Mantido por compatibilidade com o contrato existente. + */ public double _dt { get; private set; } + // ============================================================ + // Redundância BMS x AB36V + // ============================================================ + + public double BmsTensao_V { get; private set; } + public double BmsCorrenteLiquida_A { get; private set; } + public double BmsSoc_percent { get; private set; } + + public double SensorTensao_V { get; private set; } + public double SensorCorrenteLiquida_A { get; private set; } + public double SensorSocEstimado_percent { get; private set; } + + public double DiferencaTensaoFontes_V { get; private set; } + public double DiferencaCorrenteFontes_A { get; private set; } + public bool FontesCoerentes { get; private set; } + + // ============================================================ + // Energia + // ============================================================ + public double EnergiaConsumidaTotal_Wh { get; private set; } + public double EnergiaCarregadaTotal_Wh { get; private set; } + private double _energiaConsumidaInicioOperacao_Wh; private double _energiaConsumidaFimOperacao_Wh; + private double _energiaCarregadaInicioOperacao_Wh; + private double _energiaCarregadaFimOperacao_Wh; + public double EnergiaConsumidaOperacao_Wh { get @@ -168,312 +415,1301 @@ namespace AgroBase.Services if (!OperacaoAtiva && !OperacaoFinalizada) return 0; - double fim = OperacaoFinalizada - ? _energiaConsumidaFimOperacao_Wh - : EnergiaConsumidaTotal_Wh; + double fim = + OperacaoFinalizada + ? _energiaConsumidaFimOperacao_Wh + : EnergiaConsumidaTotal_Wh; - return Math.Max(0, fim - _energiaConsumidaInicioOperacao_Wh); + return Math.Max( + 0, + fim - + _energiaConsumidaInicioOperacao_Wh + ); } } - public double EnergiaConsumidaTotal_kWh => EnergiaConsumidaTotal_Wh / 1000.0; - public double EnergiaConsumidaOperacao_kWh => EnergiaConsumidaOperacao_Wh / 1000.0; - - public double WhRestante + public double EnergiaCarregadaOperacao_Wh { get { - double tensaoReferencia = TensaoInstantanea > 0 - ? TensaoInstantanea - : VariaveisEquipamento.TensaoMaximaBateria; + if (!OperacaoAtiva && !OperacaoFinalizada) + return 0; - return CargaAtualAmp * tensaoReferencia; + double fim = + OperacaoFinalizada + ? _energiaCarregadaFimOperacao_Wh + : EnergiaCarregadaTotal_Wh; + + return Math.Max( + 0, + fim - + _energiaCarregadaInicioOperacao_Wh + ); } } + public double EnergiaConsumidaTotal_kWh => + EnergiaConsumidaTotal_Wh / 1000.0; + + public double EnergiaConsumidaOperacao_kWh => + EnergiaConsumidaOperacao_Wh / 1000.0; + + public double WhRestanteBms { get; private set; } + public double WhRestanteIntegrado { get; private set; } + + /* + * Energia restante conservadora usada pela autonomia. + */ + public double WhRestante { get; private set; } public double kWhRestante => WhRestante / 1000.0; - private const int BufferSize = 30; - private readonly Queue _historicoCorrenteDescarga = new Queue(); - private readonly Queue _historicoPotenciaDescarga = new Queue(); + public double PercentualReservaAutonomia + { + get + { + return Clamp( + VariaveisEquipamento + .PercentualTensaoBateriaMin, + 0, + 90 + ); + } + } + public double WhReservaMinima => + EnergiaMaximaNominal_Wh * + PercentualReservaAutonomia / + 100.0; + + public double WhUtilRestante => + Math.Max( + 0, + WhRestante - + WhReservaMinima + ); + + // ============================================================ + // Tempo restante + // ============================================================ + + private const int BufferPotenciaSize = 60; + + private readonly Queue + _historicoPotenciaDescarga = + new Queue(); + + // ============================================================ + // Consumo por metro e autonomia + // ============================================================ + + private readonly Queue<( + double dist_m, + double tempo_s, + double energia_Wh)> + _janelaAutonomia = + new Queue<( + double dist_m, + double tempo_s, + double energia_Wh)>(); + + private double _somaDistanciaJanela_m; + private double _somaTempoJanela_s; + private double _somaEnergiaJanela_Wh; + + private double _distanciaTotalMetrosAnterior; + + public double WhPorMetroJanela { get; private set; } + + /* + * Mantido por compatibilidade. + * Representa a referência efetivamente usada na autonomia. + */ public double WhPorMetro { get; private set; } public double kWhPorMetro => WhPorMetro / 1000.0; + public double WhPorMetroBase { get; set; } public double WhPorMetroHistorico { get; private set; } - public double WhPorMetroReferencia - { - get - { - if (WhPorMetro > 0) return WhPorMetro; - if (WhPorMetroHistorico > 0) return WhPorMetroHistorico; - return WhPorMetroBase; - } - } + public double WhPorMetroReferencia { get; private set; } + + public bool AutonomiaComHistorico { get; private set; } + public string FonteAutonomia { get; private set; } = + "fallback_campo"; public double DistanciaMaximaTeorica_m { get; private set; } public double DistanciaMaximaSegura_m { get; private set; } public double DistanciaEstimadaRestante_m { get; private set; } + public double MargemSegurancaAutonomia { get; set; } - private const double JanelaAutonomia_m = 50.0; - private readonly Queue _historicoDeltaDist_m = new Queue(); - private readonly Queue _historicoDeltaWh = new Queue(); - private double _somaDistanciaJanela_m; - private double _somaEnergiaJanela_Wh; - private double _distanciaTotalMetrosAnterior; - private double _energiaConsumidaAnterior_Wh_Total; + // ============================================================ + // Ciclo de vida da operação + // ============================================================ - public void IniciarOperacao(double distanciaTotalMetrosAtual = 0) + public void IniciarOperacao( + double distanciaTotalMetrosAtual = 0) { if (!_estadoInicializado) - return; + { + _inicioOperacaoPendente = true; + OperacaoAtiva = true; + OperacaoFinalizada = false; - _socInicialOperacao = PercentualBateria; - _distanciaInicioOperacao_m = distanciaTotalMetrosAtual; + _distanciaInicioOperacao_m = + distanciaTotalMetrosAtual; + + return; + } + + PrepararBaselineOperacao( + distanciaTotalMetrosAtual + ); + } + + private void PrepararBaselineOperacao( + double distanciaTotalMetrosAtual) + { + _socInicialOperacao = + PercentualAtual; + + _distanciaInicioOperacao_m = + distanciaTotalMetrosAtual; _consumidoIntegradoOperacao_C = 0; - _energiaConsumidaInicioOperacao_Wh = EnergiaConsumidaTotal_Wh; + _carregadoIntegradoOperacao_C = 0; + + _energiaConsumidaInicioOperacao_Wh = + EnergiaConsumidaTotal_Wh; + _energiaConsumidaFimOperacao_Wh = 0; + _energiaCarregadaInicioOperacao_Wh = + EnergiaCarregadaTotal_Wh; + + _energiaCarregadaFimOperacao_Wh = 0; + OperacaoAtiva = true; OperacaoFinalizada = false; + _inicioOperacaoPendente = false; + + ResetarHistoricoAutonomia( + distanciaTotalMetrosAtual + ); } public void FinalizarOperacao() { - if (!OperacaoAtiva) + if (!OperacaoAtiva && !_inicioOperacaoPendente) return; - _energiaConsumidaFimOperacao_Wh = EnergiaConsumidaTotal_Wh; + _energiaConsumidaFimOperacao_Wh = + EnergiaConsumidaTotal_Wh; + + _energiaCarregadaFimOperacao_Wh = + EnergiaCarregadaTotal_Wh; OperacaoAtiva = false; OperacaoFinalizada = true; + _inicioOperacaoPendente = false; } - public void AtualizarConsumo(double distanciaTotalMetrosAtual) + // ============================================================ + // Atualização principal + // ============================================================ + + public void AtualizarConsumo( + double distanciaTotalMetrosAtual) { - LerFontes(); + FonteEnergiaSnapshot bms; + FonteEnergiaSnapshot sensor; - if (!Iniciado) + LerFontes( + out bms, + out sensor + ); + + AtualizarDiagnosticoRedundancia( + bms, + sensor + ); + + FonteEnergiaSnapshot ativa = + EscolherFonteAtiva( + bms, + sensor + ); + + AplicarFonteAtiva(ativa); + + if (!Iniciado || ativa == null) + { + MarcarAutonomiaIndisponivel(); return; + } - DateTime? leituraAtual = ObterDataLeituraAtual(); + DateTime? leituraAtual = + ativa.Timestamp; - if (!leituraAtual.HasValue || leituraAtual.Value == DateTime.MinValue) + if ( + !leituraAtual.HasValue || + leituraAtual.Value == + DateTime.MinValue + ) + { + MarcarAutonomiaIndisponivel(); return; + } if (!_estadoInicializado) { - InicializarEstado(leituraAtual.Value, distanciaTotalMetrosAtual); + InicializarEstado( + leituraAtual.Value, + distanciaTotalMetrosAtual + ); + + if (_inicioOperacaoPendente) + { + PrepararBaselineOperacao( + distanciaTotalMetrosAtual + ); + } + return; } - if (_ultimaLeituraIntegrada.HasValue && leituraAtual.Value <= _ultimaLeituraIntegrada.Value) + /* + * Ao trocar de Daly para AB36V, ou vice-versa, não integra + * o intervalo entre relógios/fontes diferentes. + */ + if ( + !string.Equals( + _ultimaFonteIntegrada, + FonteAtiva, + StringComparison.OrdinalIgnoreCase + ) + ) + { + _ultimaFonteIntegrada = + FonteAtiva; + + _ultimaLeituraIntegrada = + leituraAtual.Value; + + _dt = 0; + + AtualizarEstadoEnergia(); + AtualizarAutonomiaSemIntegrar( + distanciaTotalMetrosAtual + ); + + return; + } + + if ( + _ultimaLeituraIntegrada.HasValue && + leituraAtual.Value <= + _ultimaLeituraIntegrada.Value + ) { _dt = 0; - AtualizarAutonomia(distanciaTotalMetrosAtual); + + AtualizarEstadoEnergia(); + AtualizarAutonomiaSemIntegrar( + distanciaTotalMetrosAtual + ); + return; } - _dt = (leituraAtual.Value - _ultimaLeituraIntegrada.Value).TotalSeconds; - _ultimaLeituraIntegrada = leituraAtual.Value; + _dt = + ( + leituraAtual.Value - + _ultimaLeituraIntegrada.Value + ).TotalSeconds; - if (_dt <= 0 || _dt > 10) + _ultimaLeituraIntegrada = + leituraAtual.Value; + + if ( + _dt <= 0 || + _dt > DtMaximoIntegracao_s + ) { - Variaveis.MostrarLog($"[COULOMB COUNTER] dt inválido: {_dt:F3}s"); - AtualizarCargaAtualPeloSocOuIntegrador(); - AtualizarAutonomia(distanciaTotalMetrosAtual); + Variaveis.MostrarLog( + $"[COULOMB COUNTER] dt inválido: " + + $"{_dt:F3}s | fonte={FonteAtiva}" + ); + + AtualizarEstadoEnergia(); + AtualizarAutonomiaSemIntegrar( + distanciaTotalMetrosAtual + ); + return; } - CorrenteDescarga_A = NormalizarCorrenteDescarga(CorrenteInstantanea); - PotenciaDescarga_W = Math.Max(0, TensaoInstantanea * CorrenteDescarga_A); + IntegrarEnergiaECarga( + _dt + ); - double deltaCoulombs = CorrenteDescarga_A * _dt; - double deltaEnergia_Wh = PotenciaDescarga_W * _dt / 3600.0; + AtualizarEstadoEnergia(); - _consumidoIntegradoTotal_C = Clamp(_consumidoIntegradoTotal_C + deltaCoulombs, 0, CargaMaxima_C); + AtualizarHistoricoPotencia(); - if (OperacaoAtiva && !OperacaoFinalizada) - _consumidoIntegradoOperacao_C = Clamp(_consumidoIntegradoOperacao_C + deltaCoulombs, 0, CargaMaxima_C); - - if (deltaEnergia_Wh > 0) - EnergiaConsumidaTotal_Wh += deltaEnergia_Wh; - - AtualizarCargaAtualPeloSocOuIntegrador(); - AtualizarHistoricos(); AtualizarTempoRestante(); - AtualizarAutonomia(distanciaTotalMetrosAtual); + + AtualizarAutonomia( + distanciaTotalMetrosAtual, + _dt + ); + + AtualizarDiferencasSoc(); } - private void LerFontes() + // ============================================================ + // Fontes + // ============================================================ + + private void LerFontes( + out FonteEnergiaSnapshot bms, + out FonteEnergiaSnapshot sensor) { - var dalyBms = DalyBMSService.DadosLeitura; + bms = LerDaly(); + sensor = LerSensorAb36V(); - bool bms_iniciado = dalyBms?.Iniciado ?? false; - DateTime? bms_ultima_leitura = dalyBms?.UltimoComandoRecebido; - double bms_tensao = dalyBms?.Pack?.TensaoTotal_V ?? 0; - double bms_corrente = dalyBms?.Pack?.Corrente_A ?? 0; - double bms_soc = dalyBms?.Pack?.SOC_percent ?? 0; - double bms_tmp = dalyBms?.TemperaturaMax ?? 0; + BmsDisponivel = + bms?.Valida ?? false; - var sensor = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores? - .FirstOrDefault(x => x.ID == "AB36V"); + SensorAb36VDisponivel = + sensor?.Valida ?? false; - bool sen_iniciado = sensor?.Inicializado ?? false; - DateTime? sen_ultima_leitura = sensor?.UltimaLeitura; - double sen_tensao = sensor?.ValoresLeituras? - .FirstOrDefault(x => x.funcao == FuncoesPinout.Tensao)?.atual?.valor ?? 0; + BmsTensao_V = + bms?.Tensao_V ?? 0; - double sen_corrente = (sensor?.ValoresLeituras? - .FirstOrDefault(x => x.funcao == FuncoesPinout.Corrente)?.atual?.valor ?? 0) / 1000.0; + BmsCorrenteLiquida_A = + bms?.CorrenteLiquida_A ?? 0; - double sen_soc = Math.Round( - FuncoesMatematicas.Map( - sen_tensao, - VariaveisEquipamento.TensaoMinimaBateria, - VariaveisEquipamento.TensaoMaximaBateria, - 0.0, - 100.0), 2); + BmsSoc_percent = + bms?.Soc_percent ?? 0; - double sen_tmp = sensor?.ValoresLeituras? - .FirstOrDefault(x => x.funcao == FuncoesPinout.Temperatura)?.atual?.valor ?? 0; + SensorTensao_V = + sensor?.Tensao_V ?? 0; - Iniciado = bms_iniciado || sen_iniciado; - BmsLigado = bms_iniciado; + SensorCorrenteLiquida_A = + sensor?.CorrenteLiquida_A ?? 0; - TensaoInstantanea = BmsLigado ? bms_tensao : sen_tensao; - CorrenteInstantanea = BmsLigado ? bms_corrente : sen_corrente; - PotenciaInstantanea = TensaoInstantanea * CorrenteInstantanea; - PercentualBateria = Clamp(BmsLigado ? bms_soc : sen_soc, 0, 100); - Temperatura = BmsLigado ? bms_tmp : sen_tmp; + SensorSocEstimado_percent = + sensor?.Soc_percent ?? 0; } - private DateTime? ObterDataLeituraAtual() + private FonteEnergiaSnapshot LerDaly() { - var dalyBms = DalyBMSService.DadosLeitura; + var dados = + DalyBMSService.DadosLeitura; - if (BmsLigado) - return dalyBms?.UltimoComandoRecebido; + var snapshot = + new FonteEnergiaSnapshot + { + Nome = "DalyBMS", + Inicializada = + dados?.Iniciado ?? false, - var sensor = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores? - .FirstOrDefault(x => x.ID == "AB36V"); + Timestamp = + dados?.UltimoComandoRecebido, - return sensor?.UltimaLeitura; + Tensao_V = + dados?.Pack?.TensaoTotal_V ?? 0, + + CorrenteBruta_A = + dados?.Pack?.Corrente_A ?? 0, + + Soc_percent = + dados?.Pack?.SOC_percent ?? 0, + + Temperatura_C = + dados?.TemperaturaMax ?? 0, + }; + + snapshot.CorrenteLiquida_A = + NormalizarCorrenteLiquida( + snapshot.CorrenteBruta_A, + _dalyCorrenteDescargaSinal + ); + + snapshot.SocValido = + ValorFinito(snapshot.Soc_percent) && + snapshot.Soc_percent >= 0 && + snapshot.Soc_percent <= 100; + + ValidarFonte( + snapshot, + _timeoutBms_s, + exigirSoc: true + ); + + return snapshot; } - private void InicializarEstado(DateTime leituraAtual, double distanciaTotalMetrosAtual) + private FonteEnergiaSnapshot LerSensorAb36V() { - _socInicialInstancia = PercentualBateria; + var sensor = + Variaveis + .OperacaoEmAndamento? + .DispSen? + .Dados? + .Sensores? + .FirstOrDefault( + x => x.ID == "AB36V" + ); - AtualizarCargaAtualPeloSocOuIntegrador(); + double tensao = + sensor? + .ValoresLeituras? + .FirstOrDefault( + x => + x.funcao == + FuncoesPinout.Tensao + )? + .atual? + .valor + ?? 0; + + double correnteBruta_A = + ( + sensor? + .ValoresLeituras? + .FirstOrDefault( + x => + x.funcao == + FuncoesPinout.Corrente + )? + .atual? + .valor + ?? 0 + ) / + 1000.0; + + double temperatura = + sensor? + .ValoresLeituras? + .FirstOrDefault( + x => + x.funcao == + FuncoesPinout.Temperatura + )? + .atual? + .valor + ?? 0; + + double socEstimado = + CalcularSocPorTensao( + tensao + ); + + var snapshot = + new FonteEnergiaSnapshot + { + Nome = "AB36V", + Inicializada = + sensor?.Inicializado ?? false, + + Timestamp = + sensor?.UltimaLeitura, + + Tensao_V = + tensao, + + CorrenteBruta_A = + correnteBruta_A, + + CorrenteLiquida_A = + NormalizarCorrenteLiquida( + correnteBruta_A, + _sensorCorrenteDescargaSinal + ), + + Soc_percent = + socEstimado, + + SocValido = + ValorFinito(socEstimado), + + Temperatura_C = + temperatura, + }; + + ValidarFonte( + snapshot, + _timeoutSensor_s, + exigirSoc: false + ); + + return snapshot; + } + + private void ValidarFonte( + FonteEnergiaSnapshot fonte, + double timeout_s, + bool exigirSoc) + { + if (fonte == null) + return; + + if (!fonte.Inicializada) + { + fonte.Valida = false; + fonte.MotivoInvalida = + "fonte_nao_inicializada"; + + return; + } + + if ( + !fonte.Timestamp.HasValue || + fonte.Timestamp.Value == + DateTime.MinValue + ) + { + fonte.Valida = false; + fonte.MotivoInvalida = + "timestamp_invalido"; + + return; + } + + fonte.Idade_s = + ( + DateTime.Now - + fonte.Timestamp.Value + ).TotalSeconds; + + if ( + fonte.Idade_s < -2.0 || + fonte.Idade_s > timeout_s + ) + { + fonte.Valida = false; + fonte.MotivoInvalida = + $"leitura_desatualizada_{fonte.Idade_s:F2}s"; + + return; + } + + if ( + !TensaoPlausivel( + fonte.Tensao_V + ) + ) + { + fonte.Valida = false; + fonte.MotivoInvalida = + $"tensao_invalida_{fonte.Tensao_V:F2}V"; + + return; + } + + if ( + !ValorFinito( + fonte.CorrenteLiquida_A + ) + ) + { + fonte.Valida = false; + fonte.MotivoInvalida = + "corrente_invalida"; + + return; + } + + if ( + exigirSoc && + !fonte.SocValido + ) + { + fonte.Valida = false; + fonte.MotivoInvalida = + "soc_invalido"; + + return; + } + + fonte.Valida = true; + fonte.MotivoInvalida = ""; + } + + private FonteEnergiaSnapshot EscolherFonteAtiva( + FonteEnergiaSnapshot bms, + FonteEnergiaSnapshot sensor) + { + if (bms?.Valida == true) + { + MotivoFonteAtiva = + "Daly válido e prioritário"; + + return bms; + } + + if (sensor?.Valida == true) + { + MotivoFonteAtiva = + bms == null + ? "AB36V disponível" + : $"Fallback AB36V; Daly inválido: " + + $"{bms.MotivoInvalida}"; + + return sensor; + } + + MotivoFonteAtiva = + $"Nenhuma fonte válida | " + + $"Daly={bms?.MotivoInvalida ?? "ausente"} | " + + $"AB36V={sensor?.MotivoInvalida ?? "ausente"}"; + + return null; + } + + private void AplicarFonteAtiva( + FonteEnergiaSnapshot fonte) + { + if (fonte == null) + { + Iniciado = false; + BmsLigado = false; + FonteAtiva = "nenhuma"; + + TensaoInstantanea = 0; + CorrenteInstantanea = 0; + CorrenteLiquida_A = 0; + CorrenteDescarga_A = 0; + CorrenteCarga_A = 0; + + PotenciaInstantanea = 0; + PotenciaDescarga_W = 0; + PotenciaCarga_W = 0; + + return; + } + + Iniciado = true; + + FonteAtiva = + fonte.Nome; + + BmsLigado = + string.Equals( + fonte.Nome, + "DalyBMS", + StringComparison.OrdinalIgnoreCase + ); + + TensaoInstantanea = + fonte.Tensao_V; + + CorrenteInstantanea = + fonte.CorrenteBruta_A; + + CorrenteLiquida_A = + fonte.CorrenteLiquida_A; + + CorrenteDescarga_A = + Math.Max( + 0, + CorrenteLiquida_A + ); + + CorrenteCarga_A = + Math.Max( + 0, + -CorrenteLiquida_A + ); + + PotenciaInstantanea = + TensaoInstantanea * + CorrenteLiquida_A; + + PotenciaDescarga_W = + TensaoInstantanea * + CorrenteDescarga_A; + + PotenciaCarga_W = + TensaoInstantanea * + CorrenteCarga_A; + + PercentualBateria = + Clamp( + fonte.Soc_percent, + 0, + 100 + ); + + Temperatura = + fonte.Temperatura_C; + + if ( + !string.Equals( + _ultimaFonteLogada, + FonteAtiva, + StringComparison.OrdinalIgnoreCase + ) + ) + { + Variaveis.MostrarLog( + $"[COULOMB COUNTER] Fonte ativa: " + + $"{FonteAtiva} | {MotivoFonteAtiva}" + ); + + _ultimaFonteLogada = + FonteAtiva; + } + } + + private void AtualizarDiagnosticoRedundancia( + FonteEnergiaSnapshot bms, + FonteEnergiaSnapshot sensor) + { + DiferencaTensaoFontes_V = 0; + DiferencaCorrenteFontes_A = 0; + FontesCoerentes = true; + + if ( + bms?.Valida != true || + sensor?.Valida != true + ) + { + return; + } + + DiferencaTensaoFontes_V = + Math.Abs( + bms.Tensao_V - + sensor.Tensao_V + ); + + DiferencaCorrenteFontes_A = + Math.Abs( + bms.CorrenteLiquida_A - + sensor.CorrenteLiquida_A + ); + + double maiorCorrente = + Math.Max( + Math.Abs( + bms.CorrenteLiquida_A + ), + Math.Abs( + sensor.CorrenteLiquida_A + ) + ); + + double limiteCorrente = + Math.Max( + DiferencaCorrenteMinAlerta_A, + maiorCorrente * + DiferencaCorrenteRelativaAlerta + ); + + FontesCoerentes = + DiferencaTensaoFontes_V <= + DiferencaTensaoAlerta_V && + DiferencaCorrenteFontes_A <= + limiteCorrente; + } + + // ============================================================ + // Inicialização e integração + // ============================================================ + + private void InicializarEstado( + DateTime leituraAtual, + double distanciaTotalMetrosAtual) + { + double socInicial = + Clamp( + PercentualBateria, + 0, + 100 + ); + + _socInicialInstancia = + socInicial; + + _cargaIntegradaAtual_C = + CargaMaxima_C * + socInicial / + 100.0; _consumidoIntegradoTotal_C = 0; _consumidoIntegradoOperacao_C = 0; + + _carregadoIntegradoTotal_C = 0; + _carregadoIntegradoOperacao_C = 0; + EnergiaConsumidaTotal_Wh = 0; + EnergiaCarregadaTotal_Wh = 0; + + _ultimaLeituraIntegrada = + leituraAtual; + + _ultimaFonteIntegrada = + FonteAtiva; - _ultimaLeituraIntegrada = leituraAtual; _estadoInicializado = true; - _distanciaTotalMetrosAnterior = distanciaTotalMetrosAtual; - _energiaConsumidaAnterior_Wh_Total = EnergiaConsumidaTotal_Wh; - _dt = 0; - AtualizarAutonomia(distanciaTotalMetrosAtual); + AtualizarEstadoEnergia(); + + ResetarHistoricoAutonomia( + distanciaTotalMetrosAtual + ); + + AtualizarAutonomiaSemIntegrar( + distanciaTotalMetrosAtual + ); } - private void AtualizarCargaAtualPeloSocOuIntegrador() + private void IntegrarEnergiaECarga( + double dt_s) { - if (CargaMaxima_C <= 0) + double deltaCargaLiquida_C = + CorrenteLiquida_A * + dt_s; + + /* + * Descarga positiva reduz a carga integrada. + * Carga negativa aumenta a carga integrada. + */ + _cargaIntegradaAtual_C = + Clamp( + _cargaIntegradaAtual_C - + deltaCargaLiquida_C, + 0, + CargaMaxima_C + ); + + if (CorrenteDescarga_A > 0) { - CargaAtual_C = 0; - return; + double deltaDescarga_C = + CorrenteDescarga_A * + dt_s; + + _consumidoIntegradoTotal_C = + Clamp( + _consumidoIntegradoTotal_C + + deltaDescarga_C, + 0, + CargaMaxima_C + ); + + if ( + OperacaoAtiva && + !OperacaoFinalizada + ) + { + _consumidoIntegradoOperacao_C = + Clamp( + _consumidoIntegradoOperacao_C + + deltaDescarga_C, + 0, + CargaMaxima_C + ); + } + + double deltaEnergia_Wh = + PotenciaDescarga_W * + dt_s / + 3600.0; + + EnergiaConsumidaTotal_Wh += + Math.Max( + 0, + deltaEnergia_Wh + ); } - if (BmsLigado) + if (CorrenteCarga_A > 0) { - CargaAtual_C = CargaMaxima_C * (PercentualBateria / 100.0); + double deltaCarga_C = + CorrenteCarga_A * + dt_s; + + _carregadoIntegradoTotal_C = + Clamp( + _carregadoIntegradoTotal_C + + deltaCarga_C, + 0, + CargaMaxima_C + ); + + if ( + OperacaoAtiva && + !OperacaoFinalizada + ) + { + _carregadoIntegradoOperacao_C = + Clamp( + _carregadoIntegradoOperacao_C + + deltaCarga_C, + 0, + CargaMaxima_C + ); + } + + double deltaEnergiaCarga_Wh = + PotenciaCarga_W * + dt_s / + 3600.0; + + EnergiaCarregadaTotal_Wh += + Math.Max( + 0, + deltaEnergiaCarga_Wh + ); + } + } + + private void AtualizarEstadoEnergia() + { + double socIntegrado = + SocIntegrado_percent; + + if (BmsDisponivel) + { + /* + * Usa a menor estimativa. + * Se o BMS ou o integrador ficar otimista, o outro limita. + */ + PercentualAtual = + Math.Min( + Clamp( + BmsSoc_percent, + 0, + 100 + ), + socIntegrado + ); } else { - double cargaInicial = CargaMaxima_C * (_socInicialInstancia / 100.0); - CargaAtual_C = Clamp(cargaInicial - _consumidoIntegradoTotal_C, 0, CargaMaxima_C); + PercentualAtual = + socIntegrado; } + + CargaAtual_C = + CargaMaxima_C * + PercentualAtual / + 100.0; + + WhRestanteBms = + BmsDisponivel + ? EnergiaMaximaNominal_Wh * + Clamp( + BmsSoc_percent, + 0, + 100 + ) / + 100.0 + : 0; + + WhRestanteIntegrado = + EnergiaMaximaNominal_Wh * + socIntegrado / + 100.0; + + WhRestante = + BmsDisponivel + ? Math.Min( + WhRestanteBms, + WhRestanteIntegrado + ) + : WhRestanteIntegrado; } - private double NormalizarCorrenteDescarga(double corrente) + // ============================================================ + // Histórico de potência e tempo restante + // ============================================================ + + private void AtualizarHistoricoPotencia() { - if (Math.Abs(corrente) < 0.05) - return 0; + _historicoPotenciaDescarga.Enqueue( + PotenciaDescarga_W + ); - // Daly normalmente pode reportar descarga negativa. - // Para autonomia, queremos magnitude da descarga. - return Math.Abs(corrente); - } - - private void AtualizarHistoricos() - { - _historicoCorrenteDescarga.Enqueue(CorrenteDescarga_A); - if (_historicoCorrenteDescarga.Count > BufferSize) - _historicoCorrenteDescarga.Dequeue(); - - _historicoPotenciaDescarga.Enqueue(PotenciaDescarga_W); - if (_historicoPotenciaDescarga.Count > BufferSize) + while ( + _historicoPotenciaDescarga.Count > + BufferPotenciaSize + ) + { _historicoPotenciaDescarga.Dequeue(); + } } private void AtualizarTempoRestante() { - double potenciaMedia_W = _historicoPotenciaDescarga.Count > 0 - ? _historicoPotenciaDescarga.Average() - : 0; + double potenciaMedia_W = + _historicoPotenciaDescarga.Count > 0 + ? _historicoPotenciaDescarga.Average() + : 0; - if (potenciaMedia_W > 1 && WhRestante > 0) - TempoEstimadoMinutos = (WhRestante / potenciaMedia_W) * 60.0; + double margem = + Clamp( + MargemSegurancaAutonomia, + 0, + 0.90 + ); + + if ( + potenciaMedia_W > + PotenciaMinimaTempo_W && + WhUtilRestante > 0 + ) + { + TempoEstimadoMinutos = + WhUtilRestante / + potenciaMedia_W * + 60.0 * + (1.0 - margem); + } else + { TempoEstimadoMinutos = 0; + } } - private void AtualizarAutonomia(double distanciaTotalMetrosAtual) + // ============================================================ + // Autonomia por distância + // ============================================================ + + private void AtualizarAutonomia( + double distanciaTotalMetrosAtual, + double deltaTempo_s) { - if (!_estadoInicializado) - return; + double deltaDist_m = + CalcularDeltaDistancia( + distanciaTotalMetrosAtual + ); - double deltaDist_m = Math.Max(0, distanciaTotalMetrosAtual - _distanciaTotalMetrosAnterior); - _distanciaTotalMetrosAnterior = distanciaTotalMetrosAtual; + double deltaEnergia_Wh = + PotenciaDescarga_W * + Math.Max( + 0, + deltaTempo_s + ) / + 3600.0; - double energiaTotalAtual_Wh = EnergiaConsumidaTotal_Wh; - double deltaWh = Math.Max(0, energiaTotalAtual_Wh - _energiaConsumidaAnterior_Wh_Total); - _energiaConsumidaAnterior_Wh_Total = energiaTotalAtual_Wh; + AtualizarJanelaAutonomia( + deltaDist_m, + deltaTempo_s, + deltaEnergia_Wh + ); - if (deltaDist_m > 0 && deltaWh >= 0) + RecalcularAutonomia(); + } + + private void AtualizarAutonomiaSemIntegrar( + double distanciaTotalMetrosAtual) + { + CalcularDeltaDistancia( + distanciaTotalMetrosAtual + ); + + RecalcularAutonomia(); + } + + private double CalcularDeltaDistancia( + double distanciaTotalMetrosAtual) + { + if ( + distanciaTotalMetrosAtual < + _distanciaTotalMetrosAnterior + ) { - _historicoDeltaDist_m.Enqueue(deltaDist_m); - _historicoDeltaWh.Enqueue(deltaWh); + _distanciaTotalMetrosAnterior = + distanciaTotalMetrosAtual; - _somaDistanciaJanela_m += deltaDist_m; - _somaEnergiaJanela_Wh += deltaWh; - - while (_somaDistanciaJanela_m > JanelaAutonomia_m && _historicoDeltaDist_m.Count > 0) - { - _somaDistanciaJanela_m -= _historicoDeltaDist_m.Dequeue(); - _somaEnergiaJanela_Wh -= _historicoDeltaWh.Dequeue(); - } + return 0; } - if (_somaDistanciaJanela_m > 0.5) - WhPorMetroHistorico = _somaEnergiaJanela_Wh / _somaDistanciaJanela_m; + double deltaDist_m = + Math.Max( + 0, + distanciaTotalMetrosAtual - + _distanciaTotalMetrosAnterior + ); - WhPorMetro = WhPorMetroHistorico > 0 ? WhPorMetroHistorico : WhPorMetroBase; + _distanciaTotalMetrosAnterior = + distanciaTotalMetrosAtual; - if (WhPorMetroReferencia > 0) + return deltaDist_m; + } + + private void AtualizarJanelaAutonomia( + double deltaDist_m, + double deltaTempo_s, + double deltaEnergia_Wh) + { + deltaDist_m = + Math.Max( + 0, + deltaDist_m + ); + + deltaTempo_s = + Math.Max( + 0, + deltaTempo_s + ); + + deltaEnergia_Wh = + Math.Max( + 0, + deltaEnergia_Wh + ); + + /* + * Mesmo com distância zero, a energia entra na janela. + * Assim espera, calibração, manobras paradas e processamento + * continuam compondo o custo real da operação. + */ + _janelaAutonomia.Enqueue( + ( + deltaDist_m, + deltaTempo_s, + deltaEnergia_Wh + ) + ); + + _somaDistanciaJanela_m += + deltaDist_m; + + _somaTempoJanela_s += + deltaTempo_s; + + _somaEnergiaJanela_Wh += + deltaEnergia_Wh; + + while ( + ( + _somaDistanciaJanela_m > + JanelaAutonomia_m || + _somaTempoJanela_s > + JanelaAutonomiaTempo_s + ) && + _janelaAutonomia.Count > 0 + ) { - DistanciaMaximaTeorica_m = WhRestante / WhPorMetroReferencia; - DistanciaMaximaSegura_m = DistanciaMaximaTeorica_m * (1.0 - MargemSegurancaAutonomia); - DistanciaEstimadaRestante_m = DistanciaMaximaSegura_m; + var old = + _janelaAutonomia.Dequeue(); + + _somaDistanciaJanela_m -= + old.dist_m; + + _somaTempoJanela_s -= + old.tempo_s; + + _somaEnergiaJanela_Wh -= + old.energia_Wh; + } + + if ( + _somaDistanciaJanela_m > + MinDistanciaTaxa_m + ) + { + WhPorMetroJanela = + _somaEnergiaJanela_Wh / + _somaDistanciaJanela_m; + } + + if ( + _somaDistanciaJanela_m >= + MinDistanciaHistorico_m && + WhPorMetroJanela > 0 + ) + { + WhPorMetroHistorico = + WhPorMetroHistorico <= 0 + ? WhPorMetroJanela + : WhPorMetroHistorico * + (1.0 - AlphaHistorico) + + WhPorMetroJanela * + AlphaHistorico; + + AutonomiaComHistorico = true; + } + } + + private void RecalcularAutonomia() + { + if (!Iniciado || !_estadoInicializado) + { + MarcarAutonomiaIndisponivel(); + return; + } + + if ( + AutonomiaComHistorico && + WhPorMetroHistorico > 0 + ) + { + FonteAutonomia = + "historico_operacional"; + + double consumoObservado = + Math.Max( + WhPorMetroJanela, + WhPorMetroHistorico + ); + + WhPorMetroReferencia = + consumoObservado * + FatorConservadorHistorico; + } + else if (WhPorMetroJanela > 0) + { + FonteAutonomia = + "janela_inicial"; + + WhPorMetroReferencia = + Math.Max( + WhPorMetroBase, + WhPorMetroJanela + ); + } + else + { + FonteAutonomia = + "fallback_campo"; + + WhPorMetroReferencia = + WhPorMetroBase; + } + + WhPorMetro = + WhPorMetroReferencia; + + if ( + WhUtilRestante > 0 && + WhPorMetroReferencia > 0 + ) + { + DistanciaMaximaTeorica_m = + WhUtilRestante / + WhPorMetroReferencia; + + double margem = + Clamp( + MargemSegurancaAutonomia, + 0, + 0.90 + ); + + DistanciaMaximaSegura_m = + DistanciaMaximaTeorica_m * + (1.0 - margem); + + DistanciaEstimadaRestante_m = + DistanciaMaximaSegura_m; } else { @@ -483,81 +1719,352 @@ namespace AgroBase.Services } } - public void AtualizarValorConsumidoTotal(double consumoTotal_Ah) + private void MarcarAutonomiaIndisponivel() { - double coulombsConsumidos = consumoTotal_Ah * 3600.0; - _consumidoIntegradoTotal_C = Clamp(coulombsConsumidos, 0, CargaMaxima_C); + WhPorMetro = 0; + WhPorMetroReferencia = 0; - AtualizarCargaAtualPeloSocOuIntegrador(); + DistanciaMaximaTeorica_m = 0; + DistanciaMaximaSegura_m = 0; + DistanciaEstimadaRestante_m = 0; + + TempoEstimadoMinutos = 0; + + FonteAutonomia = + "fonte_energia_indisponivel"; } - public bool ConseguePercorrer(double distanciaRequerida_m) + // ============================================================ + // Auditoria SOC x integração + // ============================================================ + + private void AtualizarDiferencasSoc() + { + if (!BmsDisponivel) + { + DiferencaSocBmsVsIntegrado_Total = 0; + DiferencaSocBmsVsIntegrado_Operacao = 0; + return; + } + + double consumidoBmsTotal = + Math.Max( + 0, + _socInicialInstancia - + BmsSoc_percent + ); + + DiferencaSocBmsVsIntegrado_Total = + PercentualConsumidoTotalIntegrado - + consumidoBmsTotal; + + if ( + OperacaoAtiva || + OperacaoFinalizada + ) + { + double consumidoBmsOperacao = + Math.Max( + 0, + _socInicialOperacao - + BmsSoc_percent + ); + + DiferencaSocBmsVsIntegrado_Operacao = + PercentualConsumidoOperacaoIntegrado - + consumidoBmsOperacao; + } + else + { + DiferencaSocBmsVsIntegrado_Operacao = 0; + } + } + + // ============================================================ + // API pública + // ============================================================ + + public void AtualizarValorConsumidoTotal( + double consumoTotal_Ah) + { + double coulombsConsumidos = + Math.Max( + 0, + consumoTotal_Ah + ) * + 3600.0; + + _consumidoIntegradoTotal_C = + Clamp( + coulombsConsumidos, + 0, + CargaMaxima_C + ); + + double cargaInicial_C = + CargaMaxima_C * + _socInicialInstancia / + 100.0; + + _cargaIntegradaAtual_C = + Clamp( + cargaInicial_C - + _consumidoIntegradoTotal_C + + _carregadoIntegradoTotal_C, + 0, + CargaMaxima_C + ); + + AtualizarEstadoEnergia(); + RecalcularAutonomia(); + } + + public bool ConseguePercorrer( + double distanciaRequerida_m) { if (distanciaRequerida_m <= 0) return true; - return DistanciaMaximaSegura_m >= distanciaRequerida_m; + return + Iniciado && + _estadoInicializado && + DistanciaMaximaSegura_m >= + distanciaRequerida_m; } - private void ResetarTudo() + // ============================================================ + // Helpers + // ============================================================ + + private double CalcularSocPorTensao( + double tensao) { - Iniciado = false; - BmsLigado = false; - OperacaoAtiva = false; - OperacaoFinalizada = false; + if (!ValorFinito(tensao)) + return 0; - _estadoInicializado = false; - _ultimaLeituraIntegrada = null; + double tensaoMin = + VariaveisEquipamento + .TensaoMinimaBateria; - CargaAtual_C = 0; - _socInicialInstancia = 0; - _socInicialOperacao = 0; + double tensaoMax = + VariaveisEquipamento + .TensaoMaximaBateria; - _consumidoIntegradoTotal_C = 0; - _consumidoIntegradoOperacao_C = 0; + if (tensaoMax <= tensaoMin) + return 0; - EnergiaConsumidaTotal_Wh = 0; - _energiaConsumidaInicioOperacao_Wh = 0; - _energiaConsumidaFimOperacao_Wh = 0; - - PercentualBateria = 0; - Temperatura = 0; - CorrenteInstantanea = 0; - CorrenteDescarga_A = 0; - TensaoInstantanea = 0; - PotenciaInstantanea = 0; - PotenciaDescarga_W = 0; - TempoEstimadoMinutos = 0; - _dt = 0; - - ResetarHistoricosAutonomia(); + return Clamp( + FuncoesMatematicas.Map( + tensao, + tensaoMin, + tensaoMax, + 0.0, + 100.0 + ), + 0, + 100 + ); } - private void ResetarHistoricosAutonomia() + private bool TensaoPlausivel( + double tensao) { - _historicoCorrenteDescarga.Clear(); - _historicoPotenciaDescarga.Clear(); + if (!ValorFinito(tensao)) + return false; - _historicoDeltaDist_m.Clear(); - _historicoDeltaWh.Clear(); + double tensaoMin = + VariaveisEquipamento + .TensaoMinimaBateria; + + double tensaoMax = + VariaveisEquipamento + .TensaoMaximaBateria; + + if ( + tensaoMin <= 0 || + tensaoMax <= tensaoMin + ) + { + return + tensao >= 10 && + tensao <= 80; + } + + return + tensao >= tensaoMin * 0.70 && + tensao <= tensaoMax * 1.20; + } + + private double NormalizarCorrenteLiquida( + double correnteBruta, + double sinalDescarga) + { + if (!ValorFinito(correnteBruta)) + return 0; + + if ( + Math.Abs(correnteBruta) < + CorrenteZonaMorta_A + ) + { + return 0; + } + + return + correnteBruta * + sinalDescarga; + } + + private static double NormalizarSinal( + double sinal, + double padrao) + { + if ( + double.IsNaN(sinal) || + double.IsInfinity(sinal) || + Math.Abs(sinal) < 0.5 + ) + { + return padrao; + } + + return sinal >= 0 + ? 1.0 + : -1.0; + } + + private static bool ValorFinito( + double valor) + { + return + !double.IsNaN(valor) && + !double.IsInfinity(valor); + } + + private void ResetarHistoricoAutonomia( + double distanciaAtual_m = 0) + { + _janelaAutonomia.Clear(); _somaDistanciaJanela_m = 0; + _somaTempoJanela_s = 0; _somaEnergiaJanela_Wh = 0; - _distanciaTotalMetrosAnterior = 0; - _energiaConsumidaAnterior_Wh_Total = 0; + _distanciaTotalMetrosAnterior = + distanciaAtual_m; - WhPorMetro = 0; + WhPorMetroJanela = 0; WhPorMetroHistorico = 0; + WhPorMetro = 0; + WhPorMetroReferencia = 0; + + AutonomiaComHistorico = false; + FonteAutonomia = + "fallback_campo"; DistanciaMaximaTeorica_m = 0; DistanciaMaximaSegura_m = 0; DistanciaEstimadaRestante_m = 0; } - private static double Clamp(double value, double min, double max) + private void ResetarTudo() { - return FuncoesMatematicas.Clamp(value, min, max); + Iniciado = false; + BmsLigado = false; + + BmsDisponivel = false; + SensorAb36VDisponivel = false; + + FonteAtiva = "nenhuma"; + MotivoFonteAtiva = ""; + + OperacaoAtiva = false; + OperacaoFinalizada = false; + + _estadoInicializado = false; + _inicioOperacaoPendente = false; + + _ultimaLeituraIntegrada = null; + _ultimaFonteIntegrada = ""; + _ultimaFonteLogada = ""; + + _cargaIntegradaAtual_C = 0; + CargaAtual_C = 0; + + _socInicialInstancia = 0; + _socInicialOperacao = 0; + + _distanciaInicioOperacao_m = 0; + + _consumidoIntegradoTotal_C = 0; + _consumidoIntegradoOperacao_C = 0; + + _carregadoIntegradoTotal_C = 0; + _carregadoIntegradoOperacao_C = 0; + + PercentualBateria = 0; + PercentualAtual = 0; + + Temperatura = 0; + + CorrenteInstantanea = 0; + CorrenteLiquida_A = 0; + CorrenteDescarga_A = 0; + CorrenteCarga_A = 0; + + TensaoInstantanea = 0; + + PotenciaInstantanea = 0; + PotenciaDescarga_W = 0; + PotenciaCarga_W = 0; + + TempoEstimadoMinutos = 0; + _dt = 0; + + BmsTensao_V = 0; + BmsCorrenteLiquida_A = 0; + BmsSoc_percent = 0; + + SensorTensao_V = 0; + SensorCorrenteLiquida_A = 0; + SensorSocEstimado_percent = 0; + + DiferencaTensaoFontes_V = 0; + DiferencaCorrenteFontes_A = 0; + FontesCoerentes = true; + + DiferencaSocBmsVsIntegrado_Total = 0; + DiferencaSocBmsVsIntegrado_Operacao = 0; + + EnergiaConsumidaTotal_Wh = 0; + EnergiaCarregadaTotal_Wh = 0; + + _energiaConsumidaInicioOperacao_Wh = 0; + _energiaConsumidaFimOperacao_Wh = 0; + + _energiaCarregadaInicioOperacao_Wh = 0; + _energiaCarregadaFimOperacao_Wh = 0; + + WhRestanteBms = 0; + WhRestanteIntegrado = 0; + WhRestante = 0; + + _historicoPotenciaDescarga.Clear(); + + ResetarHistoricoAutonomia(); + } + + private static double Clamp( + double value, + double min, + double max) + { + return + FuncoesMatematicas.Clamp( + value, + min, + max + ); } } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Services/HerbicideCounterService.cs b/AgroBase/AgroBase/Services/HerbicideCounterService.cs index 6a1edde30..4ee80e241 100644 --- a/AgroBase/AgroBase/Services/HerbicideCounterService.cs +++ b/AgroBase/AgroBase/Services/HerbicideCounterService.cs @@ -9,22 +9,97 @@ namespace AgroBase.Services { public class HerbicideCounterService { - public HerbicideCounterService(double capacidadeLitros, double densidadeProduto, double vazaoPorBicoBase_mLs = 33.0) + /* + * Convenções desta classe: + * + * 1) Volume atual: + * vem do sensor de massa do reservatório. + * + * 2) Consumo instantâneo: + * usa preferencialmente o sensor de fluxo. + * Se o fluxo estiver indisponível, usa a queda filtrada do reservatório. + * + * 3) Autonomia operacional seletiva: + * considera todo o deslocamento da operação, inclusive trechos sem atuação. + * É esta autonomia que deve ser usada para decidir se o rover pode entrar + * e concluir o próximo corredor. + * + * 4) Autonomia de pulverização contínua: + * considera o pulverizador ligado continuamente. + * É uma métrica diagnóstica e um pior caso, não deve bloquear diretamente + * a entrada no corredor de uma operação seletiva. + */ + + private const double MinVelocidadeReferencia_m_s = 0.10; + private const double MinDistanciaTaxa_m = 0.50; + private const double MinDistanciaHistoricoOperacional_m = 15.0; + private const double MinTempoTaxa_s = 1.0; + + private const double JanelaPulverizacaoContinua_m = 50.0; + private const double JanelaTempoPulverizacao_s = 60.0; + + private const double JanelaOperacional_m = 120.0; + private const double JanelaOperacionalTempo_s = 300.0; + + private const double AlphaHistorico = 0.20; + private const double AlphaVolumeReservatorio = 0.25; + + private const double VazaoMinimaValida_mLs = 0.5; + private const double FatorMaximoVazaoSobreCurva = 2.5; + + private const double MaxSaltoVolumeOperacao_L = 4.0; + private const double FatorMaximoDeltaMassa = 3.0; + + private readonly Dictionary _vazaoTotalPorQuantidadeBicos_mLs; + + public HerbicideCounterService( + double capacidadeLitros, + double densidadeProduto, + double vazaoPorBicoBase_mLs = 17.5, + double lPorMetroOperacionalBase = 0.030, + IReadOnlyDictionary vazaoTotalPorQuantidadeBicos_mLs = null) { - CapacidadeLitros = capacidadeLitros; - DensidadeProduto = densidadeProduto; + CapacidadeLitros = Math.Max(0, capacidadeLitros); + DensidadeProduto = Math.Max(0, densidadeProduto); - QuantidadeBicos = VariaveisEquipamento.QuantidadeBicosPulverizadores; + QuantidadeBicos = Math.Max( + 0, + VariaveisEquipamento.QuantidadeBicosPulverizadores + ); - VazaoPorBicoBase_mLs = vazaoPorBicoBase_mLs; - VazaoPorBicoBase_LMin = vazaoPorBicoBase_mLs * 60.0 / 1000.0; - VazaoTotalTodosBicos_LMin = VazaoPorBicoBase_LMin * QuantidadeBicos; + /* + * Mantido por compatibilidade com UI e código legado. + * Não é mais multiplicado linearmente para todos os bicos. + */ + VazaoPorBicoBase_mLs = Math.Max(0, vazaoPorBicoBase_mLs); + VazaoPorBicoBase_LMin = VazaoPorBicoBase_mLs * 60.0 / 1000.0; + + _vazaoTotalPorQuantidadeBicos_mLs = + MontarCurvaVazaoPadrao( + QuantidadeBicos, + VazaoPorBicoBase_mLs, + vazaoTotalPorQuantidadeBicos_mLs + ); + + VazaoTotalTodosBicos_LMin = + ObterVazaoTotalEstimada_mLs(QuantidadeBicos) * 60.0 / 1000.0; + + /* + * 0,030 L/m foi escolhido como fallback de campo conservador: + * fica próximo do pior corredor completo observado, sem assumir + * sete bicos ligados durante 100% do percurso. + */ + LPorMetroBase = Math.Max(0, lPorMetroOperacionalBase); MargemSegurancaAutonomia = 0.30; ResetarTudo(); } + // ============================================================ + // Estado geral + // ============================================================ + public bool Iniciado { get; private set; } public bool OperacaoAtiva { get; private set; } public bool OperacaoFinalizada { get; private set; } @@ -38,14 +113,38 @@ namespace AgroBase.Services public double VazaoPorBicoBase_LMin { get; } public double VazaoTotalTodosBicos_LMin { get; } + public IReadOnlyDictionary CurvaVazaoTotalPorQuantidadeBicos_mLs => + _vazaoTotalPorQuantidadeBicos_mLs; + public double MassaAtual_Kg { get; private set; } public double MassaAtual_Kg_Calc { get; private set; } + + public double VolumeAtualBruto_L { get; private set; } public double VolumeAtual_L { get; private set; } + public bool LeituraReservatorioConfiavel { get; private set; } + + public double VolumeReservaMinima_L + { + get + { + double percentualMinimo = Clamp( + VariaveisEquipamento.PercentualReservatorioMinCritio, + 0, + 100 + ); + + return CapacidadeLitros * percentualMinimo / 100.0; + } + } + + public double VolumeUtilAutonomia_L => + Math.Max(0, VolumeAtual_L - VolumeReservaMinima_L); private double _volumeInicialInstancia_L; private double _volumeInicialOperacao_L; - public double VolumeConsumidoTotal_L => Math.Max(0, _volumeInicialInstancia_L - VolumeAtual_L); + public double VolumeConsumidoTotal_L => + Math.Max(0, _volumeInicialInstancia_L - VolumeAtual_L); public double VolumeConsumidoOperacao_L { @@ -54,7 +153,10 @@ namespace AgroBase.Services if (!OperacaoAtiva && !OperacaoFinalizada) return 0; - return Math.Max(0, _volumeInicialOperacao_L - VolumeAtual_L); + return Math.Max( + 0, + _volumeInicialOperacao_L - VolumeAtual_L + ); } } @@ -62,8 +164,14 @@ namespace AgroBase.Services { get { - if (CapacidadeLitros <= 0) return 0; - return Clamp((VolumeAtual_L / CapacidadeLitros) * 100.0, 0, 100); + if (CapacidadeLitros <= 0) + return 0; + + return Clamp( + VolumeAtual_L / CapacidadeLitros * 100.0, + 0, + 100 + ); } } @@ -71,8 +179,14 @@ namespace AgroBase.Services { get { - if (CapacidadeLitros <= 0) return 0; - return Clamp((VolumeConsumidoTotal_L / CapacidadeLitros) * 100.0, 0, 100); + if (CapacidadeLitros <= 0) + return 0; + + return Clamp( + VolumeConsumidoTotal_L / CapacidadeLitros * 100.0, + 0, + 100 + ); } } @@ -80,8 +194,14 @@ namespace AgroBase.Services { get { - if (CapacidadeLitros <= 0) return 0; - return Clamp((VolumeConsumidoOperacao_L / CapacidadeLitros) * 100.0, 0, 100); + if (CapacidadeLitros <= 0) + return 0; + + return Clamp( + VolumeConsumidoOperacao_L / CapacidadeLitros * 100.0, + 0, + 100 + ); } } @@ -89,17 +209,30 @@ namespace AgroBase.Services { get { - if (_volumeInicialOperacao_L <= 0) return 0; - return Clamp((VolumeConsumidoOperacao_L / _volumeInicialOperacao_L) * 100.0, 0, 100); + if (_volumeInicialOperacao_L <= 0) + return 0; + + return Clamp( + VolumeConsumidoOperacao_L / _volumeInicialOperacao_L * 100.0, + 0, + 100 + ); } } + // ============================================================ + // Pulverização e fluxo + // ============================================================ + public int BicosAtivos { get; private set; } public bool Pulverizando { get; private set; } public bool PulverizadorLiberado { get; private set; } public double VazaoFluxoAtual_mLs { get; private set; } - public double VazaoFluxoAtual_LMin => VazaoFluxoAtual_mLs * 60.0 / 1000.0; + public bool VazaoFluxoValida { get; private set; } + + public double VazaoFluxoAtual_LMin => + VazaoFluxoAtual_mLs * 60.0 / 1000.0; public double VazaoEstimativaBicos_mLs { get; private set; } public double VazaoReferencia_mLs { get; private set; } @@ -107,30 +240,81 @@ namespace AgroBase.Services public double LPorMinuto { get; private set; } public double LPorMinutoEstimadoBicos { get; private set; } + /* + * Compatibilidade: + * LPorMetro e LPorMetroHistorico continuam significando + * consumo por metro enquanto o pulverizador está efetivamente atuando. + */ public double LPorMetro { get; private set; } public double LPorMetroHistorico { get; private set; } + + /* + * Métricas novas para a operação seletiva. + * Incluem todo o deslocamento, inclusive metros sem atuação. + */ + public double LPorMetroOperacional { get; private set; } + public double LPorMetroOperacionalHistorico { get; private set; } public double LPorMetroBase { get; private set; } + public double LPorMetroReferenciaOperacional { get; private set; } + public double LPorMetroReferenciaPulverizacaoContinua { get; private set; } + + public bool AutonomiaOperacionalComHistorico { get; private set; } + public string FonteAutonomiaOperacional { get; private set; } = "indisponivel"; + public double MargemSegurancaAutonomia { get; set; } + /* + * Estas propriedades continuam sendo a autonomia usada pelo corredor. + * Agora representam a operação seletiva real. + */ public double DistanciaMaximaTeorica_m { get; private set; } public double DistanciaMaximaSegura_m { get; private set; } - public double DistanciaEstimadaRestantePulverizando_m => DistanciaMaximaSegura_m; + + public double DistanciaEstimadaRestantePulverizando_m => + DistanciaMaximaSegura_m; + + /* + * Pior caso diagnóstico: pulverização ligada continuamente. + */ + public double DistanciaMaximaTeoricaPulverizacaoContinua_m { get; private set; } + public double DistanciaMaximaSeguraPulverizacaoContinua_m { get; private set; } public double TempoEstimadoMinutosPulverizando { get; private set; } + public double VolumeConsumidoFluxoTotal_L { get; private set; } + public double VolumeConsumidoFluxoOperacao_L { get; private set; } + + public double DiferencaConsumoFluxoVsReservatorioOperacao_L => + VolumeConsumidoFluxoOperacao_L - VolumeConsumidoOperacao_L; + public double HerbicidaPorAtuacao_L { get; private set; } - private const double JanelaAutonomia_m = 50.0; - private const double JanelaTempo_s = 60.0; + // ============================================================ + // Janelas de consumo + // ============================================================ - private readonly Queue<(double dist_m, double vol_L)> _janelaDist = new Queue<(double dist_m, double vol_L)>(); - private readonly Queue<(double tempo_s, double vol_L)> _janelaTempo = new Queue<(double tempo_s, double vol_L)>(); + private readonly Queue<(double dist_m, double vol_L)> + _janelaPulverizacaoDist = + new Queue<(double dist_m, double vol_L)>(); - private double _somaDistanciaJanela_m; - private double _somaVolumeDistJanela_L; - private double _somaTempoJanela_s; - private double _somaVolumeTempoJanela_L; + private readonly Queue<(double tempo_s, double vol_L)> + _janelaPulverizacaoTempo = + new Queue<(double tempo_s, double vol_L)>(); + + private readonly Queue<(double dist_m, double tempo_s, double vol_L)> + _janelaOperacional = + new Queue<(double dist_m, double tempo_s, double vol_L)>(); + + private double _somaDistanciaPulverizacao_m; + private double _somaVolumePulverizacaoDist_L; + + private double _somaTempoPulverizacao_s; + private double _somaVolumePulverizacaoTempo_L; + + private double _somaDistanciaOperacional_m; + private double _somaTempoOperacional_s; + private double _somaVolumeOperacional_L; private double _volumeAnterior_L; private double _distanciaTotalAnterior_m; @@ -141,8 +325,17 @@ namespace AgroBase.Services private double _tempoInicioOperacaoPendente_s; private bool _inicializado; + private bool _volumeFiltradoInicializado; - public void IniciarOperacao(double distanciaTotalMetrosAtual, double tempoTotalSegundosAtual) + private int _atuacoesInicioOperacao; + + // ============================================================ + // Ciclo de vida + // ============================================================ + + public void IniciarOperacao( + double distanciaTotalMetrosAtual, + double tempoTotalSegundosAtual) { if (!_inicializado) { @@ -155,22 +348,14 @@ namespace AgroBase.Services _tempoInicioOperacaoPendente_s = tempoTotalSegundosAtual; - ResetarJanelas(); + ResetarJanelasOperacao(); return; } - _volumeInicialOperacao_L = VolumeAtual_L; - - _distanciaTotalAnterior_m = - distanciaTotalMetrosAtual; - _tempoTotalAnterior_s = - tempoTotalSegundosAtual; - - OperacaoAtiva = true; - OperacaoFinalizada = false; - _inicioOperacaoPendente = false; - - ResetarJanelas(); + PrepararBaselineOperacao( + distanciaTotalMetrosAtual, + tempoTotalSegundosAtual + ); } public void FinalizarOperacao() @@ -183,41 +368,101 @@ namespace AgroBase.Services _inicioOperacaoPendente = false; } - public void AtualizarConsumo(double distanciaTotalMetrosAtual, double tempoTotalSegundosAtual, double velocidadeAtual_m_s) + private void PrepararBaselineOperacao( + double distanciaTotalMetrosAtual, + double tempoTotalSegundosAtual) { - Iniciado = Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.SensorMassaReservatorio?.Inicializado ?? false; + _volumeInicialOperacao_L = VolumeAtual_L; + + _distanciaTotalAnterior_m = + distanciaTotalMetrosAtual; + + _tempoTotalAnterior_s = + tempoTotalSegundosAtual; + + _atuacoesInicioOperacao = + ObterTotalAtuacoes(); + + VolumeConsumidoFluxoOperacao_L = 0; + + OperacaoAtiva = true; + OperacaoFinalizada = false; + _inicioOperacaoPendente = false; + + ResetarJanelasOperacao(); + } + + // ============================================================ + // Atualização principal + // ============================================================ + + public void AtualizarConsumo( + double distanciaTotalMetrosAtual, + double tempoTotalSegundosAtual, + double velocidadeAtual_m_s) + { + var sensorMassa = + Variaveis + .OperacaoEmAndamento? + .DispAtu? + .Dados? + .SensorMassaReservatorio; + + Iniciado = sensorMassa?.Inicializado ?? false; if (!Iniciado) { - PulverizadorLiberado = false; - BicosAtivos = 0; - Pulverizando = false; - VazaoFluxoAtual_mLs = 0; - VazaoEstimativaBicos_mLs = 0; - VazaoReferencia_mLs = 0; - LPorMinutoEstimadoBicos = 0; + MarcarIndisponivel(); return; } - LerReservatorio(out double massaLida_Kg, out double massaCalc_Kg, out double volumeMedido_L); + double deltaTempo_s = _inicializado + ? Math.Max( + 0, + tempoTotalSegundosAtual - _tempoTotalAnterior_s + ) + : 0; + + LerReservatorio( + out double massaLida_Kg, + out double massaCalc_Kg, + out double volumeMedido_L + ); MassaAtual_Kg = massaLida_Kg; MassaAtual_Kg_Calc = massaCalc_Kg; - VolumeAtual_L = volumeMedido_L; + VolumeAtualBruto_L = volumeMedido_L; + + AtualizarVolumeReservatorioFiltrado( + volumeMedido_L, + deltaTempo_s + ); BicosAtivos = ObterQuantidadeBicosAtivos(); Pulverizando = BicosAtivos > 0; - VazaoFluxoAtual_mLs = ObterVazaoFluxo_mLs(); - VazaoEstimativaBicos_mLs = BicosAtivos * VazaoPorBicoBase_mLs; - VazaoReferencia_mLs = VazaoFluxoAtual_mLs > 0 - ? VazaoFluxoAtual_mLs - : VazaoEstimativaBicos_mLs; + VazaoEstimativaBicos_mLs = + ObterVazaoTotalEstimada_mLs(BicosAtivos); - LPorMinutoEstimadoBicos = VazaoEstimativaBicos_mLs * 60.0 / 1000.0; + VazaoFluxoAtual_mLs = + ObterVazaoFluxo_mLs(); + + VazaoFluxoValida = + ValidarVazaoFluxo( + VazaoFluxoAtual_mLs, + BicosAtivos + ); + + VazaoReferencia_mLs = + VazaoFluxoValida + ? VazaoFluxoAtual_mLs + : VazaoEstimativaBicos_mLs; + + LPorMinutoEstimadoBicos = + VazaoEstimativaBicos_mLs * 60.0 / 1000.0; PulverizadorLiberado = - PercentualAtual >= VariaveisEquipamento.PercentualReservatorioMinCritio; + VolumeAtual_L > VolumeReservaMinima_L; if (!_inicializado) { @@ -228,84 +473,427 @@ namespace AgroBase.Services if (OperacaoAtiva || _inicioOperacaoPendente) { - /* - * Primeira leitura válida depois do início da operação. - * Usa o volume atual como baseline da operação para começar - * PercentualConsumidoOperacao em zero. - */ - _volumeInicialOperacao_L = VolumeAtual_L; - - _distanciaTotalAnterior_m = - distanciaTotalMetrosAtual; - _tempoTotalAnterior_s = - tempoTotalSegundosAtual; - - OperacaoAtiva = true; - OperacaoFinalizada = false; - _inicioOperacaoPendente = false; - - ResetarJanelas(); + PrepararBaselineOperacao( + distanciaTotalMetrosAtual, + tempoTotalSegundosAtual + ); } - AtualizarAutonomia(velocidadeAtual_m_s); + AtualizarAutonomia( + velocidadeAtual_m_s + ); + return; } - double deltaVolumeMassa_L = Math.Max(0, _volumeAnterior_L - VolumeAtual_L); - _volumeAnterior_L = VolumeAtual_L; + double deltaVolumeMassa_L = + CalcularDeltaVolumeMassa(deltaTempo_s); - double deltaDist_m = Math.Max(0, distanciaTotalMetrosAtual - _distanciaTotalAnterior_m); - _distanciaTotalAnterior_m = distanciaTotalMetrosAtual; + double deltaDist_m = Math.Max( + 0, + distanciaTotalMetrosAtual - + _distanciaTotalAnterior_m + ); - double deltaTempo_s = Math.Max(0, tempoTotalSegundosAtual - _tempoTotalAnterior_s); - _tempoTotalAnterior_s = tempoTotalSegundosAtual; + _distanciaTotalAnterior_m = + distanciaTotalMetrosAtual; - double deltaVolumeFluxo_L = 0; + _tempoTotalAnterior_s = + tempoTotalSegundosAtual; - if (Pulverizando && deltaTempo_s > 0 && VazaoReferencia_mLs > 0) - deltaVolumeFluxo_L = VazaoReferencia_mLs * deltaTempo_s / 1000.0; + double deltaVolumeFluxo_L = + CalcularDeltaVolumeFluxo(deltaTempo_s); - double deltaVolumeParaTaxa_L = EscolherDeltaVolumeParaTaxa(deltaVolumeMassa_L, deltaVolumeFluxo_L); + if (deltaVolumeFluxo_L > 0) + { + VolumeConsumidoFluxoTotal_L += + deltaVolumeFluxo_L; + if (OperacaoAtiva && !OperacaoFinalizada) + { + VolumeConsumidoFluxoOperacao_L += + deltaVolumeFluxo_L; + } + } + + double deltaVolumeParaTaxa_L = + EscolherDeltaVolumeParaTaxa( + deltaVolumeMassa_L, + deltaVolumeFluxo_L + ); + + /* + * Janela de pulverização contínua: + * somente quando os bicos estão atuando. + */ if (Pulverizando && deltaVolumeParaTaxa_L > 0) { - AtualizarJanelaDistancia(deltaDist_m, deltaVolumeParaTaxa_L); - AtualizarJanelaTempo(deltaTempo_s, deltaVolumeParaTaxa_L); + AtualizarJanelaPulverizacaoDistancia( + deltaDist_m, + deltaVolumeParaTaxa_L + ); + + AtualizarJanelaPulverizacaoTempo( + deltaTempo_s, + deltaVolumeParaTaxa_L + ); + } + + /* + * Janela operacional: + * recebe todo o deslocamento e todo o consumo da operação. + * Metros sem pulverização entram com volume zero. + * Consumo parado também entra, com distância zero. + */ + if ( + OperacaoAtiva && + !OperacaoFinalizada && + ( + deltaDist_m > 0 || + deltaTempo_s > 0 || + deltaVolumeParaTaxa_L > 0 + ) + ) + { + AtualizarJanelaOperacional( + deltaDist_m, + deltaTempo_s, + deltaVolumeParaTaxa_L + ); } AtualizarHerbicidaPorAtuacao(); - AtualizarAutonomia(velocidadeAtual_m_s); + AtualizarAutonomia( + velocidadeAtual_m_s + ); } - private double EscolherDeltaVolumeParaTaxa(double deltaMassa_L, double deltaFluxo_L) + private void MarcarIndisponivel() { - if (deltaFluxo_L > 0) - return deltaFluxo_L; + PulverizadorLiberado = false; + BicosAtivos = 0; + Pulverizando = false; - return deltaMassa_L; + VazaoFluxoAtual_mLs = 0; + VazaoFluxoValida = false; + VazaoEstimativaBicos_mLs = 0; + VazaoReferencia_mLs = 0; + + LPorMinutoEstimadoBicos = 0; + + DistanciaMaximaTeorica_m = 0; + DistanciaMaximaSegura_m = 0; + + DistanciaMaximaTeoricaPulverizacaoContinua_m = 0; + DistanciaMaximaSeguraPulverizacaoContinua_m = 0; + + TempoEstimadoMinutosPulverizando = 0; + + FonteAutonomiaOperacional = "reservatorio_indisponivel"; } - private void LerReservatorio(out double massaLida_Kg, out double massaCalc_Kg, out double volumeMedido_L) + // ============================================================ + // Reservatório + // ============================================================ + + private void LerReservatorio( + out double massaLida_Kg, + out double massaCalc_Kg, + out double volumeMedido_L) { - var sensor = Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.SensorMassaReservatorio; - massaLida_Kg = sensor?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.Massa)?.atual?.valor ?? 0; + var sensor = + Variaveis + .OperacaoEmAndamento? + .DispAtu? + .Dados? + .SensorMassaReservatorio; - double leituraBruta = sensor?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.LeituraBruta)?.atual?.valor ?? 0; + massaLida_Kg = + sensor? + .ValoresLeituras? + .FirstOrDefault( + x => x.funcao == FuncoesPinout.Massa + )? + .atual? + .valor + ?? 0; - double.TryParse(sensor?.Parametros?.FirstOrDefault(x => x.Descricao == "Offset")?.Valor ?? "0", out double offset); + double leituraBruta = + sensor? + .ValoresLeituras? + .FirstOrDefault( + x => x.funcao == FuncoesPinout.LeituraBruta + )? + .atual? + .valor + ?? 0; - massaCalc_Kg = leituraBruta + offset; + double.TryParse( + sensor? + .Parametros? + .FirstOrDefault( + x => x.Descricao == "Offset" + )? + .Valor + ?? "0", + out double offset + ); - volumeMedido_L = DensidadeProduto > 0 ? massaCalc_Kg / DensidadeProduto : 0; + massaCalc_Kg = + leituraBruta + offset; - volumeMedido_L = Clamp(volumeMedido_L, 0, CapacidadeLitros); + volumeMedido_L = + DensidadeProduto > 0 + ? massaCalc_Kg / DensidadeProduto + : 0; + + if ( + double.IsNaN(volumeMedido_L) || + double.IsInfinity(volumeMedido_L) + ) + { + volumeMedido_L = 0; + } + + volumeMedido_L = Clamp( + volumeMedido_L, + 0, + CapacidadeLitros + ); + } + + private void AtualizarVolumeReservatorioFiltrado( + double volumeMedido_L, + double deltaTempo_s) + { + LeituraReservatorioConfiavel = true; + + if (!_volumeFiltradoInicializado) + { + VolumeAtual_L = volumeMedido_L; + _volumeAnterior_L = volumeMedido_L; + + _volumeFiltradoInicializado = true; + return; + } + + double salto = + volumeMedido_L - VolumeAtual_L; + + /* + * Durante uma operação, uma variação instantânea muito grande + * normalmente é balanço da calda, leitura inválida ou reconexão. + * Mantemos o último valor e aguardamos a próxima amostra. + */ + if ( + OperacaoAtiva && + Math.Abs(salto) > MaxSaltoVolumeOperacao_L + ) + { + LeituraReservatorioConfiavel = false; + return; + } + + double alpha = + OperacaoAtiva + ? AlphaVolumeReservatorio + : 1.0; + + VolumeAtual_L = + VolumeAtual_L * (1.0 - alpha) + + volumeMedido_L * alpha; + + VolumeAtual_L = Clamp( + VolumeAtual_L, + 0, + CapacidadeLitros + ); + } + + private double CalcularDeltaVolumeMassa( + double deltaTempo_s) + { + double deltaVolume = + Math.Max( + 0, + _volumeAnterior_L - VolumeAtual_L + ); + + _volumeAnterior_L = + VolumeAtual_L; + + if (!LeituraReservatorioConfiavel) + return 0; + + if (deltaVolume <= 0) + return 0; + + double vazaoMaxima_mLs = + Math.Max( + ObterVazaoTotalEstimada_mLs( + Math.Max(BicosAtivos, QuantidadeBicos) + ), + VazaoReferencia_mLs + ); + + double maxEsperado_L = + vazaoMaxima_mLs * + Math.Max(deltaTempo_s, 0) / + 1000.0 * + FatorMaximoDeltaMassa; + + /* + * Dá uma tolerância mínima porque o sensor de massa + * é mais lento e pode concentrar a queda em uma amostra. + */ + maxEsperado_L = + Math.Max(maxEsperado_L, 0.25); + + if ( + OperacaoAtiva && + deltaVolume > maxEsperado_L + ) + { + return 0; + } + + return deltaVolume; + } + + // ============================================================ + // Fluxo e curva por quantidade de bicos + // ============================================================ + + private static Dictionary MontarCurvaVazaoPadrao( + int quantidadeBicos, + double vazaoPorBicoFallback_mLs, + IReadOnlyDictionary curvaCustomizada) + { + /* + * Pontos 1..4 vêm do comportamento observado na operação. + * O ponto de 7 bicos usa o pior caso já medido em bancada/campo. + * 5 e 6 são interpolados de forma conservadora. + */ + var curva = + new Dictionary + { + { 0, 0.0 }, + { 1, 17.5 }, + { 2, 20.8 }, + { 3, 24.8 }, + { 4, 25.0 }, + { 5, 36.7 }, + { 6, 48.3 }, + { 7, 60.0 }, + }; + + if (curvaCustomizada != null) + { + foreach (var item in curvaCustomizada) + { + if ( + item.Key >= 0 && + !double.IsNaN(item.Value) && + !double.IsInfinity(item.Value) && + item.Value >= 0 + ) + { + curva[item.Key] = + item.Value; + } + } + } + + int maximoNecessario = + Math.Max(quantidadeBicos, 7); + + for (int qtd = 1; qtd <= maximoNecessario; qtd++) + { + if (curva.ContainsKey(qtd)) + continue; + + /* + * Para equipamentos com mais de sete bicos, usa crescimento + * linear conservador a partir do último ponto conhecido. + */ + double ultimoConhecido = + curva + .Where(x => x.Key < qtd) + .OrderByDescending(x => x.Key) + .Select(x => x.Value) + .FirstOrDefault(); + + double incremento = + Math.Max( + vazaoPorBicoFallback_mLs, + 1.0 + ); + + curva[qtd] = + ultimoConhecido + incremento; + } + + return curva; + } + + public double ObterVazaoTotalEstimada_mLs( + int quantidadeBicosAtivos) + { + quantidadeBicosAtivos = + Math.Max(0, quantidadeBicosAtivos); + + if ( + _vazaoTotalPorQuantidadeBicos_mLs + .TryGetValue( + quantidadeBicosAtivos, + out double vazao + ) + ) + { + return Math.Max(0, vazao); + } + + if (quantidadeBicosAtivos <= 0) + return 0; + + int ultimoPonto = + _vazaoTotalPorQuantidadeBicos_mLs + .Keys + .Where(x => x > 0) + .DefaultIfEmpty(0) + .Max(); + + double ultimaVazao = + _vazaoTotalPorQuantidadeBicos_mLs + .TryGetValue( + ultimoPonto, + out double valor + ) + ? valor + : 0; + + int bicosAdicionais = + Math.Max( + 0, + quantidadeBicosAtivos - ultimoPonto + ); + + return Math.Max( + 0, + ultimaVazao + + bicosAdicionais * + Math.Max( + VazaoPorBicoBase_mLs, + 1.0 + ) + ); } private double ObterVazaoFluxo_mLs() { var dados = Variaveis - .OperacaoEmAndamento + .OperacaoEmAndamento? .DispAtu? .Dados; @@ -315,9 +903,11 @@ namespace AgroBase.Services double vazao = dados.VazaoFluxoMLpSReferencia; - if (double.IsNaN(vazao) || + if ( + double.IsNaN(vazao) || double.IsInfinity(vazao) || - vazao < 0) + vazao < 0 + ) { return 0; } @@ -325,107 +915,441 @@ namespace AgroBase.Services return vazao; } + private bool ValidarVazaoFluxo( + double vazao_mLs, + int bicosAtivos) + { + if (!Pulverizando || bicosAtivos <= 0) + return false; + + if ( + double.IsNaN(vazao_mLs) || + double.IsInfinity(vazao_mLs) || + vazao_mLs < VazaoMinimaValida_mLs + ) + { + return false; + } + + double referencia_mLs = + Math.Max( + ObterVazaoTotalEstimada_mLs( + bicosAtivos + ), + VazaoMinimaValida_mLs + ); + + double maximoAceitavel_mLs = + Math.Max( + referencia_mLs * + FatorMaximoVazaoSobreCurva, + ObterVazaoTotalEstimada_mLs( + QuantidadeBicos + ) * + 1.5 + ); + + return vazao_mLs <= maximoAceitavel_mLs; + } + private int ObterQuantidadeBicosAtivos() { - return Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores?.Count(x => x.Inicializado && x.ComandoAtuar) ?? 0; - } - - private void Inicializar(double distanciaTotalMetrosAtual, double tempoTotalSegundosAtual) - { - _inicializado = true; - - _volumeInicialInstancia_L = VolumeAtual_L; - _volumeAnterior_L = VolumeAtual_L; - - _distanciaTotalAnterior_m = distanciaTotalMetrosAtual; - _tempoTotalAnterior_s = tempoTotalSegundosAtual; - } - - private void AtualizarJanelaDistancia(double deltaDist_m, double deltaVolume_L) - { - if (deltaDist_m <= 0 || deltaVolume_L <= 0) - return; - - _janelaDist.Enqueue((deltaDist_m, deltaVolume_L)); - - _somaDistanciaJanela_m += deltaDist_m; - _somaVolumeDistJanela_L += deltaVolume_L; - - while (_somaDistanciaJanela_m > JanelaAutonomia_m && _janelaDist.Count > 0) - { - var old = _janelaDist.Dequeue(); - - _somaDistanciaJanela_m -= old.dist_m; - _somaVolumeDistJanela_L -= old.vol_L; - } - - if (_somaDistanciaJanela_m > 0.5) - { - LPorMetro = _somaVolumeDistJanela_L / _somaDistanciaJanela_m; - - const double alpha = 0.2; - - LPorMetroHistorico = LPorMetroHistorico <= 0 - ? LPorMetro - : LPorMetroHistorico * (1.0 - alpha) + LPorMetro * alpha; - } - } - - private void AtualizarJanelaTempo(double deltaTempo_s, double deltaVolume_L) - { - if (deltaTempo_s <= 0 || deltaVolume_L <= 0) - return; - - _janelaTempo.Enqueue((deltaTempo_s, deltaVolume_L)); - - _somaTempoJanela_s += deltaTempo_s; - _somaVolumeTempoJanela_L += deltaVolume_L; - - while (_somaTempoJanela_s > JanelaTempo_s && _janelaTempo.Count > 0) - { - var old = _janelaTempo.Dequeue(); - - _somaTempoJanela_s -= old.tempo_s; - _somaVolumeTempoJanela_L -= old.vol_L; - } - - if (_somaTempoJanela_s > 1.0) - LPorMinuto = (_somaVolumeTempoJanela_L / _somaTempoJanela_s) * 60.0; - } - - private void AtualizarHerbicidaPorAtuacao() - { - int qtdAtuacoes = + return Variaveis - .OperacaoEmAndamento + .OperacaoEmAndamento? .DispAtu? .Dados? .BicosPulverizadores? - .Sum(x => x?.Atuacoes ?? 0) + .Count( + x => + x != null && + x.Inicializado && + x.Comandar && + x.ComandoAtuar + ) ?? 0; + } - if (qtdAtuacoes > 0) + private int ObterTotalAtuacoes() + { + return + Variaveis + .OperacaoEmAndamento? + .DispAtu? + .Dados? + .BicosPulverizadores? + .Sum( + x => x?.Atuacoes ?? 0 + ) + ?? 0; + } + + private double CalcularDeltaVolumeFluxo( + double deltaTempo_s) + { + if ( + !Pulverizando || + deltaTempo_s <= 0 || + !VazaoFluxoValida || + VazaoFluxoAtual_mLs <= 0 + ) { - HerbicidaPorAtuacao_L = - VolumeConsumidoOperacao_L / - qtdAtuacoes; + return 0; } - else + + return + VazaoFluxoAtual_mLs * + deltaTempo_s / + 1000.0; + } + + private double EscolherDeltaVolumeParaTaxa( + double deltaMassa_L, + double deltaFluxo_L) + { + /* + * Na operação analisada, a integral do fluxo e a queda do + * reservatório fecharam com diferença inferior a 1%. + * Por isso o fluxo é a fonte primária para taxas rápidas. + */ + if (deltaFluxo_L > 0) + return deltaFluxo_L; + + if (Pulverizando && deltaMassa_L > 0) + return deltaMassa_L; + + return 0; + } + + // ============================================================ + // Janelas e taxas + // ============================================================ + + private void AtualizarJanelaPulverizacaoDistancia( + double deltaDist_m, + double deltaVolume_L) + { + if ( + deltaDist_m <= 0 || + deltaVolume_L <= 0 + ) { - HerbicidaPorAtuacao_L = 0; + return; + } + + _janelaPulverizacaoDist.Enqueue( + (deltaDist_m, deltaVolume_L) + ); + + _somaDistanciaPulverizacao_m += + deltaDist_m; + + _somaVolumePulverizacaoDist_L += + deltaVolume_L; + + while ( + _somaDistanciaPulverizacao_m > + JanelaPulverizacaoContinua_m && + _janelaPulverizacaoDist.Count > 0 + ) + { + var old = + _janelaPulverizacaoDist.Dequeue(); + + _somaDistanciaPulverizacao_m -= + old.dist_m; + + _somaVolumePulverizacaoDist_L -= + old.vol_L; + } + + if ( + _somaDistanciaPulverizacao_m > + MinDistanciaTaxa_m + ) + { + LPorMetro = + _somaVolumePulverizacaoDist_L / + _somaDistanciaPulverizacao_m; + + LPorMetroHistorico = + LPorMetroHistorico <= 0 + ? LPorMetro + : LPorMetroHistorico * + (1.0 - AlphaHistorico) + + LPorMetro * + AlphaHistorico; } } - private void AtualizarAutonomia(double velocidadeAtual_m_s) + private void AtualizarJanelaPulverizacaoTempo( + double deltaTempo_s, + double deltaVolume_L) { - double consumoRef_LPorMetro = CalcularLPorMetroReferencia(velocidadeAtual_m_s); - - if (VolumeAtual_L > 0 && consumoRef_LPorMetro > 0) + if ( + deltaTempo_s <= 0 || + deltaVolume_L <= 0 + ) { - DistanciaMaximaTeorica_m = VolumeAtual_L / consumoRef_LPorMetro; + return; + } - double margem = Clamp(MargemSegurancaAutonomia, 0.0, 0.9); - DistanciaMaximaSegura_m = DistanciaMaximaTeorica_m * (1.0 - margem); + _janelaPulverizacaoTempo.Enqueue( + (deltaTempo_s, deltaVolume_L) + ); + + _somaTempoPulverizacao_s += + deltaTempo_s; + + _somaVolumePulverizacaoTempo_L += + deltaVolume_L; + + while ( + _somaTempoPulverizacao_s > + JanelaTempoPulverizacao_s && + _janelaPulverizacaoTempo.Count > 0 + ) + { + var old = + _janelaPulverizacaoTempo.Dequeue(); + + _somaTempoPulverizacao_s -= + old.tempo_s; + + _somaVolumePulverizacaoTempo_L -= + old.vol_L; + } + + if ( + _somaTempoPulverizacao_s > + MinTempoTaxa_s + ) + { + LPorMinuto = + _somaVolumePulverizacaoTempo_L / + _somaTempoPulverizacao_s * + 60.0; + } + } + + private void AtualizarJanelaOperacional( + double deltaDist_m, + double deltaTempo_s, + double deltaVolume_L) + { + deltaDist_m = + Math.Max(0, deltaDist_m); + + deltaTempo_s = + Math.Max(0, deltaTempo_s); + + deltaVolume_L = + Math.Max(0, deltaVolume_L); + + _janelaOperacional.Enqueue( + ( + deltaDist_m, + deltaTempo_s, + deltaVolume_L + ) + ); + + _somaDistanciaOperacional_m += + deltaDist_m; + + _somaTempoOperacional_s += + deltaTempo_s; + + _somaVolumeOperacional_L += + deltaVolume_L; + + while ( + ( + _somaDistanciaOperacional_m > + JanelaOperacional_m || + _somaTempoOperacional_s > + JanelaOperacionalTempo_s + ) && + _janelaOperacional.Count > 0 + ) + { + var old = + _janelaOperacional.Dequeue(); + + _somaDistanciaOperacional_m -= + old.dist_m; + + _somaTempoOperacional_s -= + old.tempo_s; + + _somaVolumeOperacional_L -= + old.vol_L; + } + + if ( + _somaDistanciaOperacional_m > + MinDistanciaTaxa_m + ) + { + LPorMetroOperacional = + _somaVolumeOperacional_L / + _somaDistanciaOperacional_m; + } + + if ( + deltaDist_m > 0 && + _somaDistanciaOperacional_m >= + MinDistanciaHistoricoOperacional_m && + LPorMetroOperacional > 0 + ) + { + LPorMetroOperacionalHistorico = + LPorMetroOperacionalHistorico <= 0 + ? LPorMetroOperacional + : LPorMetroOperacionalHistorico * + (1.0 - AlphaHistorico) + + LPorMetroOperacional * + AlphaHistorico; + + AutonomiaOperacionalComHistorico = + true; + } + } + + // ============================================================ + // Autonomia + // ============================================================ + + private double ObterVelocidadeReferencia_m_s( + double velocidadeAtual_m_s) + { + if ( + !double.IsNaN(velocidadeAtual_m_s) && + !double.IsInfinity(velocidadeAtual_m_s) && + velocidadeAtual_m_s >= + MinVelocidadeReferencia_m_s + ) + { + return velocidadeAtual_m_s; + } + + return + FuncoesMatematicas + .CalculaVelocidadeMsPercentual( + Variaveis + .OperacaoEmAndamento? + .Parametros? + .Controle? + .MovVelocidadeCErvasPercent + ?? 30 + ); + } + + private double CalcularLPorMetroOperacionalReferencia() + { + if ( + AutonomiaOperacionalComHistorico && + LPorMetroOperacionalHistorico > 0 + ) + { + FonteAutonomiaOperacional = + "historico_operacional"; + + /* + * Usa o maior entre a janela recente e o histórico suavizado. + * Assim uma região recente mais carregada de ervas eleva + * imediatamente a estimativa, mas uma região limpa não + * derruba a autonomia de forma otimista demais. + */ + return Math.Max( + LPorMetroOperacional, + LPorMetroOperacionalHistorico + ); + } + + if (LPorMetroOperacional > 0) + { + FonteAutonomiaOperacional = + "janela_operacional_inicial"; + + return Math.Max( + LPorMetroOperacional, + LPorMetroBase + ); + } + + FonteAutonomiaOperacional = + "fallback_campo"; + + return LPorMetroBase; + } + + private double CalcularLPorMetroPulverizacaoContinuaReferencia( + double velocidadeAtual_m_s) + { + if (LPorMetroHistorico > 0) + return LPorMetroHistorico; + + if (LPorMetro > 0) + return LPorMetro; + + double velocidadeReferencia_m_s = + ObterVelocidadeReferencia_m_s( + velocidadeAtual_m_s + ); + + double vazaoReferencia_LMin = + VazaoTotalTodosBicos_LMin; + + if ( + velocidadeReferencia_m_s > 0.01 && + vazaoReferencia_LMin > 0 + ) + { + double velocidade_m_min = + velocidadeReferencia_m_s * 60.0; + + return + vazaoReferencia_LMin / + velocidade_m_min; + } + + return 0; + } + + private void AtualizarAutonomia( + double velocidadeAtual_m_s) + { + LPorMetroReferenciaOperacional = + CalcularLPorMetroOperacionalReferencia(); + + LPorMetroReferenciaPulverizacaoContinua = + CalcularLPorMetroPulverizacaoContinuaReferencia( + velocidadeAtual_m_s + ); + + double margem = + Clamp( + MargemSegurancaAutonomia, + 0.0, + 0.90 + ); + + double volumeUtil_L = + VolumeUtilAutonomia_L; + + if ( + volumeUtil_L > 0 && + LPorMetroReferenciaOperacional > 0 + ) + { + DistanciaMaximaTeorica_m = + volumeUtil_L / + LPorMetroReferenciaOperacional; + + DistanciaMaximaSegura_m = + DistanciaMaximaTeorica_m * + (1.0 - margem); } else { @@ -433,16 +1357,42 @@ namespace AgroBase.Services DistanciaMaximaSegura_m = 0; } - double consumoTempoRef_LMin = LPorMinuto > 0 - ? LPorMinuto - : LPorMinutoEstimadoBicos; - - if (VolumeAtual_L > 0 && consumoTempoRef_LMin > 0) + if ( + volumeUtil_L > 0 && + LPorMetroReferenciaPulverizacaoContinua > 0 + ) { - double margem = Clamp(MargemSegurancaAutonomia, 0.0, 0.9); + DistanciaMaximaTeoricaPulverizacaoContinua_m = + volumeUtil_L / + LPorMetroReferenciaPulverizacaoContinua; + DistanciaMaximaSeguraPulverizacaoContinua_m = + DistanciaMaximaTeoricaPulverizacaoContinua_m * + (1.0 - margem); + } + else + { + DistanciaMaximaTeoricaPulverizacaoContinua_m = + 0; + + DistanciaMaximaSeguraPulverizacaoContinua_m = + 0; + } + + double consumoTempoRef_LMin = + LPorMinuto > 0 + ? LPorMinuto + : LPorMinutoEstimadoBicos; + + if ( + volumeUtil_L > 0 && + consumoTempoRef_LMin > 0 + ) + { TempoEstimadoMinutosPulverizando = - (VolumeAtual_L / consumoTempoRef_LMin) * (1.0 - margem); + volumeUtil_L / + consumoTempoRef_LMin * + (1.0 - margem); } else { @@ -450,63 +1400,92 @@ namespace AgroBase.Services } } - private double CalcularLPorMetroReferencia(double velocidadeAtual_m_s) - { - // 1. Histórico real já consolidado - if (LPorMetroHistorico > 0) - return LPorMetroHistorico; - - // 2. Janela atual real - if (LPorMetro > 0) - return LPorMetro; - - // 3. Pré-cálculo enquanto o robô ainda está parado - double velocidadeReferencia_m_s = velocidadeAtual_m_s; - - if (velocidadeReferencia_m_s < 0.10) - { - velocidadeReferencia_m_s = - FuncoesMatematicas.CalculaVelocidadeMsPercentual( - Variaveis.OperacaoEmAndamento - .Parametros? - .Controle? - .MovVelocidadeCErvasPercent ?? 30 - ); - } - - // Para a verificação pré-corredor, usar o pior caso: - // todos os bicos pulverizando. - double vazaoReferencia_LMin = VazaoTotalTodosBicos_LMin; - - if (velocidadeReferencia_m_s > 0.01 && - vazaoReferencia_LMin > 0) - { - double velocidade_m_min = velocidadeReferencia_m_s * 60.0; - return vazaoReferencia_LMin / velocidade_m_min; - } - - return 0; - } - - public bool ConseguePulverizarRua(double comprimentoRua_m) + public bool ConseguePulverizarRua( + double comprimentoRua_m) { if (comprimentoRua_m <= 0) return true; - return DistanciaMaximaSegura_m >= comprimentoRua_m; + return + Iniciado && + PulverizadorLiberado && + DistanciaMaximaSegura_m >= + comprimentoRua_m; } - public void DefinirVolumeAtual(double volume_L) - { - volume_L = Clamp(volume_L, 0, CapacidadeLitros); + // ============================================================ + // Métricas auxiliares + // ============================================================ + private void AtualizarHerbicidaPorAtuacao() + { + int atuacoesOperacao = + Math.Max( + 0, + ObterTotalAtuacoes() - + _atuacoesInicioOperacao + ); + + if (atuacoesOperacao <= 0) + { + HerbicidaPorAtuacao_L = 0; + return; + } + + double consumoOperacao_L = + VolumeConsumidoFluxoOperacao_L > 0 + ? VolumeConsumidoFluxoOperacao_L + : VolumeConsumidoOperacao_L; + + HerbicidaPorAtuacao_L = + consumoOperacao_L / + atuacoesOperacao; + } + + public void DefinirVolumeAtual( + double volume_L) + { + volume_L = Clamp( + volume_L, + 0, + CapacidadeLitros + ); + + VolumeAtualBruto_L = volume_L; VolumeAtual_L = volume_L; + _volumeAnterior_L = volume_L; _volumeInicialInstancia_L = volume_L; - ResetarJanelas(); + _volumeFiltradoInicializado = true; + LeituraReservatorioConfiavel = true; + + ResetarJanelasOperacao(); } + private void Inicializar( + double distanciaTotalMetrosAtual, + double tempoTotalSegundosAtual) + { + _inicializado = true; + + _volumeInicialInstancia_L = + VolumeAtual_L; + + _volumeAnterior_L = + VolumeAtual_L; + + _distanciaTotalAnterior_m = + distanciaTotalMetrosAtual; + + _tempoTotalAnterior_s = + tempoTotalSegundosAtual; + } + + // ============================================================ + // Reset + // ============================================================ + private void ResetarTudo() { Iniciado = false; @@ -515,7 +1494,10 @@ namespace AgroBase.Services MassaAtual_Kg = 0; MassaAtual_Kg_Calc = 0; + + VolumeAtualBruto_L = 0; VolumeAtual_L = 0; + LeituraReservatorioConfiavel = false; _volumeInicialInstancia_L = 0; _volumeInicialOperacao_L = 0; @@ -525,22 +1507,41 @@ namespace AgroBase.Services PulverizadorLiberado = false; VazaoFluxoAtual_mLs = 0; + VazaoFluxoValida = false; VazaoEstimativaBicos_mLs = 0; VazaoReferencia_mLs = 0; LPorMetro = 0; LPorMetroHistorico = 0; - LPorMetroBase = 0; + + LPorMetroOperacional = 0; + LPorMetroOperacionalHistorico = 0; + + LPorMetroReferenciaOperacional = 0; + LPorMetroReferenciaPulverizacaoContinua = 0; + LPorMinuto = 0; LPorMinutoEstimadoBicos = 0; + AutonomiaOperacionalComHistorico = false; + FonteAutonomiaOperacional = "indisponivel"; + DistanciaMaximaTeorica_m = 0; DistanciaMaximaSegura_m = 0; + + DistanciaMaximaTeoricaPulverizacaoContinua_m = 0; + DistanciaMaximaSeguraPulverizacaoContinua_m = 0; + TempoEstimadoMinutosPulverizando = 0; + VolumeConsumidoFluxoTotal_L = 0; + VolumeConsumidoFluxoOperacao_L = 0; + HerbicidaPorAtuacao_L = 0; _inicializado = false; + _volumeFiltradoInicializado = false; + _volumeAnterior_L = 0; _distanciaTotalAnterior_m = 0; _tempoTotalAnterior_s = 0; @@ -549,31 +1550,58 @@ namespace AgroBase.Services _distanciaInicioOperacaoPendente_m = 0; _tempoInicioOperacaoPendente_s = 0; - ResetarJanelas(); + _atuacoesInicioOperacao = 0; + + ResetarJanelasOperacao(); } - private void ResetarJanelas() + private void ResetarJanelasOperacao() { - _janelaDist.Clear(); - _janelaTempo.Clear(); + _janelaPulverizacaoDist.Clear(); + _janelaPulverizacaoTempo.Clear(); + _janelaOperacional.Clear(); - _somaDistanciaJanela_m = 0; - _somaVolumeDistJanela_L = 0; + _somaDistanciaPulverizacao_m = 0; + _somaVolumePulverizacaoDist_L = 0; - _somaTempoJanela_s = 0; - _somaVolumeTempoJanela_L = 0; + _somaTempoPulverizacao_s = 0; + _somaVolumePulverizacaoTempo_L = 0; + + _somaDistanciaOperacional_m = 0; + _somaTempoOperacional_s = 0; + _somaVolumeOperacional_L = 0; LPorMetro = 0; + LPorMetroHistorico = 0; + + LPorMetroOperacional = 0; + LPorMetroOperacionalHistorico = 0; + + LPorMetroReferenciaOperacional = 0; + LPorMetroReferenciaPulverizacaoContinua = 0; + LPorMinuto = 0; + + AutonomiaOperacionalComHistorico = false; + FonteAutonomiaOperacional = "fallback_campo"; } - private static double Clamp(double value, double min, double max) + private static double Clamp( + double value, + double min, + double max) { - return FuncoesMatematicas.Clamp(value, min, max); + return + FuncoesMatematicas.Clamp( + value, + min, + max + ); } - - // ---------------- AGITADOR ---------------- + // ============================================================ + // AGITADOR + // ============================================================ private bool _agitadorIntermitenteLigado = false; private DateTime _agitadorUltimaTroca = DateTime.MinValue; @@ -588,34 +1616,51 @@ namespace AgroBase.Services private int AtuAgitadorTempoDesligadoMs = 30000; private int AtuAgitadorBoostRetomadaMs = 15000; - private double PotenciaLimitePorNivel(double nivelPct) + private double PotenciaLimitePorNivel( + double nivelPct) { - if (nivelPct <= 5) return 0; - if (nivelPct <= 15) return 55; - if (nivelPct <= 30) return 65; - if (nivelPct <= 60) return 75; + if (nivelPct <= 5) + return 0; + + if (nivelPct <= 15) + return 55; + + if (nivelPct <= 30) + return 65; + + if (nivelPct <= 60) + return 75; + return 100; } - public void AtualizarAgitadorCalda(ModoAgitadorCalda modo) + public void AtualizarAgitadorCalda( + ModoAgitadorCalda modo) { - var op = Variaveis.OperacaoEmAndamento; + var op = + Variaveis.OperacaoEmAndamento; - if (op?.Sensoriamento?.Operacao?.Calibrando == true) + if ( + op? + .Sensoriamento? + .Operacao? + .Calibrando == true + ) { - /* - * Durante a calibração, RealizarTestesIniciais() - * é o proprietário temporário do agitador. - */ return; } - var agitador = op?.DispAtu?.Dados?.AgitadorDeCalda; + var agitador = + op? + .DispAtu? + .Dados? + .AgitadorDeCalda; if (!(agitador?.Inicializado ?? false)) return; - var controle = op.Parametros?.Controle; + var controle = + op.Parametros?.Controle; if (controle == null) { @@ -625,7 +1670,12 @@ namespace AgroBase.Services return; } - bool operacaoConcluida = op.Sensoriamento?.Operacao?.StatusOperacaoAtual == StatusOperacao.Concluido; + bool operacaoConcluida = + op + .Sensoriamento? + .Operacao? + .StatusOperacaoAtual == + StatusOperacao.Concluido; bool operacaoAtiva = op.Sensoriamento.Operacao.OperacaoIniciada && @@ -633,20 +1683,37 @@ namespace AgroBase.Services !operacaoConcluida && PulverizadorLiberado; - bool bicosAtuando = op.DispAtu?.Dados?.BicosPulverizadores?.Any(x => x.Comandar && x.ComandoAtuar) ?? false; + bool bicosAtuando = + op + .DispAtu? + .Dados? + .BicosPulverizadores? + .Any( + x => + x.Comandar && + x.ComandoAtuar + ) + ?? false; double potenciaFinal = 0; DateTime agora = DateTime.Now; - if (operacaoAtiva && !_agitadorOperacaoAtivaAnterior) + if ( + operacaoAtiva && + !_agitadorOperacaoAtivaAnterior + ) { - _agitadorBoostAte = agora.AddMilliseconds(AtuAgitadorBoostRetomadaMs); + _agitadorBoostAte = + agora.AddMilliseconds( + AtuAgitadorBoostRetomadaMs + ); _agitadorUltimaTroca = agora; _agitadorIntermitenteLigado = true; } - bool emBoost = agora < _agitadorBoostAte; + bool emBoost = + agora < _agitadorBoostAte; switch (modo) { @@ -659,15 +1726,18 @@ namespace AgroBase.Services { if (emBoost) { - potenciaFinal = AtuAgitadorPotenciaBoost; + potenciaFinal = + AtuAgitadorPotenciaBoost; } else if (bicosAtuando) { - potenciaFinal = AtuAgitadorPotenciaContinua; + potenciaFinal = + AtuAgitadorPotenciaContinua; } else { - potenciaFinal = AtuAgitadorPotenciaManutencao; + potenciaFinal = + AtuAgitadorPotenciaManutencao; } } break; @@ -681,37 +1751,65 @@ namespace AgroBase.Services } else if (emBoost) { - potenciaFinal = AtuAgitadorPotenciaBoost; + potenciaFinal = + AtuAgitadorPotenciaBoost; } else { - int tempoAtualMs = _agitadorIntermitenteLigado ? AtuAgitadorTempoLigadoMs : AtuAgitadorTempoDesligadoMs; + int tempoAtualMs = + _agitadorIntermitenteLigado + ? AtuAgitadorTempoLigadoMs + : AtuAgitadorTempoDesligadoMs; - if ((agora - _agitadorUltimaTroca).TotalMilliseconds >= tempoAtualMs) + if ( + ( + agora - + _agitadorUltimaTroca + ).TotalMilliseconds >= + tempoAtualMs + ) { - _agitadorIntermitenteLigado = !_agitadorIntermitenteLigado; + _agitadorIntermitenteLigado = + !_agitadorIntermitenteLigado; - _agitadorUltimaTroca = agora; + _agitadorUltimaTroca = + agora; } - potenciaFinal = _agitadorIntermitenteLigado ? AtuAgitadorPotenciaIntermitente : 0; + potenciaFinal = + _agitadorIntermitenteLigado + ? AtuAgitadorPotenciaIntermitente + : 0; } break; } - potenciaFinal = Math.Min(potenciaFinal, PotenciaLimitePorNivel(PercentualAtual)); + potenciaFinal = + Math.Min( + potenciaFinal, + PotenciaLimitePorNivel( + PercentualAtual + ) + ); - potenciaFinal = Clamp(potenciaFinal, 0, 100); + potenciaFinal = + Clamp( + potenciaFinal, + 0, + 100 + ); + + agitador.PotenciaSP = + potenciaFinal; - /* - * Este método é a fonte de verdade do agitador. - */ - agitador.PotenciaSP = potenciaFinal; agitador.PressaoSP = -1; - agitador.ComandoAtuar = agitador.Comandar && potenciaFinal > 0; + agitador.ComandoAtuar = + agitador.Comandar && + potenciaFinal > 0; - _agitadorOperacaoAtivaAnterior = operacaoAtiva; + _agitadorOperacaoAtivaAnterior = + operacaoAtiva; } } } \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 4697289fa..c58aa301d 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py @@ -35,20 +35,28 @@ class CameraIMU(ModuloDiagnosticoBase): timeout_imu_s=0.40, timeout_camera_s=2.0, max_disagreement_deg=4.0, - roll_attention_deg=7.0, - roll_risk_deg=11.0, - roll_critical_deg=16.0, - roll_emergency_deg=22.0, - pitch_attention_deg=8.0, - pitch_risk_deg=14.0, - pitch_critical_deg=20.0, - pitch_emergency_deg=28.0, + + # Limites principais vindos do equipamento. + # No rover: + # roll = inclinação frontal + # pitch = inclinação lateral + roll_max_default_deg=45.0, + pitch_max_default_deg=25.0, + + # Níveis derivados do limite máximo configurado. + angulo_attention_factor=0.50, + angulo_risk_factor=0.75, + angulo_emergency_factor=1.20, + limites_refresh_s=1.0, + + # Taxas continuam independentes do ângulo máximo. roll_rate_attention_dps=25.0, roll_rate_risk_dps=40.0, roll_rate_critical_dps=65.0, pitch_rate_attention_dps=25.0, pitch_rate_risk_dps=45.0, pitch_rate_critical_dps=75.0, + filtro_alpha=0.35, historico_len=100, modulos_imu=None, @@ -61,15 +69,35 @@ class CameraIMU(ModuloDiagnosticoBase): self.timeout_camera_s = float(timeout_camera_s) self.max_disagreement_deg = float(max_disagreement_deg) - self.roll_attention_deg = float(roll_attention_deg) - self.roll_risk_deg = float(roll_risk_deg) - self.roll_critical_deg = float(roll_critical_deg) - self.roll_emergency_deg = float(roll_emergency_deg) + # ============================================================ + # Limites angulares do equipamento + # ============================================================ - self.pitch_attention_deg = float(pitch_attention_deg) - self.pitch_risk_deg = float(pitch_risk_deg) - self.pitch_critical_deg = float(pitch_critical_deg) - self.pitch_emergency_deg = float(pitch_emergency_deg) + self.roll_max_default_deg = float(roll_max_default_deg) + self.pitch_max_default_deg = float(pitch_max_default_deg) + + self.angulo_attention_factor = float(angulo_attention_factor) + self.angulo_risk_factor = float(angulo_risk_factor) + self.angulo_emergency_factor = float(angulo_emergency_factor) + + self.limites_refresh_s = max(float(limites_refresh_s), 0.25) + self._ultimo_refresh_limites = 0.0 + + # Valores iniciais seguros. Serão substituídos pelos dados do Redis. + self.roll_max_deg = self.roll_max_default_deg + self.pitch_max_deg = self.pitch_max_default_deg + + self.roll_attention_deg = 0.0 + self.roll_risk_deg = 0.0 + self.roll_critical_deg = 0.0 + self.roll_emergency_deg = 0.0 + + self.pitch_attention_deg = 0.0 + self.pitch_risk_deg = 0.0 + self.pitch_critical_deg = 0.0 + self.pitch_emergency_deg = 0.0 + + self._recalcular_faixas_angulares() self.roll_rate_attention_dps = float(roll_rate_attention_dps) self.roll_rate_risk_dps = float(roll_rate_risk_dps) @@ -108,6 +136,9 @@ class CameraIMU(ModuloDiagnosticoBase): self.roll_rate_filtrado = 0.0 self.pitch_rate_filtrado = 0.0 + # Não usar self.last_data para controlar a inicialização. + # Pode existir last_data de uma condição "sem_imu". + self._filtro_fusao_inicializado = False self.roll_hist = deque(maxlen=historico_len) self.pitch_hist = deque(maxlen=historico_len) @@ -596,6 +627,120 @@ class CameraIMU(ModuloDiagnosticoBase): return fontes, ignoradas, desabilitadas + def _validar_limite_angulo(self, valor, valor_atual): + """ + Valida um limite angular vindo do Redis. + + Em caso de valor ausente ou inválido, mantém o último valor válido. + Isso impede que uma falha momentânea no contexto altere a segurança. + """ + try: + valor = float(valor) + + if not math.isfinite(valor): + return float(valor_atual) + + # Evita configurações claramente inválidas. + if valor < 5.0 or valor > 85.0: + return float(valor_atual) + + return valor + + except Exception: + return float(valor_atual) + + def _recalcular_faixas_angulares(self): + """ + Deriva attention, risk, critical e emergency usando os dois + limites principais definidos pelo operador. + + No contrato físico deste rover: + - roll = inclinação frontal + - pitch = inclinação lateral + """ + + self.roll_attention_deg = ( + self.roll_max_deg * self.angulo_attention_factor + ) + self.roll_risk_deg = ( + self.roll_max_deg * self.angulo_risk_factor + ) + self.roll_critical_deg = self.roll_max_deg + self.roll_emergency_deg = min( + 89.0, + max( + self.roll_max_deg + 2.0, + self.roll_max_deg * self.angulo_emergency_factor, + ) + ) + + self.pitch_attention_deg = ( + self.pitch_max_deg * self.angulo_attention_factor + ) + self.pitch_risk_deg = ( + self.pitch_max_deg * self.angulo_risk_factor + ) + self.pitch_critical_deg = self.pitch_max_deg + self.pitch_emergency_deg = min( + 89.0, + max( + self.pitch_max_deg + 2.0, + self.pitch_max_deg * self.angulo_emergency_factor, + ) + ) + + def _atualizar_limites_equipamento(self, forcar=False): + """ + Atualiza periodicamente os limites definidos pelo equipamento. + + Não consulta o Redis a 50 Hz, pois esses parâmetros mudam raramente. + """ + agora = time.perf_counter() + + if ( + not forcar + and (agora - self._ultimo_refresh_limites) < self.limites_refresh_s + ): + return + + self._ultimo_refresh_limites = agora + + equipamento = ContextoGlobalRedis.get_equipamento() or {} + + roll_anterior = self.roll_max_deg + pitch_anterior = self.pitch_max_deg + + novo_roll_max = self._validar_limite_angulo( + equipamento.get("angulo_roll_max"), + self.roll_max_deg, + ) + + novo_pitch_max = self._validar_limite_angulo( + equipamento.get("angulo_pitch_max"), + self.pitch_max_deg, + ) + + mudou = ( + abs(novo_roll_max - self.roll_max_deg) > 1e-6 + or abs(novo_pitch_max - self.pitch_max_deg) > 1e-6 + ) + + self.roll_max_deg = novo_roll_max + self.pitch_max_deg = novo_pitch_max + + self._recalcular_faixas_angulares() + + if mudou: + self.mostrar_log( + "Limites angulares atualizados | " + f"roll/frontal={roll_anterior:.1f}°→{self.roll_max_deg:.1f}° | " + f"pitch/lateral={pitch_anterior:.1f}°→{self.pitch_max_deg:.1f}° | " + f"critical=({self.roll_critical_deg:.1f}°, " + f"{self.pitch_critical_deg:.1f}°) | " + f"emergency=({self.roll_emergency_deg:.1f}°, " + f"{self.pitch_emergency_deg:.1f}°)" + ) + # ============================================================ # Fusão # ============================================================ @@ -695,6 +840,87 @@ class CameraIMU(ModuloDiagnosticoBase): "single_source": False, } + def _atualizar_filtro_fusao(self, fusao): + """ + Atualiza uma única vez por ciclo os valores filtrados da atitude final. + + Os ângulos filtrados serão usados tanto: + - na decisão de risco; + - quanto na publicação e nos logs. + + As taxas brutas continuam disponíveis para detectar movimentos rápidos. + """ + + roll = float(fusao.get("roll", 0.0)) + pitch = float(fusao.get("pitch", 0.0)) + yaw = float(fusao.get("yaw", 0.0)) + + roll_rate = float(fusao.get("roll_rate_dps", 0.0)) + pitch_rate = float(fusao.get("pitch_rate_dps", 0.0)) + + alpha = self.clamp(float(self.filtro_alpha), 0.0, 1.0) + + if not self._filtro_fusao_inicializado: + self.roll_filtrado = roll + self.pitch_filtrado = pitch + self.yaw_filtrado = yaw + + self.roll_rate_filtrado = roll_rate + self.pitch_rate_filtrado = pitch_rate + + self._filtro_fusao_inicializado = True + + else: + self.roll_filtrado = self._ema( + self.roll_filtrado, + roll, + alpha, + ) + + self.pitch_filtrado = self._ema( + self.pitch_filtrado, + pitch, + alpha, + ) + + # Yaw precisa respeitar o wrap -180° / +180°. + dyaw = self._angle_delta_deg( + yaw, + self.yaw_filtrado, + ) + + self.yaw_filtrado += alpha * dyaw + self.yaw_filtrado = ( + self.yaw_filtrado + 180.0 + ) % 360.0 - 180.0 + + self.roll_rate_filtrado = self._ema( + self.roll_rate_filtrado, + roll_rate, + alpha, + ) + + self.pitch_rate_filtrado = self._ema( + self.pitch_rate_filtrado, + pitch_rate, + alpha, + ) + + return { + "roll": float(self.roll_filtrado), + "pitch": float(self.pitch_filtrado), + "yaw": float(self.yaw_filtrado), + "roll_rate_dps": float(self.roll_rate_filtrado), + "pitch_rate_dps": float(self.pitch_rate_filtrado), + } + + def _resetar_filtro_fusao(self): + """ + Faz a próxima leitura válida inicializar o filtro diretamente + com a atitude atual, evitando carregar valores antigos. + """ + self._filtro_fusao_inicializado = False + # ============================================================ # Risco e ação sugerida # ============================================================ @@ -918,6 +1144,7 @@ class CameraIMU(ModuloDiagnosticoBase): ignoradas, desabilitadas, fusao, + atitude_filtrada, divergencia, risco, redundancia_disponivel, @@ -931,24 +1158,12 @@ class CameraIMU(ModuloDiagnosticoBase): pitch_rate = float(fusao["pitch_rate_dps"]) yaw_rate = float(fusao["yaw_rate_dps"]) - a = self.filtro_alpha + roll_filtrado = float(atitude_filtrada["roll"]) + pitch_filtrado = float(atitude_filtrada["pitch"]) + yaw_filtrado = float(atitude_filtrada["yaw"]) - if self.last_data is None: - self.roll_filtrado = roll - self.pitch_filtrado = pitch - self.yaw_filtrado = yaw - self.roll_rate_filtrado = roll_rate - self.pitch_rate_filtrado = pitch_rate - else: - self.roll_filtrado = self._ema(self.roll_filtrado, roll, a) - self.pitch_filtrado = self._ema(self.pitch_filtrado, pitch, a) - - dyaw = self._angle_delta_deg(yaw, self.yaw_filtrado) - self.yaw_filtrado = self.yaw_filtrado + a * dyaw - self.yaw_filtrado = (self.yaw_filtrado + 180.0) % 360.0 - 180.0 - - self.roll_rate_filtrado = self._ema(self.roll_rate_filtrado, roll_rate, a) - self.pitch_rate_filtrado = self._ema(self.pitch_rate_filtrado, pitch_rate, a) + roll_rate_filtrado = float(atitude_filtrada["roll_rate_dps"]) + pitch_rate_filtrado = float(atitude_filtrada["pitch_rate_dps"]) self.roll_hist.append(roll) self.pitch_hist.append(pitch) @@ -988,21 +1203,21 @@ class CameraIMU(ModuloDiagnosticoBase): "yaw": round(yaw, 3), # Dado levemente suavizado. - "roll_filtrado": round(self.roll_filtrado, 3), - "pitch_filtrado": round(self.pitch_filtrado, 3), - "yaw_filtrado": round(self.yaw_filtrado, 3), + "roll_filtrado": round(roll_filtrado, 3), + "pitch_filtrado": round(pitch_filtrado, 3), + "yaw_filtrado": round(yaw_filtrado, 3), # Compatibilidade com contrato antigo. - "roll_seg": round(self.roll_filtrado, 3), - "pitch_seg": round(self.pitch_filtrado, 3), + "roll_seg": round(roll_filtrado, 3), + "pitch_seg": round(pitch_filtrado, 3), # Tendência angular. "roll_rate_dps": round(roll_rate, 3), "pitch_rate_dps": round(pitch_rate, 3), "yaw_rate_dps": round(yaw_rate, 3), - "roll_rate_filtrado_dps": round(self.roll_rate_filtrado, 3), - "pitch_rate_filtrado_dps": round(self.pitch_rate_filtrado, 3), + "roll_rate_filtrado_dps": round(roll_rate_filtrado, 3), + "pitch_rate_filtrado_dps": round(pitch_rate_filtrado, 3), # Segurança. "risk_level": risco["risk_level"], @@ -1016,6 +1231,43 @@ class CameraIMU(ModuloDiagnosticoBase): "stop": risco["stop"], "emergency_stop": risco["emergency_stop"], + "convencao_eixos": { + "roll": "frontal", + "pitch": "lateral", + }, + + "limites_angulo": { + "roll": { + "semantica": "frontal", + "attention_deg": round(self.roll_attention_deg, 3), + "risk_deg": round(self.roll_risk_deg, 3), + "critical_deg": round(self.roll_critical_deg, 3), + "emergency_deg": round(self.roll_emergency_deg, 3), + "max_configurado_deg": round(self.roll_max_deg, 3), + }, + "pitch": { + "semantica": "lateral", + "attention_deg": round(self.pitch_attention_deg, 3), + "risk_deg": round(self.pitch_risk_deg, 3), + "critical_deg": round(self.pitch_critical_deg, 3), + "emergency_deg": round(self.pitch_emergency_deg, 3), + "max_configurado_deg": round(self.pitch_max_deg, 3), + }, + }, + + "roll_max_configurado_deg": round(self.roll_max_deg, 3), + "pitch_max_configurado_deg": round(self.pitch_max_deg, 3), + + "roll_attention_deg": round(self.roll_attention_deg, 3), + "roll_risk_deg": round(self.roll_risk_deg, 3), + "roll_critical_deg": round(self.roll_critical_deg, 3), + "roll_emergency_deg": round(self.roll_emergency_deg, 3), + + "pitch_attention_deg": round(self.pitch_attention_deg, 3), + "pitch_risk_deg": round(self.pitch_risk_deg, 3), + "pitch_critical_deg": round(self.pitch_critical_deg, 3), + "pitch_emergency_deg": round(self.pitch_emergency_deg, 3), + # Diagnóstico de fusão. "confidence": round(float(fusao.get("confidence", 0.0)), 3), "fonte_principal": fonte_principal, @@ -1102,6 +1354,8 @@ class CameraIMU(ModuloDiagnosticoBase): try: agora = time.time() + self._atualizar_limites_equipamento() + fontes, ignoradas, desabilitadas = self._ler_fontes_imu() modulos_validos = { @@ -1115,6 +1369,7 @@ class CameraIMU(ModuloDiagnosticoBase): ) if len(fontes) <= 0: + self._resetar_filtro_fusao() data = self._montar_saida_sem_imu( ignoradas, desabilitadas, @@ -1124,11 +1379,20 @@ class CameraIMU(ModuloDiagnosticoBase): fusao = self._fundir_fontes(fontes) divergencia = self._calcular_divergencia(fontes) + # Atualiza o filtro antes de avaliar o risco. + atitude_filtrada = self._atualizar_filtro_fusao(fusao) + risco = self._avaliar_risco( - roll=fusao["roll"], - pitch=fusao["pitch"], + # Ângulos filtrados evitam parada por um único pico de leitura. + roll=atitude_filtrada["roll"], + pitch=atitude_filtrada["pitch"], + + # Taxas brutas continuam rápidas. + # Se o robô começar a tombar rapidamente, a segurança não fica + # esperando o filtro angular acompanhar. roll_rate=fusao["roll_rate_dps"], pitch_rate=fusao["pitch_rate_dps"], + sensores_agree=divergencia["sensors_agree"], redundancia_disponivel=redundancia_disponivel, ) @@ -1138,6 +1402,7 @@ class CameraIMU(ModuloDiagnosticoBase): ignoradas=ignoradas, desabilitadas=desabilitadas, fusao=fusao, + atitude_filtrada=atitude_filtrada, divergencia=divergencia, risco=risco, redundancia_disponivel=redundancia_disponivel, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py index 35cc34c33..243fb3cda 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py @@ -441,6 +441,10 @@ class CameraManager: return True + def reiniciar_deteccoes(self): + self._reset_runtime_state() + return True, "estado operacional limpo" + # ============================================================ # Warmup # ============================================================ diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/main.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/main.py index b96170992..12bb59441 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/main.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/main.py @@ -81,8 +81,12 @@ def main(): tipos = dados.get("params", {}).get("tipos", []) get_camera_manager().salvar_frames(tipos, nome, pasta) elif acao == WeedWorkerCommandType.ReiniciarDeteccoes: - if get_camera_manager().weed_detector is not None: - get_camera_manager().weed_detector._reiniciar_deteccoes() + manager = get_camera_manager() + ok, motivo = manager.reiniciar_deteccoes() + if ok: + mostrar_log(f"[weed] Detecções reiniciadas: {motivo}") + else: + mostrar_log(f"[weed] Não foi possível reiniciar detecções: {motivo}") else: mostrar_log(f"⚠️ Comando desconhecido: {acao.name}") except Exception as e: