diff --git a/AgroBase/AgroBase/Forms/frmJoystick.cs b/AgroBase/AgroBase/Forms/frmJoystick.cs index 6c0cbc2ed..5edd30ded 100644 --- a/AgroBase/AgroBase/Forms/frmJoystick.cs +++ b/AgroBase/AgroBase/Forms/frmJoystick.cs @@ -1,4 +1,5 @@ using System; +using System.Linq; using System.Windows.Forms; using static AgroBase.Models.Enums; @@ -25,14 +26,14 @@ namespace AgroBase.Forms private void frmJoystick_KeyDown(object sender, KeyEventArgs e) { - T_Code disp = GeneralJoystick.GetDispositivoComando(e.KeyCode); - GeneralJoystick.EnviaComandoMotor(e.KeyCode, disp); + var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode); + GeneralJoystick.EnviaComandoMotor(comando?.key ?? e.KeyCode, comando?.Dispositivo ?? T_Code.Vzo); } private void frmJoystick_KeyUp(object sender, KeyEventArgs e) { - T_Code disp = GeneralJoystick.GetDispositivoComando(e.KeyCode); - GeneralJoystick.EnviaComandoMotor(Keys.Escape, disp); + var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode); + GeneralJoystick.EnviaComandoMotor(comando?.key ?? Keys.Escape, comando?.Dispositivo ?? T_Code.Vzo); } } diff --git a/AgroBase/AgroBase/Models/GeneralJoystick.cs b/AgroBase/AgroBase/Models/GeneralJoystick.cs index 1b8ddc949..6b87b7939 100644 --- a/AgroBase/AgroBase/Models/GeneralJoystick.cs +++ b/AgroBase/AgroBase/Models/GeneralJoystick.cs @@ -22,22 +22,22 @@ namespace AgroBase public static List Comandos = new List() { }; public static List DeParaComandos = new List() { - new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.R, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Space, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.Add, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.D1, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" }, - new DeParaComandos() { key = Keys.D2, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo, PararAoSoltar = true, IDcontrolar = "B02" }, - new DeParaComandos() { key = Keys.D3, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima, PararAoSoltar = true, IDcontrolar = "B03" }, - new DeParaComandos() { key = Keys.D4, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita, PararAoSoltar = true, IDcontrolar = "B04" }, + new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.R, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3, PararAoSoltar = false }, + new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Space, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1, PararAoSoltar = false }, + new DeParaComandos() { key = Keys.Add, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1, PararAoSoltar = false }, + new DeParaComandos() { key = Keys.D1, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" }, + new DeParaComandos() { key = Keys.D2, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo, PararAoSoltar = true, IDcontrolar = "B02" }, + new DeParaComandos() { key = Keys.D3, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima, PararAoSoltar = true, IDcontrolar = "B03" }, + new DeParaComandos() { key = Keys.D4, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita, PararAoSoltar = true, IDcontrolar = "B04" }, }; public static List BotoesJoyPressionados = new List() { }; public static List MovimentosCombinados = new List() @@ -523,7 +523,7 @@ namespace AgroBase var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao.botao); if (Comando != null) { - EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar); + EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao.botao); } }); _botoesSoltos.ForEach(botao => @@ -534,7 +534,7 @@ namespace AgroBase var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao); if (Comando != null && Comando.PararAoSoltar) { - EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar); + EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao, ForcarComando: true); } }); } @@ -547,6 +547,7 @@ namespace AgroBase double centro = 32767; double deadzone = 3000; double maximo = 65535.0; + var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; switch (botao) { @@ -609,13 +610,13 @@ namespace AgroBase case BotoesJoystick.Xis: direcao = Direcao.Frente; dispositivo = T_Code.Mov; - percent = 1.0; + percent = Math.Max(pControle.MovVelocidadeCErvasPercent, (pControle.MovVelocidadeSErvasPercent - pControle.MovVelocidadeCErvasPercent) / 2.0); break; case BotoesJoystick.Quadrado: direcao = Direcao.Tras; dispositivo = T_Code.Mov; - percent = 1.0; + percent = Math.Max(pControle.MovVelocidadeCErvasPercent, (pControle.MovVelocidadeSErvasPercent - pControle.MovVelocidadeCErvasPercent) / 2.0); break; } @@ -624,7 +625,6 @@ namespace AgroBase //Console.WriteLine($"botao: {botao}, valor: {valor}, percentual: {percent * 100.0:F2}%"); - var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; var _Controle = Variaveis.OperacaoEmAndamento.Controle; _Controle.Direcao = direcao; int mx = direcao == Direcao.Esquerda || direcao == Direcao.Tras ? -1 : 1; @@ -639,7 +639,7 @@ namespace AgroBase } } - public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, bool ForcarComando = false) + public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, BotoesJoystick botao = BotoesJoystick.Vazio, bool ForcarComando = false) { // Verifica se deve interromper o fluxo // interrompe se nao estiver diagnosticando, nao forca o envio, e se o modo for manual vai verificar o status da operacao e do sonar, pois automatico faz esse controle em outro lugar @@ -668,7 +668,8 @@ namespace AgroBase Direcao DirecaoAtual = Direcao.Parado; - var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo); + var _Controle = Variaveis.OperacaoEmAndamento.Controle; + var tipoControle = _Controle?.TiposControle?.FirstOrDefault(x => x?.Tipo == Comando.Dispositivo); // Cancelar Comando Anterior if (Comando.Comando == Keys.Escape) @@ -688,6 +689,14 @@ namespace AgroBase Comando.IDcontrolar = !string.IsNullOrEmpty(ID) ? ID : ComandosDisp[ComandosDisp.Count - 1].IDcontrolar; } DirecaoAtual = Direcao.Parado; + + switch (botao) + { + case BotoesJoystick.Bolinha: + _Controle.EmFreio = false; + break; + } + Pressionado = true; } } @@ -695,17 +704,15 @@ namespace AgroBase else if (Comando.Comando == Keys.Add) { Comando.Dispositivo = T_Code.Mov; - Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo); - DirecaoAtual = Controle?.UltimaDirecao ?? Direcao.Parado; - Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP++; + DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado; + _Controle.PercentualVelocidadeSP++; Pressionado = true; } else if (Comando.Comando == Keys.Subtract) { Comando.Dispositivo = T_Code.Mov; - Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo); - DirecaoAtual = Controle?.UltimaDirecao ?? Direcao.Parado; - Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP--; + DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado; + _Controle.PercentualVelocidadeSP--; Pressionado = true; } // Controle Direcao dos Motores BLDC (MOV) @@ -725,6 +732,7 @@ namespace AgroBase { Comando.Dispositivo = T_Code.Mov; DirecaoAtual = Direcao.EmFreio; + _Controle.EmFreio = true; Pressionado = true; } // Controle de Rotação dos Motores de Passo (DIR) @@ -788,7 +796,7 @@ namespace AgroBase case T_Code.Mov: { //Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ForEach(x => x.MovMotor.EnviarComandoControle(DirecaoAtual)); - Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { Comando.Dispositivo }); + Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List() { Comando.Dispositivo }); break; } case T_Code.Dir: @@ -806,7 +814,7 @@ namespace AgroBase } else { - Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { Comando.Dispositivo }); + Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List() { Comando.Dispositivo }); //foreach (var mod in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos) //{ // if (modsControlar == null || modsControlar.Contains(mod.DirMotor.Mod_ID)) @@ -830,12 +838,11 @@ namespace AgroBase } } - Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo); - - if (Controle != null) - { - Controle.UltimaDirecao = DirecaoAtual; - } + //Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo); + //if (Controle != null) + //{ + // Controle.UltimaDirecao = DirecaoAtual; + //} } UltimaDirecao = DirecaoAtual; @@ -978,7 +985,6 @@ namespace AgroBase { List ComandosImpedidos = new List(); var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual; - Sonar?.Resumo?.AtualizarStatusBloqueio(false); var DeveParar = Sonar?.Resumo?.DeveParar ?? false; if (DeveParar) { @@ -1020,11 +1026,6 @@ namespace AgroBase return !ComandosImpedidos.Any(x => x == Comando); } - public static T_Code GetDispositivoComando(Keys Tecla, BotoesJoystick botao = BotoesJoystick.Vazio) - { - T_Code disp = DeParaComandos.FirstOrDefault(x => x.key == Tecla || x.botaoJoy == botao)?.Dispositivo ?? T_Code.Vzo; - return disp; - } } diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 7908e103d..d989e0faa 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -1334,7 +1334,7 @@ namespace AgroBase.Models public List EnviarProtocoloComando(T_Code Tipo, bool ForcarEnvio) { - if (!Variaveis.OperacaoEmAndamento.Iniciado) + if (!Variaveis.OperacaoEmAndamento.Iniciado && !ForcarEnvio) { return new List(); } @@ -1488,11 +1488,11 @@ namespace AgroBase.Models Sonar?.Resumo?.AtualizarStatusBloqueio(true); if (Sonar?.Resumo?.EnviarComandoParada ?? false) { - GeneralJoystick.EnviaComandoMotor(Keys.Space, T_Code.Mov); + GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); } else if (Sonar?.Resumo?.EnviarComandoRetomada ?? false) { - GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov); } if (!Variaveis.OperacaoEmAndamento.OperacaoLiberada) @@ -1501,12 +1501,12 @@ namespace AgroBase.Models if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado) { _Controle.PercentualVelocidadeSP = 0; - GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true); + GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); } if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Dir)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado) { _Controle.Angulo = 0; - GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true); + GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Dir, ForcarComando: true); } } } @@ -1719,8 +1719,9 @@ namespace AgroBase.Models if (cmdRover?.Dispositivo == T_Code.Dir && Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return; var _Controle = Variaveis.OperacaoEmAndamento.Controle; - if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP; - if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP; + int mx = cmdRover.key == Keys.Escape || cmdRover.key == Keys.Down ? -1 : 1; + if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP * mx; + if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP * mx; if (controleBase.Controle.TipoMovimentoDirecional != null) _Controle.TipoMovimento = (TipoMovimentoDirecional)controleBase.Controle.TipoMovimentoDirecional; if (controleBase.Controle.EmFreio != null) _Controle.EmFreio = (bool)controleBase.Controle.EmFreio; diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index 7b6066b41..e0325ee05 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -284,12 +284,12 @@ namespace AgroBase.Models { if (disp == T_Code.Dir) { - controle.Controle.AnguloSP = msg.P1 / 100.0; + controle.Controle.AnguloSP = (msg.P1 / 100.0); controle.Controle.TipoMovimentoDirecional = (TipoMovimentoDirecional)(byte)msg.P2; } else if (disp == T_Code.Mov) { - controle.Controle.PercentualVelocidadeSP = msg.P1 / 100.0; + controle.Controle.PercentualVelocidadeSP = (msg.P1 / 100.0); controle.Controle.EmFreio = msg.P2 == 1; } } 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 1bf8fd463..e4e3c3e6b 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 @@ -12,6 +12,11 @@ class IMUCamera(ModuloDiagnosticoBase): def __init__(self, queue=None, freq=100, angulo_inicial=26.3): self.freq = freq + self.ativo = False + self.last_packet_ts = 0.0 + self.last_publish_ts = 0.0 + self.ultimo_erro_ts = 0.0 + if queue is None: return @@ -56,8 +61,8 @@ class IMUCamera(ModuloDiagnosticoBase): self.rugosidade_idx = 0.0 self.impacto_idx = 0.0 self.estabilidade_idx = 100.0 - self.last_redis_ts = 0.0 self.redis_period = 0.10 # 100 ms + self.last_redis_ts = 0.0 self.ativo = True self.imu_thread = threading.Thread(target=self.imu_task_loop, daemon=True) @@ -189,6 +194,7 @@ class IMUCamera(ModuloDiagnosticoBase): self.imu_em_falha = False if imuData is not None and len(imuData.packets) > 0: + self.last_packet_ts = time.time() now = time.perf_counter() if self.last_loop_ts is None: dt_total = 1.0 / self.freq @@ -297,6 +303,7 @@ class IMUCamera(ModuloDiagnosticoBase): t_pub = time.perf_counter() if (t_pub - self.last_redis_ts) >= self.redis_period: + self.last_publish_ts = time.time() self.last_redis_ts = t_pub ContextoGlobalRedis.atualizar_ctx_dict( @@ -318,6 +325,7 @@ class IMUCamera(ModuloDiagnosticoBase): ) except Exception as e: + self.ultimo_erro_ts = time.time() self.mostrar_log(f"Erro no loop: {e}") if not self.imu_em_falha: self.imu_em_falha = True @@ -334,43 +342,110 @@ class IMUCamera(ModuloDiagnosticoBase): def atualizar_saude(self): try: - modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu) + agora = time.time() + modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + FREQ_BASE = self.freq FREQ_MIN = FREQ_BASE * 0.5 SAUDE_MIN_ALERTA = 80 + SAUDE_MIN_FALHA = 45 - conectado = (time.time() * 1000) - modulo.get("timestamp", 0) < 2000 - freq_hz = modulo.get("frequencia", 0.0) - latencia = modulo.get("latencia", 0.0) + conectado = bool( + self.ativo and + self.imu_thread is not None and + self.imu_thread.is_alive() + ) + + freq_hz = float(modulo.get("frequencia", 0.0) or 0.0) + latencia = float(modulo.get("latencia", 0.0) or 0.0) + + last_packet_ts = float(self.last_packet_ts) + last_publish_ts = float(self.last_publish_ts) + ultimo_erro_ts = float(self.ultimo_erro_ts) + + tempo_sem_packet = (agora - last_packet_ts) if last_packet_ts > 0 else 999.0 + tempo_sem_publicar = (agora - last_publish_ts) if last_publish_ts > 0 else 999.0 + tempo_desde_erro = (agora - ultimo_erro_ts) if ultimo_erro_ts > 0 else 999.0 saude = 100 motivos = [] if not conectado: - motivos.append("IMU desconectado ou sem dados recentes") + motivos.append("Thread da IMU não está viva") saude = 0 - elif freq_hz <= FREQ_MIN: - motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") - saude -= 50 else: - f = min(freq_hz, FREQ_BASE) - erro = (FREQ_BASE - f) / FREQ_BASE - p = min(int(erro * 100), 30) - saude -= abs(p) - if p > 0: - motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz (penalidade {p}%)") + # ============================ + # 1) Penalização por ausência de packets + # ============================ + if tempo_sem_packet > 1.5: + saude -= 45 + motivos.append(f"Sem packets há {tempo_sem_packet:.2f}s") + elif tempo_sem_packet > 0.7: + saude -= 25 + motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s") + elif tempo_sem_packet > 0.3: + saude -= 10 + motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s") - if latencia > 0.5: - p = int(min(latencia * 100, 30)) - saude -= p - motivos.append(f"Latência alta: {latencia:.2f} s (penalidade {p}%)") + # ============================ + # 2) Penalização por ausência de publicação + # ============================ + if tempo_sem_publicar > 1.5: + saude -= 30 + motivos.append(f"Sem publicar no Redis há {tempo_sem_publicar:.2f}s") + elif tempo_sem_publicar > 0.8: + saude -= 15 + motivos.append(f"Publicação atrasada há {tempo_sem_publicar:.2f}s") + elif tempo_sem_publicar > 0.3: + saude -= 5 + motivos.append(f"Leve atraso na publicação: {tempo_sem_publicar:.2f}s") - saude = max(saude, 0) + # ============================ + # 3) Frequência + # ============================ + if freq_hz <= 0: + saude -= 20 + motivos.append("Frequência zerada") + elif freq_hz <= FREQ_MIN: + saude -= 35 + motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") + else: + f = min(freq_hz, FREQ_BASE) + erro = (FREQ_BASE - f) / FREQ_BASE + p = min(int(erro * 100), 20) + saude -= p + if p > 0: + motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz") + + # ============================ + # 4) Latência + # ============================ + if latencia > 1.0: + saude -= 25 + motivos.append(f"Latência muito alta: {latencia:.2f}s") + elif latencia > 0.5: + saude -= 15 + motivos.append(f"Latência alta: {latencia:.2f}s") + elif latencia > 0.2: + saude -= 5 + motivos.append(f"Latência moderada: {latencia:.2f}s") + + # ============================ + # 5) Erro recente + # ============================ + if tempo_desde_erro < 2.0: + saude -= 20 + motivos.append("Erro muito recente no loop") + elif tempo_desde_erro < 5.0: + saude -= 10 + motivos.append("Erro recente no loop") + + saude = max(0, min(100, saude)) status = StatusModulo.OPERANTE if not conectado: status = StatusModulo.DESCONECTADO - elif saude <= 0: + elif saude <= SAUDE_MIN_FALHA: status = StatusModulo.FALHA elif saude < SAUDE_MIN_ALERTA: status = StatusModulo.ALERTA @@ -380,13 +455,14 @@ class IMUCamera(ModuloDiagnosticoBase): "status": status.value, "saude": saude, "motivos": motivos, - "saude_idividual": [] + "saude_idividual": [], } ContextoGlobalRedis.atualizar_ctx_dict( ContextoGlobalRedis.ModKey(T_Code.Imu), saude=saude_geral ) + except Exception as e: self.mostrar_log(f"Erro ao atualizar saude: {e}") diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py index a6c81f48d..9dc3cf798 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py @@ -3,49 +3,113 @@ from shared.enums import StatusModulo, StatusOperacao, T_Code from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from manager_worker.config import mostrar_log -def verifica_controle_liberado(ignorar_parada=False): - try: - motivos = [] - _operacao = ContextoGlobalRedis.get_operacao() - status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value)) - finalizando = _operacao.get("finalizando", False) - - if not ignorar_parada and ((status_operacao != StatusOperacao.EmAndamento) or finalizando): - motivos.append(f"Operação não está em andamento ou finalizando: {status_operacao.name}") - - _controle = ContextoGlobalRedis.get_controle() - - oak_parada_por_bloqueio = _controle.get("oak_parada_por_bloqueio", False) - if oak_parada_por_bloqueio: - _snr = ContextoGlobalRedis.get_modulo(T_Code.Snr) - _snr_saude = StatusModulo(_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) - snr_operante = _snr_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA] - _dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}) - analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0) - obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False) if analise_deteccao_atualizada else False +class RegrasTaticas: + def __init__(self): + self.persistencias = { + "obstaculo_sonar": { + "ativo": False, + "ts_inicio_true": None, + "ts_inicio_false": None, + "ultimo_estado_bruto": False, + }, + "inclinacao_perigosa": { + "ativo": False, + "ts_inicio_true": None, + "ts_inicio_false": None, + "ultimo_estado_bruto": False, + }, + } + + def verifica_controle_liberado(self, ignorar_parada=False): + try: + motivos = [] + freio_necessario = False + _controle = ContextoGlobalRedis.get_controle() + _operacao = ContextoGlobalRedis.get_operacao() + status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value)) + finalizando = _operacao.get("finalizando", False) - if (not snr_operante): - motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}") - elif (snr_operante and obstaculo_detectado): - motivos.append(f"Obstáculo próximo detectado no sonar") - - imu_parada_por_inclinacao = _controle.get("imu_parada_por_inclinacao", False) - if imu_parada_por_inclinacao: - _imu = ContextoGlobalRedis.get_modulo(T_Code.Imu) - _imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) - imu_operante = _imu_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA] - _equipamento = ContextoGlobalRedis.get_equipamento() - ang_roll_max = _equipamento.get("angulo_roll_max", 15.0) - ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0) - inclinacao_perigosa = abs(_imu.get("pitch_seg", 0.0)) > ang_pitch_max or abs(_imu.get("roll_seg", 0.0)) > ang_roll_max + if not ignorar_parada and ((status_operacao != StatusOperacao.EmAndamento) or finalizando): + motivos.append(f"Operação não está em andamento ou finalizando: {status_operacao.name}") + + oak_parada_por_bloqueio = _controle.get("oak_parada_por_bloqueio", False) + if oak_parada_por_bloqueio: + _snr = ContextoGlobalRedis.get_modulo(T_Code.Snr) + _snr_saude = StatusModulo(_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) + snr_operante = _snr_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA] + _dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}) + analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0) + obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False) + + obstaculo_persistente = self._avaliar_persistencia( + nome="obstaculo_sonar", + estado_bruto=(snr_operante and analise_deteccao_atualizada and obstaculo_detectado), + tempo_acionar=0.35, + tempo_liberar=1.0 + ) - if not imu_operante: - motivos.append(f"IMU mandatório não operante: {_imu_saude.name}") - elif imu_operante and inclinacao_perigosa: - motivos.append(f"Inclinação perigosa detectada. roll: {_imu.get('roll')}, pitch: {_imu.get('pitch')}, yaw: {_imu.get('yaw')}") - - return motivos - - except Exception as e: - mostrar_log(f"Erro ao verificar liberacao do controle: {e}") + if not snr_operante: + motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}") + elif obstaculo_persistente: + motivos.append("Obstáculo próximo detectado no sonar") + freio_necessario = True + + imu_parada_por_inclinacao = _controle.get("imu_parada_por_inclinacao", False) + if imu_parada_por_inclinacao: + _imu = ContextoGlobalRedis.get_modulo(T_Code.Imu) + _imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) + imu_operante = _imu_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA] + imu_ts = _imu.get("timestamp", 0) / 1000.0 + imu_atualizada = (time.time() - imu_ts) <= 1.0 + _equipamento = ContextoGlobalRedis.get_equipamento() + ang_roll_max = _equipamento.get("angulo_roll_max", 15.0) + ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0) + roll = _imu.get("roll_seg", 0.0) + pitch = _imu.get("pitch_seg", 0.0) + inclinacao_perigosa = abs(pitch) > ang_pitch_max or abs(roll) > ang_roll_max + inclinacao_persistente = self._avaliar_persistencia( + nome="inclinacao_perigosa", + estado_bruto=(imu_operante and imu_atualizada and inclinacao_perigosa), + tempo_acionar=0.20, + tempo_liberar=1.2 + ) + + if not imu_operante: + motivos.append(f"IMU mandatório não operante: {_imu_saude.name}") + elif inclinacao_persistente: + motivos.append(f"Inclinação perigosa detectada. roll: {roll:.2f}/{ang_roll_max}, pitch: {pitch:.2f}/{ang_pitch_max}") + freio_necessario = True + + return motivos, freio_necessario + + except Exception as e: + mostrar_log(f"Erro ao verificar liberacao do controle: {e}") + + def _avaliar_persistencia(self, nome, estado_bruto, tempo_acionar, tempo_liberar): + agora = time.perf_counter() + p = self.persistencias[nome] + + if estado_bruto: + if p["ultimo_estado_bruto"] != True: + p["ts_inicio_true"] = agora + p["ts_inicio_false"] = None + elif p["ts_inicio_true"] is None: + p["ts_inicio_true"] = agora + + if not p["ativo"] and p["ts_inicio_true"] is not None: + if (agora - p["ts_inicio_true"]) >= tempo_acionar: + p["ativo"] = True + else: + if p["ultimo_estado_bruto"] != False: + p["ts_inicio_false"] = agora + p["ts_inicio_true"] = None + elif p["ts_inicio_false"] is None: + p["ts_inicio_false"] = agora + + if p["ativo"] and p["ts_inicio_false"] is not None: + if (agora - p["ts_inicio_false"]) >= tempo_liberar: + p["ativo"] = False + + p["ultimo_estado_bruto"] = estado_bruto + return p["ativo"] diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py index e3a9f816f..5e20364c1 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py @@ -1,12 +1,15 @@ from .base import ProcessadorBase from manager_worker.processadores.padroes import comando_parado -from manager_worker.modulos.regras_taticas import verifica_controle_liberado +from manager_worker.modulos.regras_taticas import RegrasTaticas class ProcessadorAguardando(ProcessadorBase): + def __init__(self): + self.regras = RegrasTaticas() + def processar(self): try: - motivos_parada = verifica_controle_liberado(ignorar_parada=True) - return comando_parado(motivos=motivos_parada) + motivos_parada, freio_necessario = self.regras.verifica_controle_liberado(ignorar_parada=True) + return comando_parado(motivos=motivos_parada, frear=freio_necessario) except Exception as e: from manager_worker.config import mostrar_log mostrar_log(f"Erro no processador Aguardando: {e}") diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py index a645b068b..c58c56d26 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py @@ -5,7 +5,7 @@ from manager_worker.modulos.pid import PIDAdaptativo from .base import ProcessadorBase from manager_worker.filtros import FiltroVelocidade from manager_worker.processadores.padroes import comando_controle -from manager_worker.modulos.regras_taticas import verifica_controle_liberado +from manager_worker.modulos.regras_taticas import RegrasTaticas from manager_worker.modulos.direcional import definir_comando as definir_comando_dir from manager_worker.modulos.movimentacao import definir_comando as definir_comando_mov @@ -24,6 +24,7 @@ class ProcessadorEmAndamento(ProcessadorBase): saida_max=ang_max ) self.ultimo_comando_enviado = time.perf_counter() + self.regras = RegrasTaticas() def processar(self): @@ -31,13 +32,13 @@ class ProcessadorEmAndamento(ProcessadorBase): agora = time.perf_counter() envio_necessario = (agora - self.ultimo_comando_enviado) > 0.3 - motivos_parada = verifica_controle_liberado() + motivos_parada, freio_necessario = self.regras.verifica_controle_liberado() if len(motivos_parada) > 0: from manager_worker.config import mostrar_log mostrar_log(f"⛔ Controle bloqueado: {' | '.join(motivos_parada)}") return comando_controle( percentual_velocidade=0, - frear=True, + frear=freio_necessario, angulo=0, tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value, simulacao=[], @@ -59,7 +60,7 @@ class ProcessadorEmAndamento(ProcessadorBase): if direcional_automatico: comando_dir = definir_comando_dir(self.pid_dir, envio_necessario) - enviar_comando = (direcional_automatico and comando_dir.get("enviar_comando", False)) or not direcional_automatico + enviar_comando = comando_dir.get("enviar_comando", False) or movimento_automatico if enviar_comando: self.ultimo_comando_enviado = agora diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py index dee4cb7d6..b358860ee 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py @@ -1,12 +1,15 @@ from .base import ProcessadorBase from manager_worker.processadores.padroes import comando_parado -from manager_worker.modulos.regras_taticas import verifica_controle_liberado +from manager_worker.modulos.regras_taticas import RegrasTaticas class ProcessadorParado(ProcessadorBase): + def __init__(self): + self.regras = RegrasTaticas() + def processar(self): try: - motivos_parada = verifica_controle_liberado(ignorar_parada=True) - return comando_parado(motivos=motivos_parada) + motivos_parada, freio_necessario = self.regras.verifica_controle_liberado(ignorar_parada=True) + return comando_parado(motivos=motivos_parada, frear=freio_necessario) except Exception as e: from manager_worker.config import mostrar_log mostrar_log(f"Erro no processador Parado: {e}") diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py index 0c07ab5f3..9c42d770d 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py @@ -44,8 +44,8 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t "motivos": motivos } -def comando_parado(motivos=None): - return comando_controle(0, False, 0, TipoMovimentoDirecional.RodasDianteiras, motivos=motivos) +def comando_parado(motivos=None, frear=False): + return comando_controle(0, frear, 0, TipoMovimentoDirecional.RodasDianteiras, motivos=motivos) def log_status_operacao(): modo = ModoOperacao(ContextoGlobalRedis.get_operacao().get("modo")) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py index d4ed8e1ae..a55b4d99d 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py @@ -392,7 +392,7 @@ class ContextoGlobalRedis: controle_ok = False motivos_dos_modulos_mandatorios.extend(_motivos_controle) - liberou = ((base_ok and parametros_ok) or debug_mode) and emergencia_ok and pausa_ok and trajetoria_ok and controle_ok + liberou = ((base_ok and parametros_ok) or debug_mode) and emergencia_ok and pausa_ok and trajetoria_ok # and controle_ok cls.atualizar_ctx_dict( CtxKey.DadosOperacao, diff --git a/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/MonitoramentoViewModel.cs b/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/MonitoramentoViewModel.cs index a81ee923c..1f6a12922 100644 --- a/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/MonitoramentoViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/MonitoramentoViewModel.cs @@ -1,5 +1,4 @@ -using AgroBase.Models; -using AgroBase.Models.Operadores; +using AgroBase.Models.Operadores; using OperationControl.Helpers; using OperationControl.Models; using System.Collections.ObjectModel; @@ -149,7 +148,7 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento { if (System.Windows.MessageBox.Show("Deseja solicitar o retorno do equipamento até a base?", "Retorno à base", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.Yes) { - var gps = OperationControl.Models.Variaveis.GpsService.UltimaLeitura; + var gps = Variaveis.GpsService.UltimaLeitura; double? lat = gps?.Latitude; double? lon = gps?.Longitude; if (lat == null || lon == null) @@ -158,7 +157,16 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento return; } - VariaveisControleOperacao.EnviarComandoRetornoBase(lat, lon); + Task.Run(async () => + { + await VariaveisControleOperacao.EnviarComandoParadaUDP(pausa: true); + await Task.Delay(500); + VariaveisControleOperacao.EnviarComandoRetornoBase(lat, lon); + await Task.Delay(1000); + await VariaveisControleOperacao.EnviarComandoParadaUDP(pausa: false); + }); + + } } diff --git a/AgroBase/OperationControl/ViewModels/Views/Operacao/OperacaoLeftViewModel.cs b/AgroBase/OperationControl/ViewModels/Views/Operacao/OperacaoLeftViewModel.cs index 2e8199463..ad393caae 100644 --- a/AgroBase/OperationControl/ViewModels/Views/Operacao/OperacaoLeftViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Views/Operacao/OperacaoLeftViewModel.cs @@ -319,15 +319,16 @@ namespace OperationControl.ViewModels.Views.Operacao private void AlternarOperacao() { - if (VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Iniciada ?? false) + var dadosRover = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura; + if (dadosRover?.Operacao?.Iniciada ?? false) { - bool PauseActivated = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Pausa ?? false; + bool PauseActivated = dadosRover?.Operacao?.Pausa ?? false; PauseActivated = !PauseActivated; VariaveisControleOperacao.EnviarComandoParada(pausa: PauseActivated); } else { - if (((VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Modo ?? AgroBase.Models.Enums.ModoOperacao.NaoDefinido) == AgroBase.Models.Enums.ModoOperacao.MapaGPS) && !(VariaveisControleOperacao.RoverEmFoco?.RuasPercorrer?.Any() ?? false)) + if (((dadosRover?.Operacao?.Modo ?? AgroBase.Models.Enums.ModoOperacao.NaoDefinido) == AgroBase.Models.Enums.ModoOperacao.MapaGPS) && !(VariaveisControleOperacao.RoverEmFoco?.RuasPercorrer?.Any() ?? false)) { System.Windows.MessageBox.Show("Para iniciar a operação automática, é necessário carregar um mapa e selecionar as ruas onde o equipamento irá operar!", "Parametrização", MessageBoxButton.OK, MessageBoxImage.Warning); return; @@ -335,13 +336,17 @@ namespace OperationControl.ViewModels.Views.Operacao if (System.Windows.MessageBox.Show("Deseja iniciar a operação com os parâmetros atuais?", "Iniciar Operação", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.Yes) { - bool simulador = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.Iniciado != true; - VariaveisControleOperacao.EnviarComandoIniciarOperacao(true, simulador); - Task.Run(async () => { + if ((dadosRover?.Operacao?.Status ?? StatusOperacao.NaoIniciado) == StatusOperacao.Concluido) + { + VariaveisControleOperacao.EnviarComandoIniciarOperacao(false, false); + await Task.Delay(500); + } + bool simulador = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.Iniciado != true; + VariaveisControleOperacao.EnviarComandoIniciarOperacao(true, simulador); await Task.Delay(1000); - if (!(VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Liberada ?? false)) + if (!(dadosRover?.Operacao?.Liberada ?? false)) { System.Windows.MessageBox.Show($"{VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.ImpedimentoErro}", "Operação iniciada com impedimento!", MessageBoxButton.OK, MessageBoxImage.Warning); }