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

criado componente de imu unificado entre as cameras OAK

This commit is contained in:
Diego Freitas 2026-06-10 21:20:10 -03:00
parent 3aae33cc50
commit 6209681ec3
6 changed files with 1744 additions and 537 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

658
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py Normal file
View File

@ -0,0 +1,658 @@
import time
import threading
import math
from collections import deque
import numpy as np
from scipy.spatial.transform import Rotation as R
from shared.enums import StatusModulo
from health_worker.modulos.base import ModuloDiagnosticoBase
class IMUCamera(ModuloDiagnosticoBase):
"""
IMU da câmera OAK usando orientação pronta do DepthAI.
Responsabilidade desta classe:
- Consumir pacotes IMU da queue DepthAI.
- Usar quaternion pronto vindo de ROTATION_VECTOR / GAME_ROTATION_VECTOR.
- Fazer calibração inicial de zero relativo.
- Calcular roll/pitch/yaw relativos ao zero calibrado.
- Publicar dados simples e confiáveis via definir_imu_camera(mx_id, data, saude).
Esta classe NÃO faz:
- Madgwick no host.
- Integração de velocidade.
- Detecção de colisão.
- Rugosidade.
- Fusão entre câmeras.
- Decisão de risco de capotamento.
"""
def __init__(
self,
mx_id,
queue=None,
freq=200,
auto_calibrar=True,
calibracao_segundos=3.0,
publish_hz=50,
filtro_alpha=0.35,
max_queue_drain=20,
nome_sensor="oak_imu",
):
self.mx_id = mx_id
self.imu_queue = queue
self.freq = float(freq)
self.nome_sensor = nome_sensor
self.auto_calibrar = bool(auto_calibrar)
self.calibracao_segundos = float(calibracao_segundos)
self.publish_period = 1.0 / max(float(publish_hz), 1.0)
self.filtro_alpha = float(filtro_alpha)
self.max_queue_drain = int(max_queue_drain)
self.ativo = False
self.imu_em_falha = False
self.last_data = None
self.ultima_saude = None
self.last_packet_ts = 0.0
self.last_publish_ts = 0.0
self.ultimo_erro_ts = 0.0
self.last_loop_ts = None
self.last_imu_ts = None
self.latencia_loop = 0.0
self.frequencia_loop = 0.0
self.packets_processados = 0
self.packets_descartados = 0
# Referência de calibração
self.calibrado = False
self.calibrando = False
self.calib_inicio_ts = 0.0
self.calib_quats_xyzw = []
self.q_ref_inv = None
# Estado angular
self.roll_deg = 0.0
self.pitch_deg = 0.0
self.yaw_deg = 0.0
self.roll_filtrado_deg = 0.0
self.pitch_filtrado_deg = 0.0
self.yaw_filtrado_deg = 0.0
self.roll_rate_dps = 0.0
self.pitch_rate_dps = 0.0
self.yaw_rate_dps = 0.0
self._last_roll_deg = None
self._last_pitch_deg = None
self._last_yaw_deg = None
self._last_angle_ts = None
# Pequena janela apenas para avaliar estabilidade na calibração
self._calib_roll_hist = deque(maxlen=200)
self._calib_pitch_hist = deque(maxlen=200)
if queue is None:
self.mostrar_log("Inicializado sem queue. Classe ficará inativa.")
return
self.ativo = True
if self.auto_calibrar:
self.iniciar_calibracao()
self.imu_thread = threading.Thread(target=self.imu_task_loop, daemon=True)
self.imu_thread.start()
self.mostrar_log(
f"Task iniciada | freq alvo={self.freq:.0f}Hz | publish={1.0/self.publish_period:.0f}Hz"
)
# ==========================================================
# Controle de vida
# ==========================================================
def parar(self):
if self.ativo:
self.ativo = False
self.mostrar_log("Task parada")
try:
th = getattr(self, "imu_thread", None)
if th is not None and th.is_alive():
th.join(timeout=1.0)
except Exception:
pass
def iniciar_calibracao(self):
self.calibrado = False
self.calibrando = True
self.calib_inicio_ts = time.perf_counter()
self.calib_quats_xyzw.clear()
self._calib_roll_hist.clear()
self._calib_pitch_hist.clear()
self.q_ref_inv = None
self.mostrar_log(
f"Iniciando calibração IMU por {self.calibracao_segundos:.1f}s. Mantenha o robô parado em área plana."
)
# ==========================================================
# Utilidades matemáticas
# ==========================================================
def _normalizar_quat_xyzw(self, q_xyzw):
q = np.asarray(q_xyzw, dtype=np.float64)
n = float(np.linalg.norm(q))
if n <= 1e-12:
return None
return q / n
def _media_quaternion_xyzw(self, quats_xyzw):
"""
Média simples com alinhamento de hemisfério.
Para calibração parada, isso é suficiente e robusto.
Evita que q e -q se anulem.
"""
if not quats_xyzw:
return None
base = self._normalizar_quat_xyzw(quats_xyzw[0])
if base is None:
return None
acc = np.zeros(4, dtype=np.float64)
for q in quats_xyzw:
qn = self._normalizar_quat_xyzw(q)
if qn is None:
continue
if np.dot(base, qn) < 0:
qn = -qn
acc += qn
return self._normalizar_quat_xyzw(acc)
def _wrap_angle_delta_deg(self, atual, anterior):
"""
Retorna menor delta angular em graus, tratando wrap de -180/180.
"""
if anterior is None:
return 0.0
return (atual - anterior + 180.0) % 360.0 - 180.0
def _extrair_quaternion_do_packet(self, packet):
"""
Extrai quaternion pronto do pacote DepthAI.
Esperado para ROTATION_VECTOR / GAME_ROTATION_VECTOR:
packet.rotationVector.i
packet.rotationVector.j
packet.rotationVector.k
packet.rotationVector.real
Retorna:
- q_xyzw para scipy Rotation
- accuracy_rad, se existir
- timestamp do sensor, se existir
"""
rv = getattr(packet, "rotationVector", None)
if rv is None:
return None, None, None
try:
q_xyzw = np.array(
[
float(rv.i),
float(rv.j),
float(rv.k),
float(rv.real),
],
dtype=np.float64,
)
except Exception:
return None, None, None
q_xyzw = self._normalizar_quat_xyzw(q_xyzw)
if q_xyzw is None:
return None, None, None
accuracy_rad = None
try:
accuracy_rad = float(rv.rotationVectorAccuracy)
except Exception:
pass
sensor_ts = None
try:
sensor_ts = rv.timestamp.get()
except Exception:
pass
return q_xyzw, accuracy_rad, sensor_ts
def _quat_relativo_para_euler(self, q_xyzw):
"""
Converte quaternion absoluto da câmera em roll/pitch/yaw relativos ao zero calibrado.
q_delta = q_ref_inv * q_atual
"""
r_atual = R.from_quat(q_xyzw)
if self.calibrado and self.q_ref_inv is not None:
r_delta = self.q_ref_inv * r_atual
else:
r_delta = r_atual
roll, pitch, yaw = r_delta.as_euler("xyz", degrees=True)
return float(roll), float(pitch), float(yaw)
def _atualizar_rates(self, roll, pitch, yaw, ts):
if self._last_angle_ts is None:
self._last_angle_ts = ts
self._last_roll_deg = roll
self._last_pitch_deg = pitch
self._last_yaw_deg = yaw
return
dt = max(float(ts - self._last_angle_ts), 1e-4)
self.roll_rate_dps = self._wrap_angle_delta_deg(roll, self._last_roll_deg) / dt
self.pitch_rate_dps = self._wrap_angle_delta_deg(pitch, self._last_pitch_deg) / dt
self.yaw_rate_dps = self._wrap_angle_delta_deg(yaw, self._last_yaw_deg) / dt
self._last_angle_ts = ts
self._last_roll_deg = roll
self._last_pitch_deg = pitch
self._last_yaw_deg = yaw
def _atualizar_filtro_leve(self, roll, pitch, yaw):
a = self.filtro_alpha
if self.packets_processados <= 1:
self.roll_filtrado_deg = roll
self.pitch_filtrado_deg = pitch
self.yaw_filtrado_deg = yaw
return
self.roll_filtrado_deg = (1.0 - a) * self.roll_filtrado_deg + a * roll
self.pitch_filtrado_deg = (1.0 - a) * self.pitch_filtrado_deg + a * pitch
# Yaw com wrap
dyaw = self._wrap_angle_delta_deg(yaw, self.yaw_filtrado_deg)
self.yaw_filtrado_deg = self.yaw_filtrado_deg + a * dyaw
self.yaw_filtrado_deg = (self.yaw_filtrado_deg + 180.0) % 360.0 - 180.0
# ==========================================================
# Calibração
# ==========================================================
def _processar_calibracao(self, q_xyzw):
"""
Alimenta calibração com quaternion atual.
Quando completa, define q_ref_inv.
"""
if not self.calibrando:
return
self.calib_quats_xyzw.append(q_xyzw)
try:
roll_abs, pitch_abs, _ = R.from_quat(q_xyzw).as_euler("xyz", degrees=True)
self._calib_roll_hist.append(float(roll_abs))
self._calib_pitch_hist.append(float(pitch_abs))
except Exception:
pass
elapsed = time.perf_counter() - self.calib_inicio_ts
if elapsed < self.calibracao_segundos:
return
q_ref = self._media_quaternion_xyzw(self.calib_quats_xyzw)
if q_ref is None:
self.calibrando = False
self.calibrado = False
self.q_ref_inv = None
self.mostrar_log("Falha na calibração: quaternion médio inválido.")
return
self.q_ref_inv = R.from_quat(q_ref).inv()
self.calibrando = False
self.calibrado = True
roll_std = float(np.std(self._calib_roll_hist)) if self._calib_roll_hist else 0.0
pitch_std = float(np.std(self._calib_pitch_hist)) if self._calib_pitch_hist else 0.0
self.mostrar_log(
f"Calibração concluída | amostras={len(self.calib_quats_xyzw)} | "
f"std_roll={roll_std:.3f}° | std_pitch={pitch_std:.3f}°"
)
# ==========================================================
# Consumo da queue
# ==========================================================
def _drain_latest_imu_data(self):
"""
Consome a fila sem deixar backlog.
Retorna o último IMUData disponível.
Isso é importante para segurança: dado velho é pior que dado ruidoso.
"""
latest = None
drenados = 0
while drenados < self.max_queue_drain:
novo = self.imu_queue.tryGet()
if novo is None:
break
latest = novo
drenados += 1
if drenados > 1:
self.packets_descartados += (drenados - 1)
return latest
def _processar_packet(self, packet):
q_xyzw, accuracy_rad, sensor_ts = self._extrair_quaternion_do_packet(packet)
if q_xyzw is None:
return False
agora = time.perf_counter()
agora_wall = time.time()
self.last_packet_ts = agora_wall
self.packets_processados += 1
self._processar_calibracao(q_xyzw)
roll, pitch, yaw = self._quat_relativo_para_euler(q_xyzw)
self.roll_deg = roll
self.pitch_deg = pitch
self.yaw_deg = yaw
self._atualizar_rates(roll, pitch, yaw, agora)
self._atualizar_filtro_leve(roll, pitch, yaw)
self.last_imu_ts = sensor_ts
self.last_data = {
"valido": True,
"mx_id": self.mx_id,
"sensor": self.nome_sensor,
"tipo": "depthai_rotation_vector",
"calibrado": bool(self.calibrado),
"calibrando": bool(self.calibrando),
"calibracao_segundos": self.calibracao_segundos,
# Ângulo bruto relativo ao zero calibrado.
# Este é o dado rápido.
"roll": round(self.roll_deg, 3),
"pitch": round(self.pitch_deg, 3),
"yaw": round(self.yaw_deg, 3),
# Ângulo com filtro leve.
# Útil para painel e consumo menos nervoso.
"roll_filtrado": round(self.roll_filtrado_deg, 3),
"pitch_filtrado": round(self.pitch_filtrado_deg, 3),
"yaw_filtrado": round(self.yaw_filtrado_deg, 3),
# Mantive aliases para compatibilidade com quem já esperava roll_seg/pitch_seg.
"roll_seg": round(self.roll_filtrado_deg, 3),
"pitch_seg": round(self.pitch_filtrado_deg, 3),
# Tendência angular.
"roll_rate_dps": round(self.roll_rate_dps, 3),
"pitch_rate_dps": round(self.pitch_rate_dps, 3),
"yaw_rate_dps": round(self.yaw_rate_dps, 3),
# Metadados do rotation vector.
"rotation_accuracy_rad": accuracy_rad,
# Saúde operacional básica.
"timestamp": agora_wall * 1000.0,
"last_packet_ts": self.last_packet_ts,
"last_publish_ts": self.last_publish_ts,
"ultimo_erro_ts": self.ultimo_erro_ts,
"latencia": round(self.latencia_loop, 6),
"frequencia": round(self.frequencia_loop, 2),
"packets_processados": self.packets_processados,
"packets_descartados": self.packets_descartados,
}
return True
# ==========================================================
# Loop principal
# ==========================================================
def imu_task_loop(self):
from camera_worker.manager import definir_imu_camera
self.last_loop_ts = time.perf_counter()
last_publish_perf = 0.0
while self.ativo:
t_loop_ini = time.perf_counter()
try:
imuData = self._drain_latest_imu_data()
if imuData is not None and self.imu_em_falha:
self.mostrar_log("Reconectado com sucesso, voltando ao modo normal")
self.imu_em_falha = False
if imuData is not None and len(imuData.packets) > 0:
# Processa todos os packets do último IMUData recebido.
# Se quiser latência mínima absoluta, pode trocar para imuData.packets[-1].
for packet in imuData.packets:
self._processar_packet(packet)
# Frequência real do loop
now = time.perf_counter()
dt_loop = max(now - self.last_loop_ts, 1e-6)
self.frequencia_loop = 1.0 / dt_loop
self.last_loop_ts = now
self.latencia_loop = now - t_loop_ini
# Publicação desacoplada da taxa de leitura
if self.last_data is not None and (now - last_publish_perf) >= self.publish_period:
self.last_publish_ts = time.time()
self.last_data["last_publish_ts"] = self.last_publish_ts
self.last_data["latencia"] = round(self.latencia_loop, 6)
self.last_data["frequencia"] = round(self.frequencia_loop, 2)
definir_imu_camera(self.mx_id, dict(self.last_data), self.ultima_saude)
last_publish_perf = now
# Sem falha: sleep pequeno, porque a queue é non-blocking.
# Não usamos sleep 1/freq rígido porque queremos drenar pacotes recentes.
time.sleep(0.001)
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
self.mostrar_log("Entrando em modo de reconexão lenta")
else:
# Mantém vivo, mas reduz agressividade para não girar CPU em erro contínuo.
time.sleep(1.0)
# ==========================================================
# Saúde
# ==========================================================
def atualizar_saude(self):
try:
agora = time.time()
data = self.get_dados()
last_packet_ts = float(data.get("last_packet_ts", self.last_packet_ts) or 0.0)
last_publish_ts = float(data.get("last_publish_ts", self.last_publish_ts) or 0.0)
ultimo_erro_ts = float(data.get("ultimo_erro_ts", self.ultimo_erro_ts) or 0.0)
freq_hz = float(data.get("frequencia", 0.0) or 0.0)
latencia = float(data.get("latencia", 0.0) or 0.0)
valido = bool(data.get("valido", False))
calibrado = bool(data.get("calibrado", False))
calibrando = bool(data.get("calibrando", False))
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
conectado = bool(self.ativo and tempo_sem_packet < 3.0)
saude = 100
motivos = []
if not self.ativo:
conectado = False
saude = 0
motivos.append("Thread da IMU parada")
elif not valido:
saude = 0
motivos.append("IMU ainda sem leitura válida")
elif not conectado:
saude = 0
motivos.append(f"Sem packets da IMU há {tempo_sem_packet:.2f}s")
else:
if calibrando:
saude -= 20
motivos.append("IMU em calibração inicial")
elif not calibrado:
saude -= 35
motivos.append("IMU ainda não calibrada")
if tempo_sem_packet > 1.0:
saude -= 35
motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s")
elif tempo_sem_packet > 0.3:
saude -= 15
motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s")
if tempo_sem_publicar > 1.0:
saude -= 25
motivos.append(f"Sem publicar há {tempo_sem_publicar:.2f}s")
elif tempo_sem_publicar > 0.3:
saude -= 10
motivos.append(f"Leve atraso de publicação: {tempo_sem_publicar:.2f}s")
# Como o loop é non-blocking, freq_hz aqui não precisa bater freq IMU.
# Mesmo assim, se estiver muito baixo, algo travou.
if freq_hz <= 5:
saude -= 20
motivos.append(f"Loop lento: {freq_hz:.2f} Hz")
if latencia > 0.2:
saude -= 15
motivos.append(f"Latência alta no loop: {latencia:.3f}s")
elif latencia > 0.05:
saude -= 5
motivos.append(f"Latência moderada no loop: {latencia:.3f}s")
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, int(saude)))
status = StatusModulo.OPERANTE
if not conectado:
status = StatusModulo.DESCONECTADO
elif saude <= 45:
status = StatusModulo.FALHA
elif saude < 80:
status = StatusModulo.ALERTA
self.ultima_saude = {
"conectado": conectado,
"status": status.value,
"saude": saude,
"motivos": motivos,
"saude_individual": [],
}
return self.ultima_saude
except Exception as e:
self.mostrar_log(f"Erro ao atualizar saúde: {e}")
self.ultima_saude = {
"conectado": False,
"status": StatusModulo.FALHA.value,
"saude": 0,
"motivos": [str(e)],
"saude_individual": [],
}
return self.ultima_saude
def get_dados(self):
if self.last_data is None:
return {
"valido": False,
"mx_id": self.mx_id,
"sensor": self.nome_sensor,
"tipo": "depthai_rotation_vector",
"calibrado": False,
"calibrando": bool(self.calibrando),
"calibracao_segundos": self.calibracao_segundos,
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"roll_filtrado": 0.0,
"pitch_filtrado": 0.0,
"yaw_filtrado": 0.0,
"roll_seg": 0.0,
"pitch_seg": 0.0,
"roll_rate_dps": 0.0,
"pitch_rate_dps": 0.0,
"yaw_rate_dps": 0.0,
"rotation_accuracy_rad": None,
"timestamp": 0.0,
"last_packet_ts": 0.0,
"last_publish_ts": 0.0,
"ultimo_erro_ts": self.ultimo_erro_ts,
"latencia": 0.0,
"frequencia": 0.0,
"packets_processados": 0,
"packets_descartados": 0,
}
return dict(self.last_data)
def mostrar_log(self, mensagem):
print(f"{time.time()} - [IMUCamera][mx_id={self.mx_id}][id={id(self)}] {mensagem}")

58
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py
View File

@ -12,7 +12,7 @@ from camera_worker.tcp_streamer import CameraTcpStreamer
from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
from camera_worker.oak_fcc3_core.oak_fcc3_client import OakFcc3Client
from health_worker.modulos.imu import IMUCamera
from camera_worker.camera_imu import IMUCamera
class CameraMultispectral:
@ -53,6 +53,10 @@ class CameraMultispectral:
self.tem_imu = False
self.tem_depth = False
self.imu_freq_hz = 200
self.imu_publish_hz = 50
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
self.iniciado = False
self.rodando = False
self.ultima_saude = {}
@ -137,31 +141,41 @@ class CameraMultispectral:
resp = self.client.start(print_debug=False)
# Se mx_id veio None, pega o ID real aberto pelo core antes de criar IMU.
try:
self.mx_id = str(getattr(self.client, "mx_id", None) or self.mx_id)
except Exception:
pass
status = self._safe_get_status()
self.tem_imu = bool(status.get("tem_imu", False))
if self.tem_imu:
try:
self.q_imu = self.client.svc.manager.q_imu
if self.q_imu is not None:
self.imu = IMUCamera(
self.mx_id,
self.q_imu,
freq=100,
angulo_inicial=0.0,
mx_id=self.mx_id,
queue=self.q_imu,
freq=self.imu_freq_hz,
auto_calibrar=True,
calibracao_segundos=3.0,
publish_hz=self.imu_publish_hz,
filtro_alpha=0.35,
max_queue_drain=20,
nome_sensor=f"{self.modelo}_imu"
)
else:
self.tem_imu = False
self.mostrar_log("[CameraMultispectral] q_imu indisponível no manager.")
except Exception as e:
self.tem_imu = False
self.q_imu = None
self.imu = None
self.mostrar_log(f"[CameraMultispectral] IMU indisponível: {e}")
# Se mx_id veio None, pega o ID real aberto pelo core, se disponível.
try:
self.mx_id = str(getattr(self.client, "mx_id", None) or self.mx_id)
except Exception:
pass
status = self._safe_get_status()
self.versao = status.get("backend", "oak_fcc3")
@ -240,7 +254,7 @@ class CameraMultispectral:
parametros = {
"tipo": "multispectral",
"module_calibration_json": self.module_calibration_json,
"frame_type": "MULTISPEC",
"frame_type": "RAW_BRUTO",
"channels": ["R", "G", "B", "RE", "NIR"],
"output_layout": "CHW",
"dtype": "float32",
@ -372,10 +386,20 @@ class CameraMultispectral:
self.dispositivo,
imu={
"valido": False,
"calibrado": False,
"calibrando": False,
"timestamp": 0.0,
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"roll_filtrado": 0.0,
"pitch_filtrado": 0.0,
"yaw_filtrado": 0.0,
"roll_seg": 0.0,
"pitch_seg": 0.0,
"roll_rate_dps": 0.0,
"pitch_rate_dps": 0.0,
"yaw_rate_dps": 0.0,
}
)
except Exception:
@ -684,10 +708,20 @@ class CameraMultispectral:
}
imu = self.imu.get_dados() if self.imu else {
"valido": False,
"calibrado": False,
"calibrando": False,
"timestamp": 0.0,
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"roll_filtrado": 0.0,
"pitch_filtrado": 0.0,
"yaw_filtrado": 0.0,
"roll_seg": 0.0,
"pitch_seg": 0.0,
"roll_rate_dps": 0.0,
"pitch_rate_dps": 0.0,
"yaw_rate_dps": 0.0,
}
resultado = dict(self._ultimo_resultado_tensor)

50
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py
View File

@ -4,7 +4,7 @@ import time
import threading
from camera_worker.tcp_streamer import CameraTcpStreamer
from health_worker.modulos.imu import IMUCamera
from camera_worker.camera_imu import IMUCamera
from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
@ -26,6 +26,9 @@ class CameraOak:
self.tem_depth = False
self.tem_imu = False
self.imu = None
self.imu_freq_hz = 200
self.imu_publish_hz = 50
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
self.rodando = False
self.iniciado = False
@ -119,8 +122,18 @@ class CameraOak:
self.q_depth = self.device.getOutputQueue(name="depth", maxSize=1, blocking=False)
if self.tem_imu and iniciar_imu:
self.q_imu = self.device.getOutputQueue(name="imu", maxSize=50, blocking=False)
self.imu = IMUCamera(self.mx_id, self.q_imu, freq=100, angulo_inicial=26.3)
self.q_imu = self.device.getOutputQueue(name="imu", maxSize=8, blocking=False)
self.imu = IMUCamera(
mx_id=self.mx_id,
queue=self.q_imu,
freq=self.imu_freq_hz,
auto_calibrar=True,
calibracao_segundos=3.0,
publish_hz=self.imu_publish_hz,
filtro_alpha=0.35,
max_queue_drain=20,
nome_sensor=f"{self.modelo}_imu"
)
if self.modelo_ia_seg is not None:
self.q_seg = self.device.getOutputQueue(name="seg", maxSize=1, blocking=False)
@ -583,10 +596,20 @@ class CameraOak:
imu = self.imu.get_dados() if self.imu else {
"valido": False,
"calibrado": False,
"calibrando": False,
"timestamp": 0.0,
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"roll_filtrado": 0.0,
"pitch_filtrado": 0.0,
"yaw_filtrado": 0.0,
"roll_seg": 0.0,
"pitch_seg": 0.0,
"roll_rate_dps": 0.0,
"pitch_rate_dps": 0.0,
"yaw_rate_dps": 0.0,
}
resultado = dict(self._rgb_cache_resultado or {})
@ -760,15 +783,28 @@ class CameraOak:
if self.tem_imu:
try:
imu = pipeline.create(dai.node.IMU)
imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 60)
imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 60)
# Para segurança de inclinação:
# GAME_ROTATION_VECTOR entrega quaternion pronto com roll/pitch referenciados à gravidade,
# sem depender de magnetômetro.
imu.enableIMUSensor(dai.IMUSensor.GAME_ROTATION_VECTOR, self.imu_freq_hz)
# Envia pacote assim que tiver amostra.
# Isso reduz latência.
imu.setBatchReportThreshold(1)
imu.setMaxBatchReports(20)
# Mantém pequeno para evitar rajadas grandes.
# Se o host atrasar, preferimos perder pacotes velhos e ficar com dado novo.
imu.setMaxBatchReports(5)
xoutImu = pipeline.create(dai.node.XLinkOut)
xoutImu.setStreamName("imu")
imu.out.link(xoutImu.input)
self.mostrar_log(f"Pipeline imu criado")
self.mostrar_log(f"Pipeline imu criado | sensor={self.imu_sensor_type} | freq={self.imu_freq_hz}Hz")
except Exception as e:
self.tem_imu = False
self.mostrar_log(f"[WARN] Falha ao montar pipeline imu: {e}")
script = None

19
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py
View File

@ -334,17 +334,24 @@ class OakFcc3Manager:
try:
imu = pipeline.create(dai.node.IMU)
imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 100)
imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 100)
# Para inclinação do rover:
# GAME_ROTATION_VECTOR entrega quaternion pronto com roll/pitch
# referenciados à gravidade e sem depender de magnetômetro.
imu.enableIMUSensor(dai.IMUSensor.GAME_ROTATION_VECTOR, 200)
# Envia assim que tiver amostra.
imu.setBatchReportThreshold(1)
imu.setMaxBatchReports(20)
# Evita rajadas grandes e reduz backlog.
imu.setMaxBatchReports(5)
xout_imu = pipeline.create(dai.node.XLinkOut)
xout_imu.setStreamName("imu")
imu.out.link(xout_imu.input)
self.has_imu_pipeline = True
print("[OAK] Pipeline IMU criado")
print("[OAK] Pipeline IMU criado | GAME_ROTATION_VECTOR | 200Hz")
except Exception as e:
self.has_imu_pipeline = False
@ -812,11 +819,11 @@ class OakFcc3Manager:
try:
self.q_imu = self.device.getOutputQueue(
name="imu",
maxSize=50,
maxSize=8,
blocking=False,
)
self.tem_imu = True
print("[OAK] Fila IMU criada")
print("[OAK] Fila IMU criada | maxSize=8")
except Exception as e:
self.q_imu = None
self.tem_imu = False

4
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py
View File

@ -13,7 +13,7 @@ def main():
from health_worker.modulos.sensoriamento import ModuloSensoriamento
from health_worker.modulos.atuador import ModuloAtuador
from health_worker.modulos.lora import ModuloLoRa
from health_worker.modulos.imu import IMUCamera
from health_worker.modulos.imu import CameraIMU
from health_worker.modulos.pc import ModuloPC
from health_worker.modulos.livox import ModuloLivox
from health_worker.modulos.ponte_ip import ModuloIPBribge
@ -30,7 +30,7 @@ def main():
T_Code.Sen: ModuloSensoriamento(),
T_Code.Atu: ModuloAtuador(),
T_Code.Lra: ModuloLoRa(),
#T_Code.Imu: IMUCamera(),
T_Code.Imu: CameraIMU(),
T_Code.Npc: ModuloPC(),
T_Code.Lvx: ModuloLivox(),
T_Code.Ipb: ModuloIPBribge(),

1486
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py
View File

File diff suppressed because it is too large Load Diff