""" MorphGuard Drone Connection Manager Unified connection management for multiple drone communication paths: - Pi WiFi/Ethernet (Companion Computer) - Pixracer USB (Direct serial) - 915MHz Radio (SiK telemetry) Features: - Simultaneous multi-path streaming - Automatic failover (<10 seconds) - Operator notifications - Connection logging """ import os import time import asyncio import logging import threading from enum import Enum from typing import Optional, Dict, Any, Callable, List from dataclasses import dataclass, field from datetime import datetime, timedelta logger = logging.getLogger("DroneConnectionManager") class PathType(Enum): """Connection path types.""" PI_WIFI = "pi_wifi" PI_ETHERNET = "pi_ethernet" USB = "usb" SERIAL = "serial" RADIO_915 = "radio_915" TCP = "tcp" UDP = "udp" class PathStatus(Enum): """Connection path status.""" DISCONNECTED = "disconnected" CONNECTING = "connecting" CONNECTED = "connected" DEGRADED = "degraded" FAILED = "failed" @dataclass class PathHealth: """Health metrics for a connection path.""" status: PathStatus = PathStatus.DISCONNECTED last_heartbeat: Optional[datetime] = None latency_ms: float = 0.0 packet_loss: float = 0.0 reconnect_count: int = 0 last_error: Optional[str] = None connected_at: Optional[datetime] = None @property def is_healthy(self) -> bool: """Check if path is healthy.""" if self.status != PathStatus.CONNECTED: return False if self.last_heartbeat: age = (datetime.now() - self.last_heartbeat).total_seconds() if age > 10.0: # 10 second timeout return False return True @property def uptime_seconds(self) -> float: """Get connection uptime in seconds.""" if self.connected_at: return (datetime.now() - self.connected_at).total_seconds() return 0.0 @dataclass class ConnectionPath: """Represents a single connection path.""" path_type: PathType uri: str baud: int = 57600 health: PathHealth = field(default_factory=PathHealth) connection: Any = None # MAVLink connection or WebSocket provides_telemetry: bool = True provides_video: bool = False priority: int = 0 # Higher = preferred @property def display_name(self) -> str: """Human-readable name for UI.""" names = { PathType.PI_WIFI: "📶 Pi WiFi", PathType.PI_ETHERNET: "🔗 Pi Ethernet", PathType.USB: "🔌 USB Direct", PathType.SERIAL: "📡 Serial UART", PathType.RADIO_915: "📻 915MHz Radio", PathType.TCP: "🌐 TCP", PathType.UDP: "🌐 UDP" } return names.get(self.path_type, str(self.path_type)) @dataclass class TelemetryData: """Unified telemetry from any source.""" timestamp: datetime source_path: PathType gps_lat: Optional[float] = None gps_lon: Optional[float] = None gps_alt: Optional[float] = None heading: Optional[float] = None roll: Optional[float] = None pitch: Optional[float] = None yaw: Optional[float] = None battery_voltage: Optional[float] = None battery_remaining: Optional[int] = None armed: bool = False flight_mode: str = "UNKNOWN" system_id: Optional[int] = None class ConnectionEvent: """Events for connection logging.""" def __init__(self, event_type: str, path: PathType, details: str = ""): self.timestamp = datetime.now() self.event_type = event_type # connected, disconnected, failover, error self.path = path self.details = details def to_dict(self) -> Dict: return { "timestamp": self.timestamp.isoformat(), "event_type": self.event_type, "path": self.path.value, "details": self.details } class DroneConnectionManager: """ Unified connection manager for drone communication. Supports simultaneous multi-path streaming with automatic failover. """ # Default log file path LOG_FILE_PATH = "/var/log/morphguard/drone_connections.log" def __init__(self, config: Dict[str, Any] = None): """ Initialize connection manager. Args: config: Configuration dictionary """ self.config = config or {} # Connection paths self.paths: Dict[PathType, ConnectionPath] = {} # Active connections (can have multiple for multi-path) self.active_paths: List[PathType] = [] # Primary path for failover self.primary_path: Optional[PathType] = None self.backup_path: Optional[PathType] = None # Failover settings self.failover_enabled = True self.failover_timeout_seconds = 10.0 # Callbacks self._on_telemetry: List[Callable[[TelemetryData], None]] = [] self._on_path_change: List[Callable[[PathType, PathStatus], None]] = [] self._on_failover: List[Callable[[PathType, PathType], None]] = [] self._on_frame: List[Callable[[Dict], None]] = [] # Connection event log (in-memory) self._event_log: List[ConnectionEvent] = [] self._max_log_size = 1000 # File-based logging self._log_file_path = self.config.get("log_file", self.LOG_FILE_PATH) self._file_logger = self._init_file_logger() # Background health monitor self._health_thread: Optional[threading.Thread] = None self._running = False # Latest telemetry from each path self._telemetry_cache: Dict[PathType, TelemetryData] = {} def _init_file_logger(self) -> Optional[logging.Logger]: """Initialize file-based connection logger.""" try: import json from logging.handlers import RotatingFileHandler # Ensure log directory exists log_dir = os.path.dirname(self._log_file_path) if log_dir and not os.path.exists(log_dir): try: os.makedirs(log_dir, mode=0o755, exist_ok=True) except PermissionError: # Fall back to /tmp if we can't write to /var/log self._log_file_path = "/tmp/morphguard_drone_connections.log" logger.warning(f"Cannot write to {log_dir}, using {self._log_file_path}") # Create dedicated connection log file_logger = logging.getLogger("DroneConnectionLog") file_logger.setLevel(logging.INFO) file_logger.propagate = False # Don't propagate to root logger # Remove existing handlers file_logger.handlers.clear() # Add rotating file handler (5MB max, keep 5 backups) handler = RotatingFileHandler( self._log_file_path, maxBytes=5 * 1024 * 1024, # 5MB backupCount=5 ) # Use JSON format for structured logging class JSONFormatter(logging.Formatter): def format(self, record): log_entry = { "timestamp": datetime.now().isoformat(), "event_type": getattr(record, 'event_type', 'unknown'), "path": getattr(record, 'path', 'unknown'), "details": record.getMessage(), "level": record.levelname } return json.dumps(log_entry) handler.setFormatter(JSONFormatter()) file_logger.addHandler(handler) logger.info(f"Connection logging initialized: {self._log_file_path}") return file_logger except Exception as e: logger.warning(f"Could not initialize file logging: {e}") return None # ------------------------------------------------------------------------- # Path Registration # ------------------------------------------------------------------------- def register_path( self, path_type: PathType, uri: str, baud: int = 57600, provides_video: bool = False, priority: int = 0 ) -> None: """ Register a connection path. Args: path_type: Type of connection path uri: Connection URI (e.g., "/dev/ttyACM0" or "ws://192.168.200.101:5000") baud: Baud rate for serial connections provides_video: Whether this path can provide video priority: Connection priority (higher = preferred) """ path = ConnectionPath( path_type=path_type, uri=uri, baud=baud, provides_video=provides_video, priority=priority ) self.paths[path_type] = path logger.info(f"Registered path: {path.display_name} ({uri})") def register_external_connection(self, path_type: PathType, connection: Any, remote_ip: str) -> bool: """ Register an incoming external connection (e.g. from Flask-Sock). Use this when the companion computer connects to us, rather than us connecting to it. """ if path_type not in self.paths: logger.warning(f"External connection for unknown path: {path_type}") return False path = self.paths[path_type] path.health.status = PathStatus.CONNECTED path.health.connected_at = datetime.now() path.health.last_heartbeat = datetime.now() path.connection = connection # Update URI to reflect actual connected IP if remote_ip: path.uri = f"ws://{remote_ip}" if path_type not in self.active_paths: self.active_paths.append(path_type) self._log_event("connected_external", path_type, f"From {remote_ip}") self._notify_path_change(path_type, PathStatus.CONNECTED) logger.info(f"✓ External connection registered: {path.display_name} from {remote_ip}") return True def get_registered_paths(self) -> List[Dict]: """Get list of registered paths with status.""" return [ { "type": p.path_type.value, "name": p.display_name, "uri": p.uri, "status": p.health.status.value, "healthy": p.health.is_healthy, "provides_video": p.provides_video, "priority": p.priority } for p in self.paths.values() ] # ------------------------------------------------------------------------- # Connection Management # ------------------------------------------------------------------------- def connect(self, path_type: PathType) -> bool: """ Connect a specific path. Args: path_type: Type of path to connect Returns: True if connection successful (or started successfully) """ if path_type not in self.paths: logger.error(f"Path not registered: {path_type}") return False path = self.paths[path_type] path.health.status = PathStatus.CONNECTING try: success = False if path_type in [PathType.PI_WIFI, PathType.PI_ETHERNET]: success = self._connect_companion(path) elif path_type in [PathType.USB, PathType.SERIAL]: success = self._connect_mavlink_serial(path) elif path_type in [PathType.RADIO_915, PathType.UDP]: success = self._connect_mavlink_udp(path) elif path_type == PathType.TCP: success = self._connect_mavlink_tcp(path) else: logger.error(f"Unknown path type: {path_type}") return False if success: # For sync connections, we might need to set status here if not set in _connect # But _connect usually starts a thread that manages status. # Just mark as CONNECTED for now, the loop will update or fail it. if path.health.status == PathStatus.CONNECTING: path.health.status = PathStatus.CONNECTED path.health.connected_at = datetime.now() path.health.last_heartbeat = datetime.now() if path_type not in self.active_paths: self.active_paths.append(path_type) self._log_event("connected", path_type) self._notify_path_change(path_type, PathStatus.CONNECTED) logger.info(f"✓ Connected: {path.display_name}") return True else: path.health.status = PathStatus.FAILED return False except Exception as e: logger.error(f"Connection failed for {path_type}: {e}") path.health.status = PathStatus.FAILED path.health.last_error = str(e) self._log_event("error", path_type, str(e)) return False def disconnect(self, path_type: PathType) -> None: """Disconnect a specific path.""" if path_type not in self.paths: return path = self.paths[path_type] try: if path.connection: if hasattr(path.connection, 'close'): path.connection.close() # For WebSocketApp and MAVLink path.connection = None except Exception as e: logger.error(f"Disconnect error: {e}") path.health.status = PathStatus.DISCONNECTED path.health.connected_at = None if path_type in self.active_paths: self.active_paths.remove(path_type) self._log_event("disconnected", path_type) self._notify_path_change(path_type, PathStatus.DISCONNECTED) logger.info(f"Disconnected: {path.display_name}") def disconnect_all(self) -> None: """Disconnect all paths.""" for path_type in list(self.active_paths): self.disconnect(path_type) # ------------------------------------------------------------------------- # Connection Implementations # ------------------------------------------------------------------------- def _connect_companion(self, path: ConnectionPath) -> bool: """Connect to companion computer via WebSocket (Sync + Thread).""" try: import websocket # Build WebSocket URL uri = path.uri if not uri.startswith("ws"): uri = f"ws://{uri}:5000/api/drone/feed" logger.info(f"Connecting to companion: {uri}") # Define callbacks for WebSocketApp def on_message(ws, message): self._handle_companion_message(path, message) def on_error(ws, error): logger.error(f"Companion WebSocket Error: {error}") # Don't change status here immediately, on_close handles it? # or maybe we should? def on_close(ws, close_status_code, close_msg): logger.info(f"Companion WebSocket Closed: {close_status_code} - {close_msg}") if path.health.status == PathStatus.CONNECTED: path.health.status = PathStatus.DISCONNECTED self._check_failover(path.path_type) self._notify_path_change(path.path_type, PathStatus.DISCONNECTED) def on_open(ws): logger.info("Companion WebSocket Opened") path.health.status = PathStatus.CONNECTED path.health.connected_at = datetime.now() path.health.last_heartbeat = datetime.now() self._notify_path_change(path.path_type, PathStatus.CONNECTED) # Create WebSocketApp ws = websocket.WebSocketApp( uri, on_open=on_open, on_message=on_message, on_error=on_error, on_close=on_close ) path.connection = ws path.provides_video = True # Run in a dedicated thread wst = threading.Thread(target=ws.run_forever, kwargs={'ping_interval': 20, 'ping_timeout': 10}) wst.daemon = True wst.start() return True except ImportError: logger.error("websocket-client not installed") return False except Exception as e: logger.error(f"Companion connection error: {e}") return False def _handle_companion_message(self, path: ConnectionPath, message: str) -> None: """Handle incoming message from companion.""" import json try: data = json.loads(message) path.health.last_heartbeat = datetime.now() if "crop_base64" in data: self._notify_frame(data) # Convert to unified telemetry telemetry = TelemetryData( timestamp=datetime.now(), source_path=path.path_type, gps_lat=data.get("gps_lat"), gps_lon=data.get("gps_lon"), gps_alt=data.get("gps_alt"), heading=data.get("heading"), roll=data.get("roll"), pitch=data.get("pitch"), yaw=data.get("yaw"), battery_voltage=data.get("batt_voltage"), armed=data.get("armed", False), flight_mode=data.get("flight_mode", "UNKNOWN") ) self._telemetry_cache[path.path_type] = telemetry self._notify_telemetry(telemetry) except json.JSONDecodeError: pass except Exception as e: logger.error(f"Message handling error: {e}") def _connect_mavlink_serial(self, path: ConnectionPath) -> bool: """Connect to Pixracer via USB/Serial.""" try: from pymavlink import mavutil logger.info(f"Connecting to MAVLink: {path.uri} @ {path.baud}") conn = mavutil.mavlink_connection(path.uri, baud=path.baud) # Wait for heartbeat msg = conn.wait_heartbeat(timeout=10) if not msg: logger.error("No heartbeat received") return False path.connection = conn # Request data streams conn.mav.request_data_stream_send( conn.target_system, conn.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1 ) # Start receive loop in background threading.Thread( target=self._mavlink_receive_loop, args=(path,), daemon=True ).start() return True except ImportError: logger.error("pymavlink not installed") return False except Exception as e: logger.error(f"MAVLink serial error: {e}") return False def _connect_mavlink_udp(self, path: ConnectionPath) -> bool: """Connect via UDP (radio link).""" try: from pymavlink import mavutil uri = path.uri if not uri.startswith("udp"): uri = f"udp:{uri}" logger.info(f"Connecting to MAVLink UDP: {uri}") conn = mavutil.mavlink_connection(uri) msg = conn.wait_heartbeat(timeout=10) if not msg: logger.error("No heartbeat received") return False path.connection = conn conn.mav.request_data_stream_send( conn.target_system, conn.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1 ) threading.Thread( target=self._mavlink_receive_loop, args=(path,), daemon=True ).start() return True except Exception as e: logger.error(f"MAVLink UDP error: {e}") return False def _connect_mavlink_tcp(self, path: ConnectionPath) -> bool: """Connect via TCP.""" try: from pymavlink import mavutil uri = path.uri if not uri.startswith("tcp"): uri = f"tcp:{uri}" logger.info(f"Connecting to MAVLink TCP: {uri}") conn = mavutil.mavlink_connection(uri) msg = conn.wait_heartbeat(timeout=10) if not msg: return False path.connection = conn threading.Thread( target=self._mavlink_receive_loop, args=(path,), daemon=True ).start() return True except Exception as e: logger.error(f"MAVLink TCP error: {e}") return False # ------------------------------------------------------------------------- # Receive Loops # ------------------------------------------------------------------------- # _companion_receive_loop REMOVED (replaced by _handle_companion_message) def _mavlink_receive_loop(self, path: ConnectionPath) -> None: """Receive data from MAVLink connection (runs in thread).""" conn = path.connection try: while path.health.status == PathStatus.CONNECTED: msg = conn.recv_match(blocking=True, timeout=1.0) if msg is None: continue path.health.last_heartbeat = datetime.now() msg_type = msg.get_type() # Build telemetry from various messages if msg_type == "GLOBAL_POSITION_INT": telemetry = TelemetryData( timestamp=datetime.now(), source_path=path.path_type, gps_lat=msg.lat / 1e7, gps_lon=msg.lon / 1e7, gps_alt=msg.alt / 1000, heading=msg.hdg / 100 ) self._telemetry_cache[path.path_type] = telemetry self._notify_telemetry(telemetry) elif msg_type == "HEARTBEAT": # Update armed status if path.path_type in self._telemetry_cache: self._telemetry_cache[path.path_type].armed = (msg.base_mode & 128) != 0 except Exception as e: logger.error(f"MAVLink receive error: {e}") finally: if path.health.status == PathStatus.CONNECTED: path.health.status = PathStatus.FAILED self._check_failover(path.path_type) # ------------------------------------------------------------------------- # Health Monitoring & Failover # ------------------------------------------------------------------------- def start_health_monitor(self) -> None: """Start background health monitoring thread.""" if self._running: return self._running = True self._health_thread = threading.Thread(target=self._health_monitor_loop, daemon=True) self._health_thread.start() logger.info("Health monitor started") def stop_health_monitor(self) -> None: """Stop health monitoring.""" self._running = False if self._health_thread: self._health_thread.join(timeout=2.0) def _health_monitor_loop(self) -> None: """Background health check loop.""" while self._running: for path_type, path in self.paths.items(): if path.health.status == PathStatus.CONNECTED: if not path.health.is_healthy: logger.warning(f"Path unhealthy: {path.display_name}") path.health.status = PathStatus.DEGRADED self._check_failover(path_type) time.sleep(1.0) def _check_failover(self, failed_path: PathType) -> None: """Check and execute failover if needed.""" if not self.failover_enabled: return if failed_path != self.primary_path: return # Not primary, no failover needed if not self.backup_path: logger.warning("No backup path configured") return if self.backup_path not in self.paths: return backup = self.paths[self.backup_path] if backup.health.status == PathStatus.CONNECTED and backup.health.is_healthy: logger.warning(f"FAILOVER: {failed_path.value} -> {self.backup_path.value}") # Swap primary and backup old_primary = self.primary_path self.primary_path = self.backup_path self.backup_path = old_primary self._log_event("failover", self.primary_path, f"From {old_primary.value}") self._notify_failover(old_primary, self.primary_path) # ------------------------------------------------------------------------- # Callbacks & Notifications # ------------------------------------------------------------------------- def on_telemetry(self, callback: Callable[[TelemetryData], None]) -> None: """Register telemetry callback.""" self._on_telemetry.append(callback) def on_path_change(self, callback: Callable[[PathType, PathStatus], None]) -> None: """Register path status change callback.""" self._on_path_change.append(callback) def on_failover(self, callback: Callable[[PathType, PathType], None]) -> None: """Register failover callback.""" self._on_failover.append(callback) def on_frame(self, callback: Callable[[Dict], None]) -> None: """Register frame data callback.""" self._on_frame.append(callback) def _notify_frame(self, data: Dict) -> None: for cb in self._on_frame: try: cb(data) except Exception as e: logger.error(f"Frame callback error: {e}") def _notify_telemetry(self, telemetry: TelemetryData) -> None: for cb in self._on_telemetry: try: cb(telemetry) except Exception as e: logger.error(f"Telemetry callback error: {e}") def _notify_path_change(self, path: PathType, status: PathStatus) -> None: for cb in self._on_path_change: try: cb(path, status) except Exception as e: logger.error(f"Path change callback error: {e}") def _notify_failover(self, old_path: PathType, new_path: PathType) -> None: for cb in self._on_failover: try: cb(old_path, new_path) except Exception as e: logger.error(f"Failover callback error: {e}") # ------------------------------------------------------------------------- # Logging # ------------------------------------------------------------------------- def _log_event(self, event_type: str, path: PathType, details: str = "") -> None: """Log a connection event to both memory and file.""" event = ConnectionEvent(event_type, path, details) self._event_log.append(event) # Trim in-memory log if too large while len(self._event_log) > self._max_log_size: self._event_log.pop(0) # Log to console logger.info(f"Event: {event_type} - {path.value} {details}") # Log to file for post-flight analysis if self._file_logger: try: # Add extra attributes for JSON formatter extra = { 'event_type': event_type, 'path': path.value } self._file_logger.info(details or event_type, extra=extra) except Exception as e: logger.debug(f"File logging error: {e}") def get_event_log(self, limit: int = 100) -> List[Dict]: """Get connection event log.""" return [e.to_dict() for e in self._event_log[-limit:]] # ------------------------------------------------------------------------- # Status & Telemetry # ------------------------------------------------------------------------- def get_status(self) -> Dict: """Get current connection status.""" return { "active_paths": [p.value for p in self.active_paths], "primary_path": self.primary_path.value if self.primary_path else None, "backup_path": self.backup_path.value if self.backup_path else None, "failover_enabled": self.failover_enabled, "paths": { p.path_type.value: { "status": p.health.status.value, "healthy": p.health.is_healthy, "latency_ms": p.health.latency_ms, "uptime": p.health.uptime_seconds, "provides_video": p.provides_video } for p in self.paths.values() } } def get_latest_telemetry(self) -> Optional[TelemetryData]: """Get latest telemetry from primary path or any available.""" if self.primary_path and self.primary_path in self._telemetry_cache: return self._telemetry_cache[self.primary_path] # Fallback to any available for path_type in self.active_paths: if path_type in self._telemetry_cache: return self._telemetry_cache[path_type] return None def get_video_source(self) -> Optional[PathType]: """Get path that provides video, if any.""" for path_type in self.active_paths: if path_type in self.paths and self.paths[path_type].provides_video: return path_type return None