From 6209681ec3061b9651facd71165dfffe35c99469 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Wed, 10 Jun 2026 21:20:10 -0300 Subject: [PATCH] criado componente de imu unificado entre as cameras OAK --- .../workers/camera_worker/camera_imu.py | 658 ++++++++ .../camera_worker/camera_multispectral.py | 58 +- .../workers/camera_worker/camera_oak.py | 50 +- .../oak_fcc3_core/oak_fcc3_manager.py | 19 +- .../Scripts/workers/health_worker/main.py | 4 +- .../workers/health_worker/modulos/imu.py | 1492 +++++++++++------ 6 files changed, 1744 insertions(+), 537 deletions(-) create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py new file mode 100644 index 000000000..8621e1791 --- /dev/null +++ b/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}") diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py index 7631ade0f..dbe56dcf1 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py +++ b/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, - 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) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py index c67f333c3..2731fa633 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py +++ b/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.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 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py index 536be8425..e983aeb85 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py +++ b/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 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py index 85c06e9c4..0bbf05f3a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py +++ b/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(), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 24b2fab46..8707cee03 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py @@ -1,533 +1,966 @@ -import numpy as np import time import threading +import math from collections import deque -from ahrs.filters import Madgwick -from scipy.spatial.transform import Rotation as R + +import numpy as np + from shared.contexto_global_redis import ContextoGlobalRedis from shared.enums import StatusModulo, T_Code from health_worker.modulos.base import ModuloDiagnosticoBase -class IMUCamera(ModuloDiagnosticoBase): - def __init__(self, mx_id, queue=None, freq=100, angulo_inicial=26.3): - self.mx_id = mx_id - self.freq = freq - self.ultima_saude = None +class CameraIMU(ModuloDiagnosticoBase): + """ + IMU final do robô. - self.ativo = False - self.last_packet_ts = 0.0 - self.last_publish_ts = 0.0 - self.ultimo_erro_ts = 0.0 + Responsabilidade: + - Ler os dados de IMU já processados dentro de cada câmera. + - Validar quais IMUs estão frescas, calibradas e confiáveis. + - Fundir roll/pitch/yaw em uma atitude final do robô. + - Gerar campos de segurança para o manager_worker. + - Publicar o resultado definitivo em ContextoGlobalRedis.ModKey(T_Code.Imu). - self.last_data = None - - if queue is None: - return + Esta classe NÃO: + - Lê fila DepthAI. + - Roda Madgwick. + - Integra acelerômetro. + - Publica imu individual de câmera. + """ - self.imu_queue = queue + def __init__( + self, + freq=50, + publish_hz=20, + timeout_imu_s=0.40, + timeout_camera_s=2.0, + max_disagreement_deg=4.0, + roll_attention_deg=7.0, + roll_risk_deg=11.0, + roll_critical_deg=16.0, + roll_emergency_deg=22.0, + pitch_attention_deg=8.0, + pitch_risk_deg=14.0, + pitch_critical_deg=20.0, + pitch_emergency_deg=28.0, + roll_rate_attention_dps=25.0, + roll_rate_risk_dps=40.0, + roll_rate_critical_dps=65.0, + pitch_rate_attention_dps=25.0, + pitch_rate_risk_dps=45.0, + pitch_rate_critical_dps=75.0, + filtro_alpha=0.35, + historico_len=100, + ): + self.freq = float(freq) + self.publish_hz = float(publish_hz) + self.publish_period = 1.0 / max(self.publish_hz, 1.0) - # beta bem mais conservador - self.filtro_imu = Madgwick(beta=0.05, frequency=freq) + self.timeout_imu_s = float(timeout_imu_s) + self.timeout_camera_s = float(timeout_camera_s) + self.max_disagreement_deg = float(max_disagreement_deg) - self.roll_inicial = 90 - angulo_inicial - self.pitch_inicial = 0.0 - self.yaw_inicial = 0.0 + self.roll_attention_deg = float(roll_attention_deg) + self.roll_risk_deg = float(roll_risk_deg) + self.roll_critical_deg = float(roll_critical_deg) + self.roll_emergency_deg = float(roll_emergency_deg) - r = R.from_euler('xyz', [self.roll_inicial, self.pitch_inicial, self.yaw_inicial], degrees=True) - q = r.as_quat() - self.q = np.array([q[3], q[0], q[1], q[2]], dtype=np.float64) + self.pitch_attention_deg = float(pitch_attention_deg) + self.pitch_risk_deg = float(pitch_risk_deg) + self.pitch_critical_deg = float(pitch_critical_deg) + self.pitch_emergency_deg = float(pitch_emergency_deg) - self.imu_em_falha = False - self.tempo_erro = 0 + self.roll_rate_attention_dps = float(roll_rate_attention_dps) + self.roll_rate_risk_dps = float(roll_rate_risk_dps) + self.roll_rate_critical_dps = float(roll_rate_critical_dps) - self.v_world = np.zeros(3, dtype=np.float64) - self.g = 9.80665 + self.pitch_rate_attention_dps = float(pitch_rate_attention_dps) + self.pitch_rate_risk_dps = float(pitch_rate_risk_dps) + self.pitch_rate_critical_dps = float(pitch_rate_critical_dps) - self.last_loop_ts = None - - # filtros de segurança - self.roll_seg = 0.0 - self.pitch_seg = 0.0 - self.alpha_seg = 0.12 - - # mediana curta para matar espinhos - self.roll_hist = deque(maxlen=5) - self.pitch_hist = deque(maxlen=5) - - self.acc_norm_hist = deque(maxlen=100) # ~1s - self.acc_z_hist = deque(maxlen=100) # ~1s - self.gyro_norm_hist = deque(maxlen=60) # ~0.6s - self.roll_seg_hist = deque(maxlen=60) # ~0.6s - self.pitch_seg_hist = deque(maxlen=60) # ~0.6s - self.impacto_hist = deque(maxlen=20) # ~0.2s para pico recente - self.em_movimento = False - self.movimento_idx = 0.0 - self.rugosidade_idx = 0.0 - self.impacto_idx = 0.0 - self.estabilidade_idx = 100.0 - self.redis_period = 0.10 # 100 ms - self.last_redis_ts = 0.0 + self.filtro_alpha = float(filtro_alpha) self.ativo = True - self.imu_thread = threading.Thread(target=self.imu_task_loop, daemon=True) - self.imu_thread.start() + self.last_data = None + self.ultima_saude = None + + self.last_loop_ts = time.time() + self.last_publish_ts = 0.0 + self.ultimo_erro_ts = 0.0 + self.latencia = 0.0 + self.frequencia = 0.0 + + self.roll_filtrado = 0.0 + self.pitch_filtrado = 0.0 + self.yaw_filtrado = 0.0 + + self.roll_rate_filtrado = 0.0 + self.pitch_rate_filtrado = 0.0 + + self.roll_hist = deque(maxlen=historico_len) + self.pitch_hist = deque(maxlen=historico_len) + self.roll_rate_hist = deque(maxlen=historico_len) + self.pitch_rate_hist = deque(maxlen=historico_len) + self.risk_hist = deque(maxlen=historico_len) + + self.thread = threading.Thread( + target=self.imu_task_loop, + name="CameraIMUFinalWorker", + daemon=True, + ) + self.thread.start() self.mostrar_log("Task iniciada") + # ============================================================ + # Ciclo 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) + if self.thread is not None and self.thread.is_alive(): + self.thread.join(timeout=1.0) except Exception: pass - def _mediana_curta(self, hist, valor): - hist.append(valor) - return float(np.median(hist)) + # ============================================================ + # Utilidades + # ============================================================ def clamp(self, valor, vmin, vmax): return max(vmin, min(vmax, valor)) - def norm_to_100(self, valor, faixa_min, faixa_max): - if faixa_max <= faixa_min: + def norm_to_100(self, valor, vmin, vmax): + if vmax <= vmin: return 0.0 - x = (valor - faixa_min) / (faixa_max - faixa_min) + x = (float(valor) - float(vmin)) / (float(vmax) - float(vmin)) return self.clamp(x * 100.0, 0.0, 100.0) - def rms(self, valores): - if not valores: + def _safe_float(self, data, key, default=0.0): + try: + v = data.get(key, default) + if v is None: + return float(default) + return float(v) + except Exception: + return float(default) + + def _safe_bool(self, data, key, default=False): + try: + return bool(data.get(key, default)) + except Exception: + return bool(default) + + def _angle_delta_deg(self, atual, anterior): + if anterior is None: return 0.0 - arr = np.asarray(valores, dtype=np.float64) - return float(np.sqrt(np.mean(arr ** 2))) + return (float(atual) - float(anterior) + 180.0) % 360.0 - 180.0 - def stddev(self, valores): - if not valores: + def _ema(self, anterior, atual, alpha): + return (1.0 - alpha) * float(anterior) + alpha * float(atual) + + def _modelo_peso_base(self, cam_data, mx_id): + modelo = str(cam_data.get("modelo", "") or "").lower() + dispositivo = str(cam_data.get("dispositivo", "") or "").lower() + + # OAK-D Lite presa no chassi/cabine deve ser a principal. + if "oak-d" in modelo or "snr" in dispositivo: + return 0.75 + + # Multiespectral na haste entra como redundância. + if "ffc" in modelo or "multiespectral" in modelo or "multispectral" in modelo: + return 0.35 + + # Fallback seguro. + return 0.50 + + # ============================================================ + # Leitura das IMUs das câmeras + # ============================================================ + + def _buscar_camera_e_imu_individual(self, mx_id): + """ + Busca os dados detalhados da câmera e a IMU individual dela. + + Ordem preferida: + 1) ContextoGlobalRedis.CamImuKey(mx_id) + Estrutura esperada: + { + "timestamp": ..., + "dados": {...}, + "saude": {...} + } + + 2) Fallback: ContextoGlobalRedis.get_camera(mx_id)["imu"] + Esse caminho existe porque definir_saude_camera também grava imu + dentro da chave detalhada da câmera. + """ + + cam_detalhada = ContextoGlobalRedis.get_camera(mx_id) or {} + + imu_ctx = None + try: + imu_ctx = ContextoGlobalRedis.get(ContextoGlobalRedis.CamImuKey(mx_id)) or {} + except Exception: + imu_ctx = {} + + imu_dados = None + imu_saude = None + + # Caminho principal: chave própria da IMU da câmera. + if isinstance(imu_ctx, dict): + dados = imu_ctx.get("dados") + saude = imu_ctx.get("saude") + + if isinstance(dados, dict): + imu_dados = dict(dados) + + if isinstance(saude, dict): + imu_saude = saude + imu_dados.setdefault("saude", saude) + + # Se a chave CamImuKey tem timestamp próprio, usa como fallback. + if "timestamp" not in imu_dados and imu_ctx.get("timestamp") is not None: + try: + imu_dados["timestamp"] = float(imu_ctx.get("timestamp")) * 1000.0 + except Exception: + pass + + # Fallback: IMU salva dentro da chave detalhada da câmera. + if imu_dados is None and isinstance(cam_detalhada, dict): + imu_camera = cam_detalhada.get("imu") + if isinstance(imu_camera, dict): + imu_dados = dict(imu_camera) + + return cam_detalhada, imu_dados + + def _extrair_imu_da_camera(self, mx_id, cam_data, imu, agora): + """ + Extrai uma fonte de IMU individual. + + cam_data: + dados detalhados da câmera, vindos de ContextoGlobalRedis.get_camera(mx_id) + + imu: + dados da IMU individual, preferencialmente vindos de CamImuKey(mx_id)["dados"] + """ + + if not isinstance(cam_data, dict): + cam_data = {} + + if not isinstance(imu, dict): + return None + + valido = self._safe_bool(imu, "valido", False) + calibrado = self._safe_bool(imu, "calibrado", False) + calibrando = self._safe_bool(imu, "calibrando", False) + + ts_ms = self._safe_float(imu, "timestamp", 0.0) + last_packet_ts = self._safe_float(imu, "last_packet_ts", 0.0) + last_publish_ts = self._safe_float(imu, "last_publish_ts", 0.0) + + # timestamp pode vir em ms. + ts_s = ts_ms / 1000.0 if ts_ms > 100000000000.0 else ts_ms + + candidatos_ts = [ + ts_s if ts_s > 0 else 0.0, + last_packet_ts if last_packet_ts > 0 else 0.0, + last_publish_ts if last_publish_ts > 0 else 0.0, + ] + + ts_ref = max(candidatos_ts) + idade_s = (agora - ts_ref) if ts_ref > 0 else 999.0 + + roll = self._safe_float(imu, "roll", 0.0) + pitch = self._safe_float(imu, "pitch", 0.0) + yaw = self._safe_float(imu, "yaw", 0.0) + + roll_filtrado = self._safe_float(imu, "roll_filtrado", imu.get("roll_seg", roll)) + pitch_filtrado = self._safe_float(imu, "pitch_filtrado", imu.get("pitch_seg", pitch)) + yaw_filtrado = self._safe_float(imu, "yaw_filtrado", yaw) + + roll_rate = self._safe_float(imu, "roll_rate_dps", 0.0) + pitch_rate = self._safe_float(imu, "pitch_rate_dps", 0.0) + yaw_rate = self._safe_float(imu, "yaw_rate_dps", 0.0) + + imu_saude = None + try: + saude_obj = imu.get("saude") + if isinstance(saude_obj, dict): + imu_saude = float(saude_obj.get("saude", 100.0)) + except Exception: + imu_saude = None + + peso_base = self._modelo_peso_base(cam_data, mx_id) + + motivos = [] + confiavel = True + + if not valido: + confiavel = False + motivos.append("imu_invalida") + + if calibrando: + confiavel = False + motivos.append("imu_calibrando") + + if not calibrado: + confiavel = False + motivos.append("imu_nao_calibrada") + + if idade_s > self.timeout_imu_s: + confiavel = False + motivos.append(f"imu_antiga_{idade_s:.2f}s") + + if imu_saude is not None and imu_saude < 45: + confiavel = False + motivos.append(f"imu_saude_baixa_{imu_saude:.0f}") + + freshness = 1.0 - self.norm_to_100(idade_s, 0.0, self.timeout_imu_s) / 100.0 + freshness = self.clamp(freshness, 0.0, 1.0) + + calib_score = 1.0 if calibrado and not calibrando else 0.35 + valido_score = 1.0 if valido else 0.0 + + saude_score = 1.0 + if imu_saude is not None: + saude_score = self.clamp(imu_saude / 100.0, 0.0, 1.0) + + confidence = peso_base * freshness * calib_score * valido_score * saude_score + + if not confiavel: + confidence = 0.0 + + return { + "mx_id": str(mx_id), + "modelo": cam_data.get("modelo"), + "dispositivo": cam_data.get("dispositivo"), + + "valido": valido, + "calibrado": calibrado, + "calibrando": calibrando, + "confiavel": bool(confiavel), + "motivos": motivos, + + "idade_s": float(idade_s), + "confidence": float(confidence), + "peso_base": float(peso_base), + + "roll": float(roll), + "pitch": float(pitch), + "yaw": float(yaw), + + "roll_filtrado": float(roll_filtrado), + "pitch_filtrado": float(pitch_filtrado), + "yaw_filtrado": float(yaw_filtrado), + + "roll_rate_dps": float(roll_rate), + "pitch_rate_dps": float(pitch_rate), + "yaw_rate_dps": float(yaw_rate), + + "timestamp": ts_ms, + "last_packet_ts": last_packet_ts, + "last_publish_ts": last_publish_ts, + } + + def _ler_fontes_imu(self): + """ + Lê as IMUs individuais das câmeras. + + get_cameras(): + usado apenas como inventário/lista de câmeras conhecidas. + + get_camera(mx_id) + CamImuKey(mx_id): + usados para buscar os dados reais de cada câmera. + """ + + agora = time.time() + + cameras_mapeadas = ContextoGlobalRedis.get_cameras() or {} + + fontes = [] + ignoradas = [] + + if not isinstance(cameras_mapeadas, dict): + return [], [] + + for mx_id in list(cameras_mapeadas.keys()): + cam_detalhada, imu_dados = self._buscar_camera_e_imu_individual(mx_id) + + fonte = self._extrair_imu_da_camera( + mx_id=mx_id, + cam_data=cam_detalhada, + imu=imu_dados, + agora=agora, + ) + + if fonte is None: + ignoradas.append({ + "mx_id": str(mx_id), + "modelo": cam_detalhada.get("modelo") if isinstance(cam_detalhada, dict) else None, + "dispositivo": cam_detalhada.get("dispositivo") if isinstance(cam_detalhada, dict) else None, + "confiavel": False, + "idade_s": 999.0, + "confidence": 0.0, + "motivos": ["sem_dados_imu_na_camera"], + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "roll_rate_dps": 0.0, + "pitch_rate_dps": 0.0, + "yaw_rate_dps": 0.0, + }) + continue + + if fonte["confiavel"]: + fontes.append(fonte) + else: + ignoradas.append(fonte) + + return fontes, ignoradas + + # ============================================================ + # Fusão + # ============================================================ + + def _weighted_mean(self, fontes, key): + pesos = np.asarray([max(0.0, f.get("confidence", 0.0)) for f in fontes], dtype=np.float64) + valores = np.asarray([float(f.get(key, 0.0)) for f in fontes], dtype=np.float64) + + soma = float(np.sum(pesos)) + if soma <= 1e-9: return 0.0 - arr = np.asarray(valores, dtype=np.float64) - return float(np.std(arr)) - def _atualizar_indices(self, velocidade_mps, gx, gy, gz, a_lin): - acc_lin_norm = float(np.linalg.norm(a_lin)) - gyro_norm_deg = float(np.rad2deg(np.linalg.norm([gx, gy, gz]))) + return float(np.sum(valores * pesos) / soma) - # Históricos - self.acc_norm_hist.append(acc_lin_norm) - self.acc_z_hist.append(float(a_lin[2])) - self.gyro_norm_hist.append(gyro_norm_deg) - self.roll_seg_hist.append(float(self.roll_seg)) - self.pitch_seg_hist.append(float(self.pitch_seg)) - self.impacto_hist.append(acc_lin_norm) + def _fused_yaw(self, fontes): + """ + Yaw é simbólico aqui, mas fazemos média circular para não quebrar em -180/180. + """ + pesos = np.asarray([max(0.0, f.get("confidence", 0.0)) for f in fontes], dtype=np.float64) + soma = float(np.sum(pesos)) + if soma <= 1e-9: + return 0.0 - # ============================ - # 1) Flag em movimento - # ============================ - vel_ok = velocidade_mps > 0.05 - gyro_ok = gyro_norm_deg > 3.0 - acc_ok = acc_lin_norm > 0.20 - self.em_movimento = bool(vel_ok or gyro_ok or acc_ok) + angs = np.radians([float(f.get("yaw", 0.0)) for f in fontes]) + s = float(np.sum(np.sin(angs) * pesos)) + c = float(np.sum(np.cos(angs) * pesos)) + return float(np.degrees(math.atan2(s, c))) - # ============================ - # 2) Movimento idx - # ============================ - vel_score = self.norm_to_100(velocidade_mps, 0.0, 1.8) - gyro_score = self.norm_to_100(gyro_norm_deg, 0.0, 40.0) - acc_score = self.norm_to_100(acc_lin_norm, 0.0, 1.5) + def _calcular_divergencia(self, fontes): + if len(fontes) < 2: + return { + "sensors_agree": True, + "roll_disagreement_deg": 0.0, + "pitch_disagreement_deg": 0.0, + "max_disagreement_deg": 0.0, + "divergentes": [], + } - movimento_idx = ( - 0.50 * vel_score + - 0.25 * gyro_score + - 0.25 * acc_score - ) - self.movimento_idx = round(self.clamp(movimento_idx, 0.0, 100.0), 1) + rolls = [float(f["roll"]) for f in fontes] + pitchs = [float(f["pitch"]) for f in fontes] - # ============================ - # 3) Rugosidade idx - # ============================ - rug_z = self.rms(self.acc_z_hist) - rug_total = self.stddev(self.acc_norm_hist) + roll_dis = float(max(rolls) - min(rolls)) + pitch_dis = float(max(pitchs) - min(pitchs)) + max_dis = max(roll_dis, pitch_dis) - rug_z_score = self.norm_to_100(rug_z, 0.02, 0.80) - rug_total_score = self.norm_to_100(rug_total, 0.01, 0.60) + sensores_agree = max_dis <= self.max_disagreement_deg - rugosidade_idx = ( - 0.65 * rug_z_score + - 0.35 * rug_total_score - ) - self.rugosidade_idx = round(self.clamp(rugosidade_idx, 0.0, 100.0), 1) + divergentes = [] + if not sensores_agree: + for f in fontes: + divergentes.append({ + "mx_id": f["mx_id"], + "roll": round(f["roll"], 3), + "pitch": round(f["pitch"], 3), + "confidence": round(f["confidence"], 3), + }) - # ============================ - # 4) Impacto idx - # ============================ - pico_impacto = max(self.impacto_hist) if self.impacto_hist else 0.0 - impacto_bruto = self.norm_to_100(pico_impacto, 0.4, 4.0) + return { + "sensors_agree": bool(sensores_agree), + "roll_disagreement_deg": roll_dis, + "pitch_disagreement_deg": pitch_dis, + "max_disagreement_deg": max_dis, + "divergentes": divergentes, + } - # decaimento suave - self.impacto_idx = max(impacto_bruto, self.impacto_idx * 0.85) - self.impacto_idx = round(self.clamp(self.impacto_idx, 0.0, 100.0), 1) + def _fundir_fontes(self, fontes): + if not fontes: + return None - # ============================ - # 5) Estabilidade idx - # ============================ - roll_std = self.stddev(self.roll_seg_hist) - pitch_std = self.stddev(self.pitch_seg_hist) - osc_ang = float(np.sqrt(roll_std**2 + pitch_std**2)) + # Se só uma fonte existe, usa ela diretamente. + if len(fontes) == 1: + f = fontes[0] + return { + "roll": f["roll"], + "pitch": f["pitch"], + "yaw": f["yaw"], + "roll_rate_dps": f["roll_rate_dps"], + "pitch_rate_dps": f["pitch_rate_dps"], + "yaw_rate_dps": f["yaw_rate_dps"], + "confidence": f["confidence"], + "fonte_principal": f["mx_id"], + "single_source": True, + } - osc_score = self.norm_to_100(osc_ang, 0.2, 8.0) + # Com duas ou mais, média ponderada por confiança. + fonte_principal = max(fontes, key=lambda x: x.get("confidence", 0.0)) - instabilidade = ( - 0.60 * osc_score + - 0.40 * self.rugosidade_idx + return { + "roll": self._weighted_mean(fontes, "roll"), + "pitch": self._weighted_mean(fontes, "pitch"), + "yaw": self._fused_yaw(fontes), + "roll_rate_dps": self._weighted_mean(fontes, "roll_rate_dps"), + "pitch_rate_dps": self._weighted_mean(fontes, "pitch_rate_dps"), + "yaw_rate_dps": self._weighted_mean(fontes, "yaw_rate_dps"), + "confidence": sum(float(f.get("confidence", 0.0)) for f in fontes), + "fonte_principal": fonte_principal["mx_id"], + "single_source": False, + } + + # ============================================================ + # Risco e ação sugerida + # ============================================================ + + def _nivel_num_to_str(self, nivel): + if nivel <= 0: + return "normal" + if nivel == 1: + return "attention" + if nivel == 2: + return "risk" + if nivel == 3: + return "critical" + return "emergency" + + def _avaliar_eixo(self, valor_abs, rate_abs, limites_ang, limites_rate): + attention, risk, critical, emergency = limites_ang + rate_attention, rate_risk, rate_critical = limites_rate + + nivel = 0 + motivos = [] + + if valor_abs >= emergency: + nivel = max(nivel, 4) + motivos.append("angulo_emergency") + elif valor_abs >= critical: + nivel = max(nivel, 3) + motivos.append("angulo_critical") + elif valor_abs >= risk: + nivel = max(nivel, 2) + motivos.append("angulo_risk") + elif valor_abs >= attention: + nivel = max(nivel, 1) + motivos.append("angulo_attention") + + if rate_abs >= rate_critical: + nivel = max(nivel, 3) + motivos.append("taxa_critical") + elif rate_abs >= rate_risk: + nivel = max(nivel, 2) + motivos.append("taxa_risk") + elif rate_abs >= rate_attention: + nivel = max(nivel, 1) + motivos.append("taxa_attention") + + return nivel, motivos + + def _avaliar_risco(self, roll, pitch, roll_rate, pitch_rate, sensores_agree, n_fontes): + abs_roll = abs(float(roll)) + abs_pitch = abs(float(pitch)) + abs_roll_rate = abs(float(roll_rate)) + abs_pitch_rate = abs(float(pitch_rate)) + + roll_nivel, roll_motivos = self._avaliar_eixo( + abs_roll, + abs_roll_rate, + ( + self.roll_attention_deg, + self.roll_risk_deg, + self.roll_critical_deg, + self.roll_emergency_deg, + ), + ( + self.roll_rate_attention_dps, + self.roll_rate_risk_dps, + self.roll_rate_critical_dps, + ), ) - estabilidade_idx = 100.0 - instabilidade - self.estabilidade_idx = round(self.clamp(estabilidade_idx, 0.0, 100.0), 1) + pitch_nivel, pitch_motivos = self._avaliar_eixo( + abs_pitch, + abs_pitch_rate, + ( + self.pitch_attention_deg, + self.pitch_risk_deg, + self.pitch_critical_deg, + self.pitch_emergency_deg, + ), + ( + self.pitch_rate_attention_dps, + self.pitch_rate_risk_dps, + self.pitch_rate_critical_dps, + ), + ) + + nivel = max(roll_nivel, pitch_nivel) + motivos = [] + + motivos += [f"roll_{m}" for m in roll_motivos] + motivos += [f"pitch_{m}" for m in pitch_motivos] + + if not sensores_agree: + nivel = max(nivel, 2) + motivos.append("imus_divergentes") + + if n_fontes <= 1: + motivos.append("sem_redundancia") + + risk_score = 0.0 + risk_score = max(risk_score, self.norm_to_100(abs_roll, self.roll_attention_deg, self.roll_emergency_deg)) + risk_score = max(risk_score, self.norm_to_100(abs_pitch, self.pitch_attention_deg, self.pitch_emergency_deg)) + risk_score = max(risk_score, self.norm_to_100(abs_roll_rate, self.roll_rate_attention_dps, self.roll_rate_critical_dps)) + risk_score = max(risk_score, self.norm_to_100(abs_pitch_rate, self.pitch_rate_attention_dps, self.pitch_rate_critical_dps)) + + # Ação sugerida para o manager_worker. + if nivel <= 0: + acao = "normal" + velocidade_factor = 1.0 + bloquear_avanco = False + stop = False + emergency_stop = False + elif nivel == 1: + acao = "reduzir_velocidade_leve" + velocidade_factor = 0.70 + bloquear_avanco = False + stop = False + emergency_stop = False + elif nivel == 2: + acao = "reduzir_velocidade_forte" + velocidade_factor = 0.35 + bloquear_avanco = False + stop = False + emergency_stop = False + elif nivel == 3: + acao = "parada_controlada" + velocidade_factor = 0.0 + bloquear_avanco = True + stop = True + emergency_stop = False + else: + acao = "emergency_stop" + velocidade_factor = 0.0 + bloquear_avanco = True + stop = True + emergency_stop = True + + return { + "risk_level": self._nivel_num_to_str(nivel), + "risk_level_num": int(nivel), + "risk_score": round(float(risk_score), 1), + "motivos_risco": motivos, + "acao_sugerida": acao, + "velocidade_factor": float(velocidade_factor), + "bloquear_avanco": bool(bloquear_avanco), + "stop": bool(stop), + "emergency_stop": bool(emergency_stop), + } + + # ============================================================ + # Publicação + # ============================================================ + + def _publicar(self, data): + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.ModKey(T_Code.Imu), + **data + ) + + def _montar_saida_sem_imu(self, fontes_ignoradas, agora): + return { + "valido": False, + "modo": "sem_imu", + "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, + "risk_level": "emergency", + "risk_level_num": 4, + "risk_score": 100.0, + "acao_sugerida": "emergency_stop", + "velocidade_factor": 0.0, + "bloquear_avanco": True, + "stop": True, + "emergency_stop": True, + "confidence": 0.0, + "sensors_count": 0, + "sensors_valid_count": 0, + "sensors_ignored_count": len(fontes_ignoradas), + "sensors_agree": False, + "roll_disagreement_deg": 0.0, + "pitch_disagreement_deg": 0.0, + "max_disagreement_deg": 0.0, + "fonte_principal": None, + "fontes": [], + "fontes_ignoradas": fontes_ignoradas, + "timestamp": agora * 1000.0, + "last_packet_ts": agora, + "last_publish_ts": agora, + "ultimo_erro_ts": self.ultimo_erro_ts, + "latencia": self.latencia, + "frequencia": self.frequencia, + } + + def _montar_saida(self, fontes, ignoradas, fusao, divergencia, risco, agora): + roll = float(fusao["roll"]) + pitch = float(fusao["pitch"]) + yaw = float(fusao["yaw"]) + + roll_rate = float(fusao["roll_rate_dps"]) + pitch_rate = float(fusao["pitch_rate_dps"]) + yaw_rate = float(fusao["yaw_rate_dps"]) + + a = self.filtro_alpha + + if self.last_data is None: + self.roll_filtrado = roll + self.pitch_filtrado = pitch + self.yaw_filtrado = yaw + self.roll_rate_filtrado = roll_rate + self.pitch_rate_filtrado = pitch_rate + else: + self.roll_filtrado = self._ema(self.roll_filtrado, roll, a) + self.pitch_filtrado = self._ema(self.pitch_filtrado, pitch, a) + + dyaw = self._angle_delta_deg(yaw, self.yaw_filtrado) + self.yaw_filtrado = self.yaw_filtrado + a * dyaw + self.yaw_filtrado = (self.yaw_filtrado + 180.0) % 360.0 - 180.0 + + self.roll_rate_filtrado = self._ema(self.roll_rate_filtrado, roll_rate, a) + self.pitch_rate_filtrado = self._ema(self.pitch_rate_filtrado, pitch_rate, a) + + self.roll_hist.append(roll) + self.pitch_hist.append(pitch) + self.roll_rate_hist.append(roll_rate) + self.pitch_rate_hist.append(pitch_rate) + self.risk_hist.append(int(risco["risk_level_num"])) + + oscilacao_roll = float(np.std(self.roll_hist)) if len(self.roll_hist) >= 2 else 0.0 + oscilacao_pitch = float(np.std(self.pitch_hist)) if len(self.pitch_hist) >= 2 else 0.0 + oscilacao_total = float(math.sqrt(oscilacao_roll ** 2 + oscilacao_pitch ** 2)) + + estabilidade_score = 100.0 - self.norm_to_100(oscilacao_total, 0.5, 8.0) + estabilidade_score = self.clamp(estabilidade_score, 0.0, 100.0) + + # Penaliza estabilidade se a redundância caiu. + if len(fontes) <= 1: + estabilidade_score = max(0.0, estabilidade_score - 10.0) + + # Penaliza se sensores divergem. + if not divergencia["sensors_agree"]: + estabilidade_score = max(0.0, estabilidade_score - 25.0) + + fonte_principal = fusao.get("fonte_principal") + + return { + "valido": True, + "modo": "redundante" if len(fontes) >= 2 else "single_source", + + # Dado rápido. + "roll": round(roll, 3), + "pitch": round(pitch, 3), + "yaw": round(yaw, 3), + + # Dado levemente suavizado. + "roll_filtrado": round(self.roll_filtrado, 3), + "pitch_filtrado": round(self.pitch_filtrado, 3), + "yaw_filtrado": round(self.yaw_filtrado, 3), + + # Compatibilidade com contrato antigo. + "roll_seg": round(self.roll_filtrado, 3), + "pitch_seg": round(self.pitch_filtrado, 3), + + # Tendência angular. + "roll_rate_dps": round(roll_rate, 3), + "pitch_rate_dps": round(pitch_rate, 3), + "yaw_rate_dps": round(yaw_rate, 3), + + "roll_rate_filtrado_dps": round(self.roll_rate_filtrado, 3), + "pitch_rate_filtrado_dps": round(self.pitch_rate_filtrado, 3), + + # Segurança. + "risk_level": risco["risk_level"], + "risk_level_num": risco["risk_level_num"], + "risk_score": risco["risk_score"], + "motivos_risco": risco["motivos_risco"], + + "acao_sugerida": risco["acao_sugerida"], + "velocidade_factor": risco["velocidade_factor"], + "bloquear_avanco": risco["bloquear_avanco"], + "stop": risco["stop"], + "emergency_stop": risco["emergency_stop"], + + # Diagnóstico de fusão. + "confidence": round(float(fusao.get("confidence", 0.0)), 3), + "fonte_principal": fonte_principal, + "single_source": bool(fusao.get("single_source", False)), + + "sensors_count": len(fontes) + len(ignoradas), + "sensors_valid_count": len(fontes), + "sensors_ignored_count": len(ignoradas), + + "sensors_agree": divergencia["sensors_agree"], + "roll_disagreement_deg": round(divergencia["roll_disagreement_deg"], 3), + "pitch_disagreement_deg": round(divergencia["pitch_disagreement_deg"], 3), + "max_disagreement_deg": round(divergencia["max_disagreement_deg"], 3), + "divergentes": divergencia["divergentes"], + + "fontes": [ + { + "mx_id": f["mx_id"], + "modelo": f.get("modelo"), + "roll": round(f["roll"], 3), + "pitch": round(f["pitch"], 3), + "yaw": round(f["yaw"], 3), + "roll_rate_dps": round(f["roll_rate_dps"], 3), + "pitch_rate_dps": round(f["pitch_rate_dps"], 3), + "idade_s": round(f["idade_s"], 3), + "confidence": round(f["confidence"], 3), + "peso_base": round(f["peso_base"], 3), + } + for f in fontes + ], + "fontes_ignoradas": [ + { + "mx_id": f["mx_id"], + "modelo": f.get("modelo"), + "idade_s": round(f["idade_s"], 3), + "motivos": f.get("motivos", []), + } + for f in ignoradas + ], + + # Índices úteis para painel/manager. + "estabilidade_idx": round(estabilidade_score, 1), + "oscilacao_roll_std": round(oscilacao_roll, 3), + "oscilacao_pitch_std": round(oscilacao_pitch, 3), + "oscilacao_total_std": round(oscilacao_total, 3), + + "timestamp": agora * 1000.0, + "last_packet_ts": agora, + "last_publish_ts": agora, + "ultimo_erro_ts": self.ultimo_erro_ts, + "latencia": round(self.latencia, 6), + "frequencia": round(self.frequencia, 2), + } + + # ============================================================ + # Loop principal + # ============================================================ def imu_task_loop(self): - from camera_worker.manager import definir_imu_camera - t0 = time.perf_counter() + ultimo_publish = 0.0 + ultimo_loop = time.perf_counter() while self.ativo: - latencia = 0.0 + t0 = time.perf_counter() + try: - agora = time.perf_counter() - t_tick = agora - t0 - t0 = agora - f_tick = 1.0 / max(t_tick, 1e-6) + agora = time.time() - imuData = self.imu_queue.tryGet() + fontes, ignoradas = self._ler_fontes_imu() - 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 len(fontes) <= 0: + data = self._montar_saida_sem_imu(ignoradas, agora) + else: + fusao = self._fundir_fontes(fontes) + divergencia = self._calcular_divergencia(fontes) - if imuData is not None and len(imuData.packets) > 0: - self.last_packet_ts = time.time() - now = time.perf_counter() - if self.last_loop_ts is None: - dt_total = 1.0 / self.freq - else: - dt_total = max(1e-4, now - self.last_loop_ts) - self.last_loop_ts = now + risco = self._avaliar_risco( + roll=fusao["roll"], + pitch=fusao["pitch"], + roll_rate=fusao["roll_rate_dps"], + pitch_rate=fusao["pitch_rate_dps"], + sensores_agree=divergencia["sensors_agree"], + n_fontes=len(fontes), + ) - dt_total = min(dt_total, 0.05) - num_packets = max(1, len(imuData.packets)) - dt_packet = max(1e-4, dt_total / num_packets) + data = self._montar_saida( + fontes=fontes, + ignoradas=ignoradas, + fusao=fusao, + divergencia=divergencia, + risco=risco, + agora=agora, + ) - for packet in imuData.packets: - accel = packet.acceleroMeter - gyro = packet.gyroscope + t1 = time.perf_counter() + self.latencia = t1 - t0 - ax = float(accel.x) - ay = float(accel.y) - az = float(accel.z) + dt_loop = max(t1 - ultimo_loop, 1e-6) + self.frequencia = 1.0 / dt_loop + ultimo_loop = t1 - gx = float(np.deg2rad(gyro.x)) - gy = float(np.deg2rad(gyro.y)) - gz = float(np.deg2rad(gyro.z)) + self.last_data = data - a_body = np.array([ax, ay, az], dtype=np.float64) - acc_norm = np.linalg.norm(a_body) + if (t1 - ultimo_publish) >= self.publish_period: + self.last_publish_ts = time.time() + data["last_publish_ts"] = self.last_publish_ts + data["latencia"] = round(self.latencia, 6) + data["frequencia"] = round(self.frequencia, 2) - self.filtro_imu.Dt = float(dt_packet) + # Atualiza saúde junto do payload final. + saude = self.atualizar_saude(publicar=False) + data["saude"] = saude - # Confia no acelerômetro só quando ele parece representar gravidade - usa_acc = abs(acc_norm - self.g) < 0.20 + self._publicar(data) - if usa_acc: - self.q = self.filtro_imu.updateIMU( - q=self.q, - gyr=np.array([gx, gy, gz], dtype=np.float64), - acc=a_body - ) - else: - # fallback: mantém última orientação integrando com gyro "na mão" - # caso sua lib não aceite acc=None - omega = np.array([gx, gy, gz], dtype=np.float64) - omega_norm = np.linalg.norm(omega) - if omega_norm > 1e-9: - theta = omega_norm * dt_packet - axis = omega / omega_norm - dq_xyz = axis * np.sin(theta / 2.0) - dq_w = np.cos(theta / 2.0) - dq = np.array([dq_w, dq_xyz[0], dq_xyz[1], dq_xyz[2]], dtype=np.float64) - - w1, x1, y1, z1 = self.q - w2, x2, y2, z2 = dq - self.q = np.array([ - w1*w2 - x1*x2 - y1*y2 - z1*z2, - w1*x2 + x1*w2 + y1*z2 - z1*y2, - w1*y2 - x1*z2 + y1*w2 + z1*x2, - w1*z2 + x1*y2 - y1*x2 + z1*w2 - ], dtype=np.float64) - - self.q /= max(np.linalg.norm(self.q), 1e-9) - - r = R.from_quat([self.q[1], self.q[2], self.q[3], self.q[0]]) - roll, pitch, yaw = r.as_euler('xyz', degrees=True) - - roll -= self.roll_inicial - pitch -= self.pitch_inicial - yaw -= self.yaw_inicial - - # mata picos curtíssimos - roll = self._mediana_curta(self.roll_hist, roll) - pitch = self._mediana_curta(self.pitch_hist, pitch) - - # filtro leve para segurança - self.roll_seg = (1.0 - self.alpha_seg) * self.roll_seg + self.alpha_seg * roll - self.pitch_seg = (1.0 - self.alpha_seg) * self.pitch_seg + self.alpha_seg * pitch - - # velocidade linear - Rwb = r.as_matrix() - a_world = Rwb @ a_body - a_lin = a_world - np.array([0.0, 0.0, self.g], dtype=np.float64) - - dead = 0.08 - a_lin[np.abs(a_lin) < dead] = 0.0 - - parado = ( - np.linalg.norm([gx, gy, gz]) < np.deg2rad(1.5) and - abs(np.linalg.norm(a_body) - self.g) < 0.10 - ) - - if parado: - self.v_world *= 0.1 - else: - self.v_world += (a_lin * dt_packet) - - v_planar = self.v_world.copy() - v_planar[2] = 0.0 - velocidade_mps = float(np.linalg.norm(v_planar)) - - self._atualizar_indices( - velocidade_mps=velocidade_mps, - gx=gx, gy=gy, gz=gz, - a_lin=a_lin - ) - - t1 = time.perf_counter() - latencia = t1 - agora - - t_pub = time.perf_counter() - if (t_pub - self.last_redis_ts) >= self.redis_period: - self.last_publish_ts = time.time() - self.last_redis_ts = t_pub - - ts_imu = (time.time() * 1000) - - #ContextoGlobalRedis.atualizar_ctx_dict( - # ContextoGlobalRedis.ModKey(T_Code.Imu), - # roll=round(-roll, 2), - # pitch=round(pitch, 2), - # yaw=round(yaw, 2), - # roll_seg=round(-self.roll_seg, 2), - # pitch_seg=round(self.pitch_seg, 2), - # vel_mps=round(velocidade_mps, 3), - # em_movimento=self.em_movimento, - # movimento_idx=round(self.movimento_idx, 1), - # rugosidade_idx=round(self.rugosidade_idx, 1), - # impacto_idx=round(self.impacto_idx, 1), - # estabilidade_idx=round(self.estabilidade_idx, 1), - # timestamp=ts_imu, - # last_packet_ts=self.last_packet_ts, - # last_publish_ts=self.last_publish_ts, - # ultimo_erro_ts=self.ultimo_erro_ts, - # latencia=latencia, - # frequencia=f_tick - #) - - self.last_data = { - "roll": round(-roll, 2), - "pitch": round(pitch, 2), - "yaw": round(yaw, 2), - "roll_seg": round(-self.roll_seg, 2), - "pitch_seg": round(self.pitch_seg, 2), - "vel_mps": round(velocidade_mps, 3), - "em_movimento": self.em_movimento, - "movimento_idx": round(self.movimento_idx, 1), - "rugosidade_idx": round(self.rugosidade_idx, 1), - "impacto_idx": round(self.impacto_idx, 1), - "estabilidade_idx": round(self.estabilidade_idx, 1), - "timestamp": ts_imu, - "last_packet_ts": self.last_packet_ts, - "last_publish_ts": self.last_publish_ts, - "ultimo_erro_ts": self.ultimo_erro_ts, - "latencia": latencia, - "frequencia": f_tick - } - - definir_imu_camera(self.mx_id, self.last_data, self.ultima_saude) + ultimo_publish = t1 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.tempo_erro = time.time() - self.mostrar_log("Entrando em modo de reconexão lenta") - else: - self.ativo = False - finally: - if self.imu_em_falha: - time.sleep(5.0) - else: - delay_corrigido = max(0.0, (1.0 / self.freq) - latencia) - time.sleep(delay_corrigido) + self.mostrar_log(f"Erro no loop IMU final: {e}") - def atualizar_saude_bkp(self): + finally: + elapsed = time.perf_counter() - t0 + sleep_s = max(0.0, (1.0 / self.freq) - elapsed) + time.sleep(sleep_s) + + # ============================================================ + # Saúde + # ============================================================ + + def atualizar_saude(self, publicar=True): try: agora = time.time() - modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} - mod_ts = modulo.get("last_packet_ts") + data = self.get_dados() - FREQ_BASE = self.freq - FREQ_MIN = FREQ_BASE * 0.5 - SAUDE_MIN_ALERTA = 80 - SAUDE_MIN_FALHA = 45 + valido = bool(data.get("valido", False)) + sensors_valid_count = int(data.get("sensors_valid_count", 0) or 0) + sensors_count = int(data.get("sensors_count", 0) or 0) + sensors_agree = bool(data.get("sensors_agree", False)) + risk_level_num = int(data.get("risk_level_num", 4) or 0) - tempo_sem_dados = (time.time() - (mod_ts or 0)) - conectado = tempo_sem_dados < 10 + 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", self.frequencia) or 0.0) + latencia = float(data.get("latencia", self.latencia) or 0.0) - freq_hz = float(modulo.get("frequencia", 0.0) or 0.0) - latencia = float(modulo.get("latencia", 0.0) or 0.0) - - last_packet_ts = float(modulo.get("last_packet_ts", self.last_packet_ts) or 0.0) - last_publish_ts = float(modulo.get("last_publish_ts", self.last_publish_ts) or 0.0) - ultimo_erro_ts = float(modulo.get("ultimo_erro_ts", self.ultimo_erro_ts) or 0.0) - - 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 - saude = 100 - motivos = [] - - if not conectado: - motivos.append(f"Thread da IMU não está viva à {tempo_sem_dados:.2f} s") - saude = 0 - else: - # ============================ - # 1) Penalização por ausência de packets - # ============================ - if tempo_sem_packet > 1.5: - saude -= 45 - motivos.append(f"Sem packets há {tempo_sem_packet:.2f}s") - elif tempo_sem_packet > 0.7: - saude -= 25 - motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s") - elif tempo_sem_packet > 0.3: - saude -= 10 - motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s") - - # ============================ - # 2) Penalização por ausência de publicação - # ============================ - if tempo_sem_publicar > 1.5: - saude -= 30 - motivos.append(f"Sem publicar no Redis há {tempo_sem_publicar:.2f}s") - elif tempo_sem_publicar > 0.8: - saude -= 15 - motivos.append(f"Publicação atrasada há {tempo_sem_publicar:.2f}s") - elif tempo_sem_publicar > 0.3: - saude -= 5 - motivos.append(f"Leve atraso na publicação: {tempo_sem_publicar:.2f}s") - - # ============================ - # 3) Frequência - # ============================ - if freq_hz <= 0: - saude -= 20 - motivos.append("Frequência zerada") - elif freq_hz <= FREQ_MIN: - saude -= 35 - motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") - else: - f = min(freq_hz, FREQ_BASE) - erro = (FREQ_BASE - f) / FREQ_BASE - p = min(int(erro * 100), 20) - saude -= p - if p > 0: - motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz") - - # ============================ - # 4) Latência - # ============================ - if latencia > 1.0: - saude -= 25 - motivos.append(f"Latência muito alta: {latencia:.2f}s") - elif latencia > 0.5: - saude -= 15 - motivos.append(f"Latência alta: {latencia:.2f}s") - elif latencia > 0.2: - saude -= 5 - motivos.append(f"Latência moderada: {latencia:.2f}s") - - # ============================ - # 5) Erro recente - # ============================ - 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, saude)) - - status = StatusModulo.OPERANTE - if not conectado: - status = StatusModulo.DESCONECTADO - elif saude <= SAUDE_MIN_FALHA: - status = StatusModulo.FALHA - elif saude < SAUDE_MIN_ALERTA: - status = StatusModulo.ALERTA - - saude_geral = { - "conectado": conectado, - "status": status.value, - "saude": saude, - "motivos": motivos, - "saude_idividual": [], - } - - ContextoGlobalRedis.atualizar_ctx_dict( - ContextoGlobalRedis.ModKey(T_Code.Imu), - saude=saude_geral - ) - - except Exception as e: - self.mostrar_log(f"Erro ao atualizar saude: {e}") - - def atualizar_saude(self): - try: - agora = time.time() - - FREQ_BASE = float(self.freq) - FREQ_MIN = FREQ_BASE * 0.5 - SAUDE_MIN_ALERTA = 80 - SAUDE_MIN_FALHA = 45 - - 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)) - - tempo_sem_packet = (agora - last_packet_ts) if last_packet_ts > 0 else 999.0 - tempo_sem_atualizar = (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 < 10.0) + conectado = bool(self.ativo and tempo_sem_publicar < 2.0) saude = 100 motivos = [] @@ -535,79 +968,89 @@ class IMUCamera(ModuloDiagnosticoBase): if not self.ativo: conectado = False saude = 0 - motivos.append("Thread da IMU parada") + motivos.append("Task IMU final 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") + sensors_count = int(data.get("sensors_count", 0) or 0) + sensors_valid_count = int(data.get("sensors_valid_count", 0) or 0) + sensors_ignored_count = int(data.get("sensors_ignored_count", 0) or 0) + + if sensors_count <= 0: + motivos.append("Nenhuma câmera com IMU detectada") + elif sensors_valid_count <= 0 and sensors_ignored_count > 0: + motivos.append("Câmeras detectadas, mas nenhuma IMU válida") + else: + motivos.append("Nenhuma IMU válida disponível") else: - if tempo_sem_packet > 1.5: - saude -= 45 - motivos.append(f"Sem packets há {tempo_sem_packet:.2f}s") - elif tempo_sem_packet > 0.7: - saude -= 25 - motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s") - elif tempo_sem_packet > 0.3: - saude -= 10 - motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s") + if sensors_valid_count <= 0: + saude = 0 + motivos.append("Sem sensores IMU válidos") - if tempo_sem_atualizar > 1.5: + elif sensors_valid_count == 1: + saude -= 15 + motivos.append("Operando sem redundância de IMU") + + if sensors_count >= 2 and not sensors_agree: saude -= 30 - motivos.append(f"Sem atualizar dados há {tempo_sem_atualizar:.2f}s") - elif tempo_sem_atualizar > 0.8: - saude -= 15 - motivos.append(f"Atualização atrasada há {tempo_sem_atualizar:.2f}s") - elif tempo_sem_atualizar > 0.3: - saude -= 5 - motivos.append(f"Leve atraso na atualização: {tempo_sem_atualizar:.2f}s") + motivos.append("IMUs divergentes") - if freq_hz <= 0: - saude -= 20 - motivos.append("Frequência zerada") - elif freq_hz <= FREQ_MIN: - saude -= 35 - motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") - else: - f = min(freq_hz, FREQ_BASE) - erro = (FREQ_BASE - f) / FREQ_BASE - p = min(int(erro * 100), 20) - saude -= p - if p > 0: - motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz") - - if latencia > 1.0: + if risk_level_num >= 4: + saude -= 40 + motivos.append("Risco emergency ativo") + elif risk_level_num >= 3: saude -= 25 - motivos.append(f"Latência muito alta: {latencia:.2f}s") - elif latencia > 0.5: - saude -= 15 - motivos.append(f"Latência alta: {latencia:.2f}s") - elif latencia > 0.2: - saude -= 5 - motivos.append(f"Latência moderada: {latencia:.2f}s") + motivos.append("Risco critical ativo") + elif risk_level_num >= 2: + saude -= 10 + motivos.append("Risco elevado ativo") + + if tempo_sem_publicar > 1.0: + saude -= 30 + motivos.append(f"Sem publicar há {tempo_sem_publicar:.2f}s") + elif tempo_sem_publicar > 0.5: + saude -= 10 + motivos.append(f"Publicação atrasada: {tempo_sem_publicar:.2f}s") + + if freq_hz <= 5: + saude -= 25 + motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") + elif freq_hz <= 10: + saude -= 10 + motivos.append(f"Frequência moderada: {freq_hz:.2f} Hz") + + if latencia > 0.2: + saude -= 20 + motivos.append(f"Latência alta: {latencia:.3f}s") + elif latencia > 0.08: + saude -= 8 + motivos.append(f"Latência moderada: {latencia:.3f}s") if tempo_desde_erro < 2.0: saude -= 20 - motivos.append("Erro muito recente no loop") + motivos.append("Erro recente no loop IMU") elif tempo_desde_erro < 5.0: saude -= 10 - motivos.append("Erro recente no loop") + motivos.append("Erro recente no loop IMU") - saude = max(0, min(100, saude)) + saude = int(max(0, min(100, saude))) status = StatusModulo.OPERANTE if not conectado: status = StatusModulo.DESCONECTADO - elif saude <= SAUDE_MIN_FALHA: + elif not valido: status = StatusModulo.FALHA - elif saude < SAUDE_MIN_ALERTA: + elif saude <= 45: + status = StatusModulo.FALHA + elif saude < 80: status = StatusModulo.ALERTA + else: + status = StatusModulo.OPERANTE self.ultima_saude = { + "timestamp": agora, "conectado": conectado, "status": status.value, "saude": saude, @@ -615,45 +1058,74 @@ class IMUCamera(ModuloDiagnosticoBase): "saude_individual": [], } + if publicar: + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.ModKey(T_Code.Imu), + saude=self.ultima_saude, + ) + return self.ultima_saude except Exception as e: - self.mostrar_log(f"Erro ao atualizar saude: {e}") + self.mostrar_log(f"Erro ao atualizar saúde IMU final: {e}") + self.ultima_saude = { + "timestamp": time.time(), "conectado": False, "status": StatusModulo.FALHA.value, "saude": 0, "motivos": [str(e)], "saude_individual": [], } + + if publicar: + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.ModKey(T_Code.Imu), + saude=self.ultima_saude, + ) + return self.ultima_saude def get_dados(self): if self.last_data is None: return { + "valido": False, + "modo": "inicializando", "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, - "vel_mps": 0.0, - "em_movimento": False, - "movimento_idx": 0.0, - "rugosidade_idx": 0.0, - "impacto_idx": 0.0, - "estabilidade_idx": 100.0, + "roll_rate_dps": 0.0, + "pitch_rate_dps": 0.0, + "yaw_rate_dps": 0.0, + "risk_level": "unknown", + "risk_level_num": 4, + "risk_score": 100.0, + "acao_sugerida": "emergency_stop", + "velocidade_factor": 0.0, + "bloquear_avanco": True, + "stop": True, + "emergency_stop": True, + "confidence": 0.0, + "sensors_count": 0, + "sensors_valid_count": 0, + "sensors_ignored_count": 0, + "sensors_agree": False, + "estabilidade_idx": 0.0, "timestamp": 0.0, "last_packet_ts": 0.0, "last_publish_ts": 0.0, - "ultimo_erro_ts": 0.0, - "latencia": 0.0, - "frequencia": 0.0, - "valido": False + "ultimo_erro_ts": self.ultimo_erro_ts, + "latencia": self.latencia, + "frequencia": self.frequencia, + "saude": self.ultima_saude, } - data = dict(self.last_data) - data["valido"] = True - return data + return dict(self.last_data) def mostrar_log(self, mensagem): - print(f"{time.time()} - [IMU][id={id(self)}] {mensagem}") \ No newline at end of file + print(f"{time.time()} - [CameraIMUFinal][id={id(self)}] {mensagem}") \ No newline at end of file