diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py index f1715ce06..cc2875c42 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py @@ -126,14 +126,41 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) pass # ----------------------------- - # 3) Clamp final corrigido + # 3) Limitador do IMU final + # ----------------------------- + imu_auxilio_movimento = _controle.get("imu_auxilio_movimento", True) + if imu_auxilio_movimento and filtrar_velocidade: + _imu = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + + imu_valido = bool(_imu.get("valido", False)) + imu_ts = float(_imu.get("timestamp", 0.0) or 0.0) + imu_ts_s = imu_ts / 1000.0 if imu_ts > 100000000000.0 else imu_ts + imu_atualizada = (time.time() - imu_ts_s) <= 1.0 if imu_ts_s > 0 else False + + risk_level_num = int(_imu.get("risk_level_num", 0) or 0) + velocidade_factor_imu = float(_imu.get("velocidade_factor", 1.0) or 1.0) + velocidade_factor_imu = max(0.0, min(1.0, velocidade_factor_imu)) + + if imu_valido and imu_atualizada: + # Attention/risk reduzem suavemente. + # Critical/emergency normalmente já serão tratados no RegrasTaticas. + if risk_level_num >= 1: + velocidade_sp *= velocidade_factor_imu + + # Se por algum motivo chegou stop aqui, zera como redundância. + if bool(_imu.get("stop", False)) or bool(_imu.get("emergency_stop", False)): + velocidade_sp = 0.0 + filtrar_velocidade = False + + # ----------------------------- + # 4) Clamp final corrigido # ----------------------------- velocidade_sp = max(0.0, min(vel_sem_ervas, velocidade_sp)) if velocidade_sp > 0.0: velocidade_sp = max(vel_min, velocidade_sp) # ----------------------------- - # 4) Filtro final + # 5) Filtro final # ----------------------------- if velocidade_sp < 1.0 or reduzir_para_pulverizar: filtro_vel.reset(velocidade_sp) 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 d5395b332..fa5acc45d 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 @@ -62,30 +62,78 @@ class RegrasTaticas: 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 = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + + _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 + + imu_ts = float(_imu.get("timestamp", 0.0) or 0.0) + imu_ts_s = imu_ts / 1000.0 if imu_ts > 100000000000.0 else imu_ts + imu_atualizada = (time.time() - imu_ts_s) <= 1.0 if imu_ts_s > 0 else False + + imu_valido = bool(_imu.get("valido", False)) + + risk_level = str(_imu.get("risk_level", "unknown") or "unknown") + risk_level_num = int(_imu.get("risk_level_num", 4) or 4) + + stop = bool(_imu.get("stop", False)) + emergency_stop = bool(_imu.get("emergency_stop", False)) + bloquear_avanco = bool(_imu.get("bloquear_avanco", False)) + + roll = float(_imu.get("roll_filtrado", _imu.get("roll_seg", _imu.get("roll", 0.0))) or 0.0) + pitch = float(_imu.get("pitch_filtrado", _imu.get("pitch_seg", _imu.get("pitch", 0.0))) or 0.0) + + roll_rate = float(_imu.get("roll_rate_dps", 0.0) or 0.0) + pitch_rate = float(_imu.get("pitch_rate_dps", 0.0) or 0.0) + + motivos_risco = _imu.get("motivos_risco", []) or [] + acao_sugerida = str(_imu.get("acao_sugerida", "") or "") + + imu_bloqueio_bruto = ( + not imu_operante + or not imu_atualizada + or not imu_valido + or emergency_stop + or stop + or bloquear_avanco + or risk_level_num >= 3 + ) inclinacao_persistente = self._avaliar_persistencia( nome="inclinacao_perigosa", - estado_bruto=(imu_operante and imu_atualizada and inclinacao_perigosa), - tempo_acionar=0.20, + estado_bruto=imu_bloqueio_bruto, + tempo_acionar=0.10 if emergency_stop else 0.20, tempo_liberar=1.2 ) if not imu_operante: - motivos.append(f"IMU mandatório por [ Parada por Inclinação ] 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}") + motivos.append( + f"IMU mandatória por [ Parada por Inclinação ] não operante: {_imu_saude.name}" + ) freio_necessario = True + + elif not imu_atualizada: + motivos.append("IMU mandatória por [ Parada por Inclinação ] desatualizada") + freio_necessario = True + + elif not imu_valido: + motivos.append("IMU mandatória por [ Parada por Inclinação ] sem leitura válida") + freio_necessario = True + + elif inclinacao_persistente: + motivos.append( + f"IMU bloqueou movimento: {risk_level} | ação={acao_sugerida} | " + f"roll={roll:.2f}°, pitch={pitch:.2f}°, " + f"roll_rate={roll_rate:.1f}°/s, pitch_rate={pitch_rate:.1f}°/s" + ) + + if motivos_risco: + motivos.append(f"Motivos IMU: {', '.join(map(str, motivos_risco))}") + + freio_necessario = bool(emergency_stop or stop or risk_level_num >= 3) pulverizador_automatico = _controle.get("pulverizador_automatico", False) if pulverizador_automatico: