ajustes no controle de velocidade e regras táticas com uso de imu

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)

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py

@@ -62,31 +62,79 @@ 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 = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {}
-                _imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
+
+                _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
+                imu_ts = float(_imu.get("timestamp", 0.0) or 0.0)
-                _equipamento = ContextoGlobalRedis.get_equipamento()
+                imu_ts_s = imu_ts / 1000.0 if imu_ts > 100000000000.0 else imu_ts
-                ang_roll_max = _equipamento.get("angulo_roll_max", 15.0)
+                imu_atualizada = (time.time() - imu_ts_s) <= 1.0 if imu_ts_s > 0 else False
-                ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0)
+
-                roll = _imu.get("roll_seg", 0.0)
+                imu_valido = bool(_imu.get("valido", False))
-                pitch = _imu.get("pitch_seg", 0.0)
+
-                inclinacao_perigosa = abs(pitch) > ang_pitch_max or abs(roll) > ang_roll_max
+                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),
+                    estado_bruto=imu_bloqueio_bruto,
-                    tempo_acionar=0.20,
+                    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}")
+                    motivos.append(
-                elif inclinacao_persistente:
+                        f"IMU mandatória por [ Parada por Inclinação ] não operante: {_imu_saude.name}"
-                    motivos.append(f"Inclinação perigosa detectada. roll: {roll:.2f}/{ang_roll_max}, pitch: {pitch:.2f}/{ang_pitch_max}")
+                    )
                     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:
                 _atu = ContextoGlobalRedis.get_modulo(T_Code.Atu)