import json import math import socket import struct import time from config import * from helpers import clamp UNIT_CODES = {"norm": 0, "px": 1, "deg": 2, "m": 3} GUIDANCE_V1_REQUEST = 1 GUIDANCE_V1_RESPONSE = 2 GUIDANCE_V1_STRUCT = struct.Struct(" 0.0 x_m = math.tan(x_rad) * distance_m if meter_valid else 0.0 y_m = math.tan(y_rad) * distance_m if meter_valid else 0.0 det_count = max(0, int(state.get("det_count") or 0)) active = bool(state.get("active", False)) if not active: target_state = 0 elif det_count > 1: target_state = 2 elif det_count == 1: target_state = 1 else: target_state = 3 box_w = max(0.0, float(state.get("box_w") or 0.0)) box_h = max(0.0, float(state.get("box_h") or 0.0)) box_area_percent = int(clamp(round(100.0 * box_w * box_h / (frame_w * frame_h)), 0, 100)) units = str(units or "px").lower() if units == "norm": x, y, valid = x_norm, y_norm, True elif units == "deg": x, y, valid = x_deg, y_deg, True elif units == "m": x, y, valid = x_m, y_m, meter_valid else: units = "px" x, y, valid = x_px, y_px, True return { "type": "fpv_error", "timestamp": time.time() if timestamp is None else float(timestamp), "frame_id": int(state.get("frame_id") or 0), "active": active, "status": str(state.get("status") or "SEARCH"), "target_id": state.get("target_id"), "object_id": int(clamp(int(object_id), 1, 255)), "target_state": target_state, "det_count": det_count, "box_area_percent": box_area_percent, "confidence": float(state.get("confidence") or 0.0), "unit": units, "valid": bool(valid), "x": float(x), "y": float(y), "mag": float(math.hypot(float(x), float(y))), "x_norm": float(x_norm), "y_norm": float(y_norm), "x_px": float(x_px), "y_px": float(y_px), "x_deg": float(x_deg), "y_deg": float(y_deg), "x_m": float(x_m), "y_m": float(y_m), "range_m": distance_m if meter_valid else None, "frame_w": int(frame_w), "frame_h": int(frame_h), } def encode_error_payload(payload, protocol): protocol = str(protocol or "json").lower() if protocol == "guidance_v1": if int(payload["target_state"]) == 0: vertical_px = horizontal_px = vertical_percent = horizontal_percent = box_percent = 0 else: vertical_px = int(clamp(round(-float(payload["y_px"])), -32768, 32767)) horizontal_px = int(clamp(round(float(payload["x_px"])), -32768, 32767)) vertical_percent = int(clamp(round(-100.0 * float(payload["y_norm"])), -100, 100)) horizontal_percent = int(clamp(round(100.0 * float(payload["x_norm"])), -100, 100)) box_percent = int(clamp(int(payload["box_area_percent"]), 0, 100)) return GUIDANCE_V1_STRUCT.pack( GUIDANCE_V1_REQUEST, int(payload["object_id"]), int(payload["target_state"]), vertical_px, horizontal_px, vertical_percent, horizontal_percent, box_percent, ) if protocol == "csv": values = [ payload["frame_id"], f"{payload['timestamp']:.6f}", int(payload["active"]), int(payload["valid"]), payload["unit"], f"{payload['x']:.6f}", f"{payload['y']:.6f}", f"{payload['mag']:.6f}", f"{payload['confidence']:.6f}", payload["status"], "" if payload["target_id"] is None else payload["target_id"], ] return (",".join(map(str, values)) + "\n").encode("ascii", errors="replace") if protocol == "bin": target_id = int(payload["target_id"] or 0) return struct.pack( "<4sIdBBffffi", b"FPVE", int(payload["frame_id"]), float(payload["timestamp"]), 1 if payload["active"] else 0, UNIT_CODES.get(payload["unit"], 1), float(payload["x"]), float(payload["y"]), float(payload["mag"]), float(payload["confidence"]), target_id, ) return json.dumps(payload, ensure_ascii=True, separators=(",", ":")).encode("utf-8") def decode_guidance_v1_response(data): if len(data) != 2 or data[0] != GUIDANCE_V1_RESPONSE or data[1] not in (0, 1, 2): return None return {"descriptor": data[0], "response": data[1]} class ErrorOutputSender: def __init__(self): self.enabled = bool(ERROR_OUTPUT_ENABLE) self.protocol = str(ERROR_OUTPUT_PROTOCOL).lower().strip() self.units = str(ERROR_OUTPUT_UNITS).lower().strip() self.host = str(ERROR_OUTPUT_HOST) self.port = int(ERROR_OUTPUT_PORT) self.every = max(1, int(ERROR_OUTPUT_EVERY)) self.hfov_deg = float(ERROR_OUTPUT_HFOV_DEG) self.vfov_deg = float(ERROR_OUTPUT_VFOV_DEG) self.range_m = float(ERROR_OUTPUT_RANGE_M) self.object_id = int(clamp(int(ERROR_OUTPUT_OBJECT_ID), 1, 255)) self._sock = None self._last_response = None def start(self): if not self.enabled: return try: self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self._sock.setblocking(False) except OSError as exc: self._sock = None print(f"[error-output] socket failed: {exc}", flush=True) def close(self): if self._sock is not None: try: self._sock.close() except OSError: pass self._sock = None def status_line(self): if not self.enabled: return "Error output disabled" object_text = f" object={self.object_id}" if self.protocol == "guidance_v1" else "" return f"Error output UDP {self.protocol}: {self.host}:{self.port} units={self.units}{object_text}" def _poll_response(self): if self.protocol != "guidance_v1" or self._sock is None: return while True: try: data, _address = self._sock.recvfrom(64) except BlockingIOError: return except OSError: return response = decode_guidance_v1_response(data) if response is not None and response["response"] != self._last_response: self._last_response = response["response"] print(f"[error-output] guidance_v1 response={self._last_response}", flush=True) def send(self, state): if (not self.enabled) or self._sock is None or (not state.get("active", False)): return None frame_id = int(state.get("frame_id") or 0) if frame_id % self.every != 0: return None payload = build_error_payload( state, units=self.units, hfov_deg=self.hfov_deg, vfov_deg=self.vfov_deg, range_m=self.range_m, object_id=self.object_id, ) data = encode_error_payload(payload, self.protocol) try: self._sock.sendto(data, (self.host, self.port)) except OSError: pass self._poll_response() return payload