criado componente de imu unificado entre as cameras OAK

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py

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

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

@@ -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,
+                            mx_id=self.mx_id,
-                            self.q_imu,
+                            queue=self.q_imu,
-                            freq=100,
+                            freq=self.imu_freq_hz,
-                            angulo_inicial=0.0,
+                            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)

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

@@ -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.q_imu = self.device.getOutputQueue(name="imu", maxSize=8, blocking=False)
-                self.imu = IMUCamera(self.mx_id, self.q_imu, freq=100, angulo_inicial=26.3)
+                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

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

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

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

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