Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
Diego Freitas 2026-06-10 21:32:38 -03:00
parent 6209681ec3
commit 11ccdca22e
2 changed files with 92 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py
View File

@ -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)

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

@ -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: