diff --git a/SUB Control/app.py b/SUB Control/app.py index e4664ac..fe6b5de 100644 --- a/SUB Control/app.py +++ b/SUB Control/app.py @@ -42,6 +42,25 @@ class Axes: yaw: float = 0.0 +def normalize_endpoint(port: str) -> str: + p = str(port).strip() + if not p: + return p + lower = p.lower() + if "sim" in lower or "5760" in lower or lower in ("tcp", "tcp:", "tcp:127.0.0.1", "127.0.0.1"): + return "tcp:127.0.0.1:5760" + if "14550" in lower or lower in ("udp", "udp:", "udpin", "udpin:"): + return "udpin:127.0.0.1:14550" + if p.startswith("127.0.0.1:") or p.startswith("localhost:"): + num = p.split(":")[1] + if num == "14550": + return f"udpin:{p}" + return f"tcp:{p}" + if ":" in p and not p.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:")): + return f"tcp:{p}" + return p + + class MavlinkWorker: def __init__(self, events: queue.Queue): self.events = events @@ -74,29 +93,52 @@ class MavlinkWorker: self.log_path = None def connect(self, port: str, baud: int): + print(f"[MavlinkWorker] connect() called: port={port!r}, baud={baud}") if self.thread and self.thread.is_alive(): + print(f"[MavlinkWorker] Thread already alive, ignoring connect()") return self.stop_event.clear() log_dir = Path(__file__).resolve().parent / "logs" log_dir.mkdir(exist_ok=True) self.log_path = log_dir / f"ardusub_{datetime.now():%Y%m%d_%H%M%S}.log" self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1) - self.log("SESSION", f"start port={port} baud={baud} control_hz={self.rate_hz}") + normalized = normalize_endpoint(port) + print(f"[MavlinkWorker] normalized endpoint: {normalized!r}") + self.log("SESSION", f"start port={normalized} (input={port}) baud={baud} control_hz={self.rate_hz}") self.events.put(("log_path", str(self.log_path))) self.thread = threading.Thread( - target=self._run, args=(port, baud), daemon=True + target=self._run, args=(normalized, baud), daemon=True ) self.thread.start() + print(f"[MavlinkWorker] worker thread started") def _run(self, port: str, baud: int): + print(f"[MavlinkWorker._run] ENTERED: port={port!r}, baud={baud}") try: + print(f"[MavlinkWorker._run] Creating mavlink_connection...") conn = mavutil.mavlink_connection( port, baud=baud, source_system=255, source_component=190, - autoreconnect=False, force_connected=True, + # Fail a missing TCP simulator immediately. pymavlink otherwise + # retries the refused socket several times and leaves the UI in + # "Opening" long after the actual failure. + autoreconnect=False, retries=0, force_connected=True, ) + print(f"[MavlinkWorker._run] mavlink_connection created: {type(conn).__name__}") self.connection = conn - self.events.put(("status", f"Port open; waiting for heartbeat at {baud} baud…")) - next_gcs_hb = 0.0 + if port.startswith("tcp:"): + self.events.put(("status", f"Connected to {port}; waiting for vehicle heartbeat…")) + else: + self.events.put(("status", f"Port {port} open; waiting for heartbeat at {baud} baud…")) + + # Send initial GCS heartbeat immediately + self.log("TX_HEARTBEAT", "GCS initial") + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, 0, mavutil.mavlink.MAV_STATE_ACTIVE, + ) + print(f"[MavlinkWorker._run] Initial heartbeat sent, entering recv loop") + next_gcs_hb = time.monotonic() + 1.0 next_control = 0.0 while not self.stop_event.is_set(): now = time.monotonic() @@ -120,6 +162,7 @@ class MavlinkWorker: else: time.sleep(0.01) except Exception as exc: + print(f"[MavlinkWorker._run] EXCEPTION: {exc}") self.events.put(("error", f"Connection error: {exc}")) finally: self._safe_stop() @@ -156,7 +199,7 @@ class MavlinkWorker: except Exception: payload = str(msg) self.log("RX", payload) - if msg_type == "HEARTBEAT" and msg.get_srcComponent() == 1: + if msg_type == "HEARTBEAT" and getattr(msg, "type", 0) != mavutil.mavlink.MAV_TYPE_GCS: first = not self.target_system self.target_system = msg.get_srcSystem() self.target_component = msg.get_srcComponent() @@ -887,6 +930,9 @@ class App(tk.Tk): def refresh_ports(self): ports = [p.device for p in list_ports.comports()] + for sim in ("tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"): + if sim not in ports: + ports.append(sim) self.port.configure(values=ports) if ports and self.port.get() not in ports: self.port.set(ports[0]) diff --git a/SUB Control/fc_spoofer.py b/SUB Control/fc_spoofer.py new file mode 100644 index 0000000..250d6ec --- /dev/null +++ b/SUB Control/fc_spoofer.py @@ -0,0 +1,553 @@ +"""ArduSub Flight Controller (FC) Spoofer & Simulator. + +Emulates an ArduSub SimpleROV-3 vehicle over MAVLink (TCP, UDP, or Serial). +Supports full parameter scans, live parameter updates (servo functions / motor reversal), +arming/disarming commands, manual control processing, and realistic telemetry streaming +(battery voltage/current with load sag, live thruster PWM mixing, simulated attitude & depth). +""" + +from __future__ import annotations + +import argparse +import math +import os +import sys +import time +from collections import deque +from dataclasses import dataclass +from typing import Any + +from pymavlink import mavutil + + +# Default SimpleROV-3 motor mappings and ArduSub parameters +DEFAULT_PARAMETERS: dict[str, tuple[float, int]] = { + # Servo functions: 33 = Motor 1 (Port), 34 = Motor 2 (Starboard), 35 = Motor 3 (Vertical) + "SERVO1_FUNCTION": (33.0, mavutil.mavlink.MAV_PARAM_TYPE_INT16), + "SERVO2_FUNCTION": (34.0, mavutil.mavlink.MAV_PARAM_TYPE_INT16), + "SERVO3_FUNCTION": (35.0, mavutil.mavlink.MAV_PARAM_TYPE_INT16), + # Remaining servos unassigned + **{f"SERVO{i}_FUNCTION": (0.0, mavutil.mavlink.MAV_PARAM_TYPE_INT16) for i in range(4, 17)}, + # Motor directions: 1 = normal, -1 = reversed + **{f"MOT_{i}_DIRECTION": (1.0, mavutil.mavlink.MAV_PARAM_TYPE_INT8) for i in range(1, 13)}, + # SimpleROV-3 thruster PWM limits + "SERVO1_MIN": (1100.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO1_TRIM": (1500.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO1_MAX": (1900.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO2_MIN": (1100.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO2_TRIM": (1500.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO2_MAX": (1900.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO3_MIN": (1100.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO3_TRIM": (1500.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "SERVO3_MAX": (1900.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + # Joystick gain parameters + "JS_GAIN_MIN": (0.10, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "JS_GAIN_DEFAULT": (0.20, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), + "JS_GAIN_MAX": (1.00, mavutil.mavlink.MAV_PARAM_TYPE_REAL32), +} + + +@dataclass +class ControlState: + x: float = 0.0 # Surge (forward/back) -1000..1000 + y: float = 0.0 # Sway (lateral) -1000..1000 + z: float = 500.0 # Heave (vertical) 0..1000 (500 = neutral) + r: float = 0.0 # Yaw (turn) -1000..1000 + last_rx: float = 0.0 + + +class FlightControllerSpoofer: + def __init__( + self, + endpoint: str = "tcpin:127.0.0.1:5760", + baud: int = 57600, + battery_cells: int = 4, + nominal_voltage: float = 16.0, + show_dashboard: bool = True, + ): + self.endpoint = endpoint + self.baud = baud + self.battery_cells = battery_cells + self.battery_base_v = nominal_voltage + self.show_dashboard = show_dashboard + + self.running = True + self.armed = False + self.mode = 19 # ArduSub MANUAL mode + self.system_status = mavutil.mavlink.MAV_STATE_ACTIVE + self.target_system = 255 + self.target_component = 190 + + self.params = dict(DEFAULT_PARAMETERS) + self.controls = ControlState() + + # Simulated physical state + self.depth_m = 1.5 # Positive depth under water + self.heading_deg = 0.0 + self.pitch_deg = 0.0 + self.roll_deg = 0.0 + self.m1_pwm = 1500 # Port + self.m2_pwm = 1500 # Starboard + self.m3_pwm = 1500 # Vertical + self.current_a = 0.35 + + self.events: deque[str] = deque(maxlen=6) + self.client_connected = False + self.total_packets_rx = 0 + self.total_packets_tx = 0 + self.start_time = time.monotonic() + + def log_event(self, text: str): + now_str = time.strftime("%H:%M:%S") + self.events.append(f"[{now_str}] {text}") + + def run(self): + print(f"\n{'='*72}") + print(f" ARDUSUB SIMPLE-ROV FLIGHT CONTROLLER SPOOFER IS RUNNING") + print(f" Target Endpoint: {self.endpoint}") + print(f" --> In Sub Control app, select or connect to: {self.endpoint.replace('tcpin:', 'tcp:')}") + print(f"{'='*72}\n", flush=True) + self.log_event(f"Simulator starting on {self.endpoint}...") + while self.running: + try: + self._run_server_session() + except KeyboardInterrupt: + break + except OSError as exc: + if getattr(exc, "winerror", None) == 10048 or "already in use" in str(exc).lower(): + print(f"\n[ERROR] Port {self.endpoint} is already in use by another program or previous spoofer instance.") + print("Please close any running python/spoofer processes and try again.\n") + break + self.log_event(f"Session reset: {exc}") + time.sleep(1.0) + except Exception as exc: + self.log_event(f"Session reset: {exc}") + time.sleep(1.0) + print("\nSpoofer stopped.") + + def _run_server_session(self): + # Bind server / open connection + if self.endpoint.startswith("tcpin:"): + shown_endpoint = self.endpoint.replace("tcpin:", "tcp:", 1) + print(f"[READY] Listening for SUB Control on {shown_endpoint}", flush=True) + print(" Leave this window open, then click Connect in the app.\n", flush=True) + conn = mavutil.mavlink_connection( + self.endpoint, + baud=self.baud, + source_system=1, + source_component=1, + autoreconnect=True, + force_connected=True, + ) + + self.log_event(f"Client connected on {self.endpoint}") + + next_heartbeat = 0.0 + next_sys_status = 0.0 + next_servos = 0.0 + next_attitude = 0.0 + next_dashboard = 0.0 + + # Send initial announcement statustext + initial_sent = False + + while self.running: + now = time.monotonic() + elapsed = now - self.start_time + time_boot_ms = int(elapsed * 1000) & 0xFFFFFFFF + time_boot_us = int(elapsed * 1000000) & 0xFFFFFFFF + + # 1. Heartbeat at 1.0 Hz + if now >= next_heartbeat: + base_mode = ( + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED + | mavutil.mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | mavutil.mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED + ) + if self.armed: + base_mode |= mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_SUBMARINE, + mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + self.mode, + self.system_status, + ) + self.total_packets_tx += 1 + next_heartbeat = now + 1.0 + + if not initial_sent and self.client_connected: + conn.mav.statustext_send( + mavutil.mavlink.MAV_SEVERITY_INFO, + b"ArduSub V4.1.0 (SimpleROV-3 Simulator)", + ) + conn.mav.statustext_send( + mavutil.mavlink.MAV_SEVERITY_INFO, + b"EKF3 IMU0 initialised", + ) + initial_sent = True + + # 2. Physics & Motor Mixing step + self._update_physics(now) + + # 3. SYS_STATUS at 1.0 Hz (Battery & Sensor Health) + if now >= next_sys_status: + # Voltage with slight noise + load sag + load_factor = ( + abs(self.m1_pwm - 1500) + + abs(self.m2_pwm - 1500) + + abs(self.m3_pwm - 1500) + ) / 1200.0 # 0.0 to 1.0 + voltage_v = self.battery_base_v - (0.45 * load_factor) + (0.02 * math.sin(now * 1.5)) + voltage_mv = int(max(10.0, voltage_v) * 1000) + + # Current: 0.35A base + up to 12.5A at full throttle + self.current_a = 0.35 + (12.5 * load_factor) + current_ca = int(self.current_a * 100) + + # Battery remaining percentage (e.g. 88% draining slowly) + remaining_pct = max(10, int(88 - (elapsed / 300.0))) + + conn.mav.sys_status_send( + 322042887, # onboard_control_sensors_present + 320969735, # onboard_control_sensors_enabled + 305241095, # onboard_control_sensors_health + 175, # load (17.5%) + voltage_mv, + current_ca, + remaining_pct, + 0, 0, 0, 0, 0, 0, + ) + self.total_packets_tx += 1 + next_sys_status = now + 1.0 + + # 4. SERVO_OUTPUT_RAW & RC_CHANNELS at 4.0 Hz + if now >= next_servos: + conn.mav.servo_output_raw_send( + time_boot_us, + 0, + self.m1_pwm, + self.m2_pwm, + self.m3_pwm, + 0, 0, 0, 0, 0, + ) + conn.mav.rc_channels_send( + time_boot_ms, + 8, + 1500 + int(self.controls.r * 0.4), # Chan 1: Yaw + 1500, # Chan 2: Roll + int(self.controls.z), # Chan 3: Throttle/Vertical + 1500 + int(self.controls.x * 0.4), # Chan 4: Pitch/Forward + 1500, 1500, 1500, 1500, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ) + self.total_packets_tx += 2 + next_servos = now + 0.25 + + # 5. ATTITUDE & VFR_HUD at 4.0 Hz + if now >= next_attitude: + conn.mav.attitude_send( + time_boot_ms, + math.radians(self.roll_deg), + math.radians(self.pitch_deg), + math.radians(self.heading_deg), + 0.0, 0.0, 0.0, + ) + conn.mav.vfr_hud_send( + abs(self.m1_pwm - 1500) / 400.0, # airspeed + abs(self.m2_pwm - 1500) / 400.0, # groundspeed + int(self.heading_deg) % 360, # heading + int((self.controls.z - 500) / 5), # throttle % + -self.depth_m, # altitude (negative depth) + (self.controls.z - 500) / 500.0, # climb rate + ) + self.total_packets_tx += 2 + next_attitude = now + 0.25 + + # 6. Process incoming MAVLink packets from App + self._handle_incoming_messages(conn) + + # 7. Render Console Dashboard + if self.show_dashboard and now >= next_dashboard: + self._render_dashboard() + next_dashboard = now + 0.15 + + time.sleep(0.01) + + def _update_physics(self, now: float): + # If manual control packet hasn't arrived for > 1.5s, auto-neutralize + if now - self.controls.last_rx > 1.5: + self.controls.x = 0.0 + self.controls.y = 0.0 + self.controls.z = 500.0 + self.controls.r = 0.0 + + if not self.armed: + self.m1_pwm = 1500 + self.m2_pwm = 1500 + self.m3_pwm = 1500 + else: + # Read motor direction parameters + mot1_dir = int(self.params.get("MOT_1_DIRECTION", (1.0,))[0]) + mot2_dir = int(self.params.get("MOT_2_DIRECTION", (1.0,))[0]) + mot3_dir = int(self.params.get("MOT_3_DIRECTION", (1.0,))[0]) + + # Differential drive mixing for SimpleROV-3: + # Motor 1 (Port): Forward + Yaw + # Motor 2 (Starboard): Forward - Yaw + # Motor 3 (Vertical): Vertical (0..1000 with 500 center) + forward_cmd = (self.controls.x / 1000.0) * 400.0 # -400..+400 + yaw_cmd = (self.controls.r / 1000.0) * 400.0 # -400..+400 + vert_cmd = ((self.controls.z - 500.0) / 500.0) * 400.0 # -400..+400 + + m1_offset = (forward_cmd + yaw_cmd) * mot1_dir + m2_offset = (forward_cmd - yaw_cmd) * mot2_dir + m3_offset = vert_cmd * mot3_dir + + self.m1_pwm = int(max(1100, min(1900, 1500 + m1_offset))) + self.m2_pwm = int(max(1100, min(1900, 1500 + m2_offset))) + self.m3_pwm = int(max(1100, min(1900, 1500 + m3_offset))) + + # Simulate subtle underwater motion + self.roll_deg = 2.5 * math.sin(now * 0.8) + self.pitch_deg = (self.controls.x / 1000.0) * 5.0 + 1.2 * math.cos(now * 0.7) + if self.armed and abs(self.controls.r) > 10: + self.heading_deg = (self.heading_deg + (self.controls.r / 1000.0) * 2.0) % 360.0 + + if self.armed and abs(self.controls.z - 500) > 20: + vert_delta = ((self.controls.z - 500.0) / 500.0) * 0.02 + self.depth_m = max(0.1, min(50.0, self.depth_m - vert_delta)) + + def _handle_incoming_messages(self, conn): + while True: + msg = conn.recv_match(blocking=False) + if not msg: + break + + self.total_packets_rx += 1 + if not self.client_connected: + self.client_connected = True + self.log_event(f"GCS Connected (SysID {msg.get_srcSystem()}, CompID {msg.get_srcComponent()})") + # Immediately send heartbeat so GCS locks on with zero delay + base_mode = ( + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED + | mavutil.mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | mavutil.mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED + ) + if self.armed: + base_mode |= mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_SUBMARINE, + mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + self.mode, + self.system_status, + ) + + mtype = msg.get_type() + if mtype == "HEARTBEAT": + self.target_system = msg.get_srcSystem() + self.target_component = msg.get_srcComponent() + + elif mtype == "COMMAND_LONG": + cmd = int(msg.command) + if cmd == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + should_arm = bool(msg.param1 == 1.0) + self.armed = should_arm + conn.mav.command_ack_send(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED) + status_str = "Motors ARMED" if self.armed else "Motors DISARMED" + self.log_event(f"Command: {status_str}") + conn.mav.statustext_send( + mavutil.mavlink.MAV_SEVERITY_INFO, + status_str.encode("ascii"), + ) + + elif cmd == mavutil.mavlink.MAV_CMD_DO_SET_MODE: + self.mode = int(msg.param2) + conn.mav.command_ack_send(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.log_event(f"Command: SET_MODE {self.mode}") + conn.mav.statustext_send( + mavutil.mavlink.MAV_SEVERITY_INFO, + b"Mode: MANUAL", + ) + + elif cmd == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL: + msg_id = int(msg.param1) + conn.mav.command_ack_send(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.log_event(f"Stream interval set for MsgID {msg_id}") + + else: + conn.mav.command_ack_send(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED) + + elif mtype == "SET_MODE": + self.mode = int(msg.custom_mode) + self.log_event(f"Set custom mode -> {self.mode}") + conn.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_INFO, b"Mode: MANUAL") + + elif mtype == "REQUEST_DATA_STREAM": + pass # Acknowledged implicitly by sending streams + + elif mtype == "PARAM_REQUEST_READ": + param_id = msg.param_id + if isinstance(param_id, bytes): + param_id = param_id.decode("ascii", errors="ignore").rstrip("\x00") + if param_id in self.params: + val, ptype = self.params[param_id] + else: + val, ptype = (0.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32) + conn.mav.param_value_send( + param_id.encode("ascii"), + float(val), + ptype, + len(self.params), + 0, + ) + self.log_event(f"Param read: {param_id} = {val}") + + elif mtype == "PARAM_REQUEST_LIST": + self.log_event("Param list requested, streaming all...") + for idx, (p_name, (p_val, p_type)) in enumerate(self.params.items()): + conn.mav.param_value_send( + p_name.encode("ascii"), + float(p_val), + p_type, + len(self.params), + idx, + ) + time.sleep(0.002) + + elif mtype == "PARAM_SET": + param_id = msg.param_id + if isinstance(param_id, bytes): + param_id = param_id.decode("ascii", errors="ignore").rstrip("\x00") + new_val = float(msg.param_value) + ptype = int(msg.param_type) + self.params[param_id] = (new_val, ptype) + conn.mav.param_value_send( + param_id.encode("ascii"), + new_val, + ptype, + len(self.params), + 0, + ) + self.log_event(f"Param updated: {param_id} -> {int(new_val) if new_val.is_integer() else new_val}") + + elif mtype == "MANUAL_CONTROL": + self.controls.x = float(msg.x) + self.controls.y = float(msg.y) + self.controls.z = float(msg.z) + self.controls.r = float(msg.r) + self.controls.last_rx = time.monotonic() + + def _render_dashboard(self): + # Clear screen ANSI + out = sys.stdout + out.write("\033[H") + + # Color codes + GREEN = "\033[92m" + RED = "\033[91m" + YELLOW = "\033[93m" + CYAN = "\033[96m" + BOLD = "\033[1m" + DIM = "\033[2m" + RESET = "\033[0m" + + def bar(val: float, min_v: float, max_v: float, width: int = 14) -> str: + norm = max(0.0, min(1.0, (val - min_v) / (max_v - min_v))) + pos = int(norm * width) + return f"[{'=' * pos}{' ' * (width - pos)}]" + + status_tag = f"{GREEN}{BOLD} ARMED {RESET}" if self.armed else f"{YELLOW}DISARMED{RESET}" + link_tag = f"{GREEN}ONLINE{RESET}" if self.client_connected else f"{RED}WAITING{RESET}" + + lines = [ + f"{BOLD}{CYAN}========================================================================{RESET}", + f"{BOLD} ArduSub SimpleROV-3 Flight Controller Spoofer & Telemetry Simulator {RESET}", + f"{BOLD}{CYAN}========================================================================{RESET}", + f" Endpoint: {BOLD}{self.endpoint}{RESET} Baud: {self.baud} Link: {link_tag} State: {status_tag} Mode: {BOLD}MANUAL{RESET}", + f"{CYAN}------------------------------------------------------------------------{RESET}", + f" {BOLD}BATTERY & POWER:{RESET}", + f" Voltage: {BOLD}{self.battery_base_v:.2f} V{RESET} ({self.battery_cells}S) Current: {BOLD}{self.current_a:.2f} A{RESET} Power: {BOLD}{self.battery_base_v * self.current_a:.1f} W{RESET}", + f" Depth: {BOLD}{self.depth_m:.2f} m{RESET} Heading: {BOLD}{self.heading_deg:.0f}°{RESET} Roll/Pitch: {self.roll_deg:+.1f}° / {self.pitch_deg:+.1f}°", + f"{CYAN}------------------------------------------------------------------------{RESET}", + f" {BOLD}INCOMING MANUAL CONTROLS (from Sub Control app):{RESET}", + f" Surge (Forward/Back): {self.controls.x:+5.0f} {bar(self.controls.x, -1000, 1000)}", + f" Yaw (Steer/Turn): {self.controls.r:+5.0f} {bar(self.controls.r, -1000, 1000)}", + f" Heave (Vertical): {self.controls.z:5.0f} {bar(self.controls.z, 0, 1000)}", + f"{CYAN}------------------------------------------------------------------------{RESET}", + f" {BOLD}SIMULATED THRUSTER PWM OUTPUTS:{RESET}", + f" Motor 1 (Port / Left): {self.m1_pwm:4d} µs {bar(self.m1_pwm, 1100, 1900)}", + f" Motor 2 (Starboard / Right): {self.m2_pwm:4d} µs {bar(self.m2_pwm, 1100, 1900)}", + f" Motor 3 (Vertical / Center): {self.m3_pwm:4d} µs {bar(self.m3_pwm, 1100, 1900)}", + f"{CYAN}------------------------------------------------------------------------{RESET}", + f" {BOLD}RECENT EVENT LOG:{RESET}", + ] + + if not self.events: + lines.append(f" {DIM}No events yet. Connect Sub Control app to begin.{RESET}") + else: + for ev in list(self.events)[-5:]: + lines.append(f" {ev}") + + lines.append(f"{CYAN}------------------------------------------------------------------------{RESET}") + lines.append(f" Packets RX: {self.total_packets_rx:<8d} Packets TX: {self.total_packets_tx:<8d} (Press Ctrl+C to exit)") + + out.write("\n".join(lines) + "\n") + out.flush() + + +def main(): + parser = argparse.ArgumentParser( + description="ArduSub Flight Controller (FC) Spoofer & Telemetry Simulator", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument( + "--endpoint", + type=str, + default="tcpin:127.0.0.1:5760", + help="MAVLink connection string (e.g. 'tcpin:127.0.0.1:5760', 'udpin:127.0.0.1:14550', 'COM3')", + ) + parser.add_argument( + "--baud", + type=int, + default=57600, + help="Baud rate (for serial COM ports)", + ) + parser.add_argument( + "--cells", + type=int, + default=4, + help="Battery cell count (4S = 16.0V nominal)", + ) + parser.add_argument( + "--voltage", + type=float, + default=15.8, + help="Nominal starting battery voltage (V)", + ) + parser.add_argument( + "--no-dashboard", + action="store_true", + help="Disable interactive terminal dashboard and use plain output", + ) + + args = parser.parse_args() + + # Enable ANSI escape sequences on Windows console + if os.name == "nt": + os.system("") + + spoofer = FlightControllerSpoofer( + endpoint=args.endpoint, + baud=args.baud, + battery_cells=args.cells, + nominal_voltage=args.voltage, + show_dashboard=not args.no_dashboard, + ) + spoofer.run() + + +if __name__ == "__main__": + main() diff --git a/SUB Control/run_spoofer.bat b/SUB Control/run_spoofer.bat new file mode 100644 index 0000000..c07c487 --- /dev/null +++ b/SUB Control/run_spoofer.bat @@ -0,0 +1,9 @@ +@echo off +title ArduSub FC Spoofer +cd /d "%~dp0" +if exist ".venv\Scripts\python.exe" ( + ".venv\Scripts\python.exe" -u fc_spoofer.py %* +) else ( + python -u fc_spoofer.py %* +) +pause diff --git a/SUB Control/run_spoofer.ps1 b/SUB Control/run_spoofer.ps1 new file mode 100644 index 0000000..0d261c9 --- /dev/null +++ b/SUB Control/run_spoofer.ps1 @@ -0,0 +1,20 @@ +$ErrorActionPreference = "Stop" +$projectRoot = Split-Path -Parent $MyInvocation.MyCommand.Path +Set-Location -LiteralPath $projectRoot + +$existingListener = Get-NetTCPConnection -LocalPort 5760 -State Listen -ErrorAction SilentlyContinue | Select-Object -First 1 +if ($existingListener) { + $existingProcess = Get-CimInstance Win32_Process -Filter "ProcessId=$($existingListener.OwningProcess)" -ErrorAction SilentlyContinue + Write-Host "" + Write-Host "The simulator is already LISTENING on tcp:127.0.0.1:5760." -ForegroundColor Green + Write-Host "Process: $($existingListener.OwningProcess) $($existingProcess.Name)" + Write-Host "Leave that simulator running and select 'Simulator (TCP 5760)' in SUB Control." + Write-Host "" + exit 0 +} + +if (Test-Path -LiteralPath ".venv\Scripts\python.exe") { + & .venv\Scripts\python.exe -u fc_spoofer.py @args +} else { + & python -u fc_spoofer.py @args +} diff --git a/SUB Control/web/app.js b/SUB Control/web/app.js index 274b7d9..810e853 100644 --- a/SUB Control/web/app.js +++ b/SUB Control/web/app.js @@ -1,25 +1,68 @@ +const STATUS_LABELS = { + disconnected: 'Disconnected — select a port and connect', + connecting: 'Connecting…', + connected_disarmed: 'Connected · Disarmed', + armed: 'Connected · Armed', + link_degraded: 'Signal lost — check tether', +}; const $=s=>document.querySelector(s), $$=s=>[...document.querySelectorAll(s)]; -const control={forward:0,turn:0,vertical:0}, keys=new Set(); let bridgeReady=false, connected=false, batteryConfig={}, servoSignature='', lastAxisSend=0, axisTimer=0; +const control={forward:0,turn:0,vertical:0}, keys=new Set(); let bridgeReady=false, appState='disconnected', batteryConfig={}, servoSignature='', lastAxisSend=0, axisTimer=0; const api=(name,...args)=>window.pywebview.api[name](...args); addEventListener('pointermove',e=>{const button=e.target.closest?.('button:not(.battery-card), .stick');if(!button)return;const rect=button.getBoundingClientRect();button.style.setProperty('--glow-x',`${e.clientX-rect.left}px`);button.style.setProperty('--glow-y',`${e.clientY-rect.top}px`)}); document.addEventListener('click',e=>{const btn=e.target.closest('.num-step');if(!btn)return;const pill=btn.closest('.number-pill');if(!pill)return;const input=pill.querySelector('input');if(!input)return;const step=+input.step||1,min=input.min!==''?+input.min:-Infinity,max=input.max!==''?+input.max:Infinity;let val=+input.value||0;if(btn.classList.contains('down'))val=Math.max(min,+(val-step).toFixed(4));else val=Math.min(max,+(val+step).toFixed(4));input.value=val;input.dispatchEvent(new Event('input',{bubbles:true}));input.dispatchEvent(new Event('change',{bubbles:true}))}); function toast(message,error=false){const el=$('#toast');el.textContent=message;el.className=`toast show${error?' error':''}`;clearTimeout(toast.t);toast.t=setTimeout(()=>el.className='toast',2600)} window.showToast=toast; -function populatePorts(ports){const p=$('#port'),old=p.value;p.innerHTML='';if(!ports.length)p.add(new Option('No COM ports',''));ports.forEach(x=>p.add(new Option(x,x)));if(ports.includes(old))p.value=old} -function sendAxes(){if(!bridgeReady)return;const transmit=()=>{lastAxisSend=performance.now();axisTimer=0;api('set_axes',control.forward,control.turn,control.vertical).catch(e=>toast(e.message,true))},wait=50-(performance.now()-lastAxisSend);if(wait<=0)transmit();else{clearTimeout(axisTimer);axisTimer=setTimeout(transmit,wait)}} -function updateReadouts(){ $('#surge-readout').textContent=Math.round(control.forward*100);$('#yaw-readout').textContent=Math.round(control.turn*100);$('#heave-readout').textContent=Math.round(control.vertical*100)} +function populatePorts(ports){ + const p=$('#port'); + if(!p)return; + const old=p.value||''; + p.innerHTML=''; + + const allList=[]; + const seen=new Set(); -window.setJoyControl=(x,y)=>{control.turn=x;control.forward=-y;updateReadouts();sendAxes()}; + const addPort=item=>{ + let dev=item, lbl=item; + if(item&&typeof item==='object'){ + dev=item.device||''; + lbl=item.label||dev; + } + if(dev&&!seen.has(dev)){ + seen.add(dev); + allList.push({device:dev, label:lbl}); + } + }; + + if(Array.isArray(ports)) ports.forEach(addPort); + addPort({device:'demo', label:'Demo Mode (no hardware)'}); + + let matched=false; + allList.forEach(item=>{ + const opt=new Option(item.label, item.device); + p.add(opt); + if(item.device===old){ + p.value=item.device; + matched=true; + } + }); + if(!matched&&allList.length>0){ + p.value=allList[0].device; + } +} +function sendAxes(){if(!bridgeReady||appState!=='armed')return;const transmit=()=>{lastAxisSend=performance.now();axisTimer=0;api('set_axes',control.forward,control.turn,control.vertical).catch(e=>toast(e.message,true))},wait=50-(performance.now()-lastAxisSend);if(wait<=0)transmit();else{clearTimeout(axisTimer);axisTimer=setTimeout(transmit,wait)}} +function updateReadouts(){ if(appState!=='armed'){$('#surge-readout').textContent='—';$('#yaw-readout').textContent='—';$('#heave-readout').textContent='—';return;}$('#surge-readout').textContent=Math.round(control.forward*100);$('#yaw-readout').textContent=Math.round(control.turn*100);$('#heave-readout').textContent=Math.round(control.vertical*100);} + +window.setJoyControl=(x,y)=>{if(appState!=='armed')return;control.turn=x;control.forward=-y;updateReadouts();sendAxes();}; function releaseHandle(el){el.classList.remove('pressed');el.classList.add('released');setTimeout(()=>el.classList.remove('released'),520)} const heave=$('#heave'),hknob=$('#heave-knob'),hfill=$('#heave-fill');let heaveDrag=false,heaveAnim=0; -function setHeave(v,send=true){v=Math.max(-1,Math.min(1,v));control.vertical=v;hknob.style.top=`${50-v*43}%`;hfill.style.top=v>=0?`${50-v*43}%`:'50%';hfill.style.height=`${Math.abs(v)*43}%`;updateReadouts();if(send)sendAxes()} +function setHeave(v,send=true){if(appState!=='armed'&&v!==0)return;v=Math.max(-1,Math.min(1,v));control.vertical=v;hknob.style.top=`${50-v*43}%`;hfill.style.top=v>=0?`${50-v*43}%`:'50%';hfill.style.height=`${Math.abs(v)*43}%`;updateReadouts();if(send&&appState==='armed')sendAxes();} function heaveEvent(e){const b=heave.getBoundingClientRect();setHeave((b.top+b.height/2-e.clientY)/(b.height*.43))} -heave.addEventListener('pointerdown',e=>{heaveDrag=true;hknob.classList.remove('released');hknob.classList.add('pressed');cancelAnimationFrame(heaveAnim);heave.setPointerCapture(e.pointerId);heaveEvent(e)});heave.addEventListener('pointermove',e=>{if(heaveDrag)heaveEvent(e)});heave.addEventListener('pointerup',()=>{heaveDrag=false;releaseHandle(hknob);easeHeave()});heave.addEventListener('pointercancel',()=>{heaveDrag=false;releaseHandle(hknob);easeHeave()}); +heave.addEventListener('pointerdown',e=>{if(appState!=='armed')return;heaveDrag=true;hknob.classList.remove('released');hknob.classList.add('pressed');cancelAnimationFrame(heaveAnim);heave.setPointerCapture(e.pointerId);heaveEvent(e)});heave.addEventListener('pointermove',e=>{if(heaveDrag)heaveEvent(e)});heave.addEventListener('pointerup',()=>{heaveDrag=false;releaseHandle(hknob);easeHeave()});heave.addEventListener('pointercancel',()=>{heaveDrag=false;releaseHandle(hknob);easeHeave()}); function easeHeave(){const sv=control.vertical,start=performance.now();function frame(now){const p=Math.min(1,(now-start)/400),smooth=p*p*(3-2*p);setHeave(sv*(1-smooth));if(p<1)heaveAnim=requestAnimationFrame(frame)}heaveAnim=requestAnimationFrame(frame)} -function applyKeys(){const turn=(keys.has('d')||keys.has('e')?1:0)-(keys.has('a')||keys.has('q')?1:0),forward=(keys.has('w')?1:0)-(keys.has('s')?1:0),vertical=(keys.has('r')?1:0)-(keys.has('f')?1:0);if(turn||forward){window.joystick3D?.setPos(turn,-forward)}else{window.joystick3D?.easeBack()}if(vertical){cancelAnimationFrame(heaveAnim);setHeave(vertical)}else if(!heaveDrag)easeHeave()} -addEventListener('keydown',e=>{if(/^(INPUT|SELECT)$/.test(e.target.tagName))return;if(e.key==='F2'){e.preventDefault();if(!e.repeat)startArmHold();return}const shortcuts={F1:'#manual',F3:'#disarm'};if(shortcuts[e.key]){e.preventDefault();if(!e.repeat)$(shortcuts[e.key]).click();return}const k=e.key.toLowerCase();if('wasdqerf'.includes(k)&&!keys.has(k)){keys.add(k);applyKeys()}});addEventListener('keyup',e=>{if(e.key==='F2'){e.preventDefault();stopArmHold(true);return}const k=e.key.toLowerCase();if(keys.delete(k))applyKeys()}); +function applyKeys(){if(appState!=='armed'){keys.clear();window.joystick3D?.easeBack();if(!heaveDrag)easeHeave();return;}const turn=(keys.has('d')||keys.has('e')?1:0)-(keys.has('a')||keys.has('q')?1:0),forward=(keys.has('w')?1:0)-(keys.has('s')?1:0),vertical=(keys.has('r')?1:0)-(keys.has('f')?1:0);if(turn||forward){window.joystick3D?.setPos(turn,-forward)}else{window.joystick3D?.easeBack()}if(vertical){cancelAnimationFrame(heaveAnim);setHeave(vertical)}else if(!heaveDrag)easeHeave();} +addEventListener('keydown',e=>{if(/^(INPUT|SELECT)$/.test(e.target.tagName))return;if(e.key==='F2'){e.preventDefault();if(appState==='connected_disarmed'&&engageButton?.classList.contains('arm')&&!e.repeat)startEngageHold();return;}if(e.key==='F1'){e.preventDefault();if(appState==='connected_disarmed'&&!engageButton?.classList.contains('arm')&&!e.repeat)engageButton?.click();return;}if(e.key==='F3'){e.preventDefault();if(appState==='armed'&&!e.repeat)engageButton?.click();return;}const k=e.key.toLowerCase();if('wasdqerf'.includes(k)){if(appState!=='armed')return;if(!keys.has(k)){keys.add(k);applyKeys();}}});addEventListener('keyup',e=>{if(e.key==='F2'){e.preventDefault();stopEngageHold(true);return;}const k=e.key.toLowerCase();if(keys.delete(k))applyKeys();}); addEventListener('blur',()=>{keys.clear();window.joystick3D?.easeBack();if(!heaveDrag)easeHeave()}); function openModal(id){ @@ -119,66 +162,237 @@ if(rovStage){ if(file)handleStlFile(file); }); } -$('#refresh').onclick=async e=>{const button=e.currentTarget;button.classList.remove('spinning');void button.offsetWidth;button.classList.add('spinning');setTimeout(()=>button.classList.remove('spinning'),650);if(bridgeReady)populatePorts(await api('refresh_ports'))};$('#connect').onclick=async()=>{try{if(connected)await api('disconnect');else await api('connect',$('#port').value,+$('#baud').value,+$('#rate').value)}catch(e){toast(e.message,true)}}; -$('#manual').onclick=async()=>{try{await api('command','manual');toast('Manual mode command sent')}catch(e){toast(e.message,true)}}; -const armButton=$('#arm'); -let armHoldTimer=null, armStart=0, armCharged=false; -const ARM_HOLD_MS=1000; +$('#refresh').onclick=async e=>{const button=e.currentTarget;button.classList.remove('spinning');void button.offsetWidth;button.classList.add('spinning');setTimeout(()=>button.classList.remove('spinning'),650);if(bridgeReady)populatePorts(await api('refresh_ports'))}; +$('#connect').onclick=async()=>{ + const btn=$('#connect'); + const label=btn.querySelector('.btn-label')||btn; + const p=$('#port'); + const port=p?p.value:''; -function updateArmProgress(){ - if(!armStart)return; - const elapsed=performance.now()-armStart; - const progress=Math.min(1,elapsed/ARM_HOLD_MS); - armButton.style.setProperty('--arm-progress',`${progress*100}%`); + if(appState==='connecting'){ + label.textContent='Canceling…'; + try{ await api('disconnect'); }catch(_){} + return; + } + + if(appState!=='disconnected'){ + label.textContent='Disconnecting…'; + try{ + await api('disconnect'); + toast('Disconnected'); + }catch(e){ + toast(e.message,true); + } + }else{ + if(!port){ + toast('Select or enter a port to connect',true); + return; + } + btn.classList.add('is-connecting'); + label.textContent='Connecting…'; + const statusEl=$('#status-text'); + if(statusEl)statusEl.textContent=`Opening ${port}…`; + toast(`Connecting to ${port}…`); + try{ + const confirmedState=await api('connect',port,+$('#baud').value,+$('#rate').value); + if(confirmedState)render(confirmedState); + toast('MAVLink connected'); + }catch(e){ + btn.classList.remove('is-connecting'); + label.textContent='Connect'; + toast(e.message,true); + if(statusEl)statusEl.textContent=`Connection error: ${e.message}`; + } + } +}; +const customPortBtn=$('#custom-port-btn'), customPortInput=$('#custom-port-input'); +if(customPortBtn&&customPortInput){ + const applyCustomPort=()=>{ + const val=customPortInput.value.trim(); + if(!val)return; + const p=$('#port'); + let exists=false; + for(const opt of p.options){ + if(opt.value===val){opt.selected=true;exists=true;break;} + } + if(!exists){ + p.add(new Option(`Custom (${val})`, val), 0); + p.value=val; + } + closeModal($('#link-modal')); + toast(`Selected port: ${val}`); + }; + customPortBtn.onclick=applyCustomPort; + customPortInput.onkeydown=e=>{if(e.key==='Enter'){e.preventDefault();applyCustomPort();}}; +} +const engageButton=$('#engage'); +let engageHoldTimer=null, engageStart=0, engageCharged=false, settingMode=false; +const ENGAGE_HOLD_MS=1000; + +function isManualMode(s){ + const m=(s&&s.mode)?String(s.mode).toUpperCase():''; + return m==='MANUAL'; +} + +function updateEngageProgress(){ + if(!engageStart||appState!=='connected_disarmed'||!engageButton?.classList.contains('arm')){ + stopEngageHold(true); + return; + } + const elapsed=performance.now()-engageStart; + const progress=Math.min(1,elapsed/ENGAGE_HOLD_MS); + engageButton.style.setProperty('--arm-progress',`${progress*100}%`); if(progress>=1){ - armCharged=true; - stopArmHold(false); - armButton.classList.add('charged'); - setTimeout(()=>armButton.classList.remove('charged'),600); + engageCharged=true; + stopEngageHold(false); + engageButton.classList.add('charged'); + setTimeout(()=>engageButton.classList.remove('charged'),600); api('command','arm').then(()=>toast('Vehicle armed')).catch(e=>toast(e.message,true)); return; } - armHoldTimer=requestAnimationFrame(updateArmProgress); + engageHoldTimer=requestAnimationFrame(updateEngageProgress); } -function startArmHold(e){ - if(armStart||armCharged)return; +function startEngageHold(e){ + if(appState!=='connected_disarmed'||!engageButton?.classList.contains('arm'))return; + if(engageStart||engageCharged)return; if(e&&e.button!==undefined&&e.button!==0)return; - armStart=performance.now(); - armCharged=false; - armButton.classList.add('charging'); - armButton.style.setProperty('--arm-progress','0%'); - armHoldTimer=requestAnimationFrame(updateArmProgress); + engageStart=performance.now(); + engageCharged=false; + engageButton.classList.add('charging'); + engageButton.style.setProperty('--arm-progress','0%'); + engageHoldTimer=requestAnimationFrame(updateEngageProgress); } -function stopArmHold(wasCancelled=true){ - if(!armStart)return; - const elapsed=performance.now()-armStart; - cancelAnimationFrame(armHoldTimer); - armHoldTimer=null; - armStart=0; - armButton.classList.remove('charging'); - armButton.style.setProperty('--arm-progress','0%'); - if(wasCancelled&&!armCharged&&elapsed70){ - toast('Hold for 1s to Arm vehicle'); +function stopEngageHold(wasCancelled=true){ + if(!engageStart)return; + const elapsed=performance.now()-engageStart; + cancelAnimationFrame(engageHoldTimer); + engageHoldTimer=null; + engageStart=0; + engageButton.classList.remove('charging'); + engageButton.style.setProperty('--arm-progress','0%'); + if(wasCancelled&&!engageCharged&&elapsed70){ + toast('Hold for 1s to Engage vehicle'); } - armCharged=false; + engageCharged=false; } -armButton.addEventListener('pointerdown',startArmHold); -armButton.addEventListener('pointerup',()=>stopArmHold(true)); -armButton.addEventListener('pointerleave',()=>stopArmHold(true)); -armButton.addEventListener('pointercancel',()=>stopArmHold(true)); -armButton.addEventListener('contextmenu',e=>e.preventDefault()); +if(engageButton){ + engageButton.addEventListener('pointerdown',startEngageHold); + engageButton.addEventListener('pointerup',()=>stopEngageHold(true)); + engageButton.addEventListener('pointerleave',()=>stopEngageHold(true)); + engageButton.addEventListener('pointercancel',()=>stopEngageHold(true)); + engageButton.addEventListener('contextmenu',e=>e.preventDefault()); + + engageButton.onclick=async()=>{ + if(appState==='armed'){ + try{ + await api('command','disarm'); + toast('Disarm command sent'); + }catch(e){ + toast(e.message,true); + } + }else if(appState==='connected_disarmed'&&!engageButton.classList.contains('arm')){ + try{ + settingMode=true; + const lbl=$('#engage-label'); + if(lbl)lbl.textContent='Setting mode…'; + await api('command','manual'); + toast('Manual mode requested'); + }catch(e){ + settingMode=false; + toast(e.message,true); + } + } + }; +} -$('#disarm').onclick=async()=>{try{await api('command','disarm');toast('Disarm command sent')}catch(e){toast(e.message,true)}}; $('#logs').onclick=()=>api('open_logs'); function setGain(value){const gain=$('#gain');gain.value=Math.max(+gain.min,Math.min(+gain.max,value));$('#gain-value').textContent=`${gain.value}%`;if(bridgeReady)api('set_gain',+gain.value)} $('#gain').oninput=e=>setGain(+e.target.value);$('#gain-down').onclick=()=>setGain(+$('#gain').value-5);$('#gain-up').onclick=()=>setGain(+$('#gain').value+5); $('#battery-form').onsubmit=async e=>{e.preventDefault();const data=Object.fromEntries(new FormData(e.target));try{batteryConfig=await api('save_battery',data);closeModal($('#battery-modal'));toast('Battery settings saved')}catch(err){toast(err.message,true)}}; function fillBatteryForm(){for(const [k,v] of Object.entries(batteryConfig)){const el=$(`#battery-form [name=${k}]`);if(el)el.value=v}} function renderMotors(servos){const signature=JSON.stringify(servos);if(signature===servoSignature)return;servoSignature=signature;const list=$('#motor-list');list.innerHTML='';for(let i=1;i<=16;i++){const s=servos[String(i)]||{},row=document.createElement('div');row.className='motor-row';const opts=['',...Array.from({length:12},(_,n)=>``)].join('');row.innerHTML=`SERVO${i}${s.direction==null?'—':s.direction===-1?'Reversed':'Normal'}`;row.querySelector('select').onchange=e=>api('set_servo_function',i,+e.target.value).catch(x=>toast(x.message,true));row.querySelector('button').onclick=()=>api('reverse_motor',i).catch(x=>toast(x.message,true));list.append(row)}} -function render(s){connected=s.connected;$('#connect').textContent=connected?'Disconnect':'Connect';const linkText=s.link==='LINK OK'?'connected':(s.link?s.link.toLowerCase():'disconnected');$('#link').textContent=linkText;$('.link-state').classList.toggle('online',s.link==='LINK OK'||s.link==='connected');$('#vehicle').textContent=s.vehicle?s.vehicle.toLowerCase():'no heartbeat';$('#status').textContent=s.status?s.status.toLowerCase():'ready';const b=s.battery;if(b){$('#battery-title').textContent=`${b.voltage.toFixed(2)} V`;$('#battery-percent').textContent=`${Math.round(b.percent)}% • ${b.cells}s`;$('#battery-fill').style.width=`${b.percent}%`;$('#battery-fill').style.background=b.low?'var(--red)':'var(--acid)'}else{$('#battery-title').textContent='—';$('#battery-percent').textContent='waiting';$('#battery-fill').style.width='0'}renderMotors(s.servos)} +function render(s){ + appState=s.app_state||'disconnected'; + document.body.dataset.appState=appState; + const isOnline=(appState==='connected_disarmed'||appState==='armed'); + const connectBtn=$('#connect'); + const connectLabel=connectBtn?connectBtn.querySelector('.btn-label'):null; + if(connectBtn){ + if(appState==='connecting'){ + if(connectLabel)connectLabel.textContent='Connecting…'; + else connectBtn.textContent='Connecting…'; + connectBtn.classList.add('is-connecting'); + connectBtn.classList.remove('is-connected'); + }else if(appState!=='disconnected'){ + if(connectLabel)connectLabel.textContent='Disconnect'; + else connectBtn.textContent='Disconnect'; + connectBtn.classList.remove('is-connecting'); + connectBtn.classList.add('is-connected'); + }else{ + if(connectLabel)connectLabel.textContent='Connect'; + else connectBtn.textContent='Connect'; + connectBtn.classList.remove('is-connecting', 'is-connected'); + } + } + const engageBtn=$('#engage'); + const engageLabel=$('#engage-label'); + if(engageBtn&&engageLabel){ + if(appState==='armed'){ + settingMode=false; + engageBtn.disabled=false; + engageBtn.className='button danger'; + engageBtn.title='Disarm (F3)'; + engageLabel.textContent='Disarm'; + }else if(appState==='connected_disarmed'){ + const manualConfirmed=isManualMode(s); + if(manualConfirmed){ + settingMode=false; + engageBtn.disabled=false; + engageBtn.className='button arm'; + engageBtn.title='Hold to Engage (F2)'; + engageLabel.textContent='Hold to Engage'; + }else{ + engageBtn.disabled=false; + engageBtn.className='button subtle'; + engageBtn.title='Set Manual Mode (F1)'; + engageLabel.textContent=settingMode?'Setting mode…':'Engage'; + } + }else{ + settingMode=false; + engageBtn.disabled=true; + engageBtn.className='button subtle'; + engageBtn.title='Engage'; + engageLabel.textContent='Engage'; + } + } + const motorBtn=$('[data-open="motors-modal"]'); + if(motorBtn)motorBtn.disabled=(!isOnline); + + const statusEl=$('#status-text'); + if(statusEl){ + if(s.status){ + statusEl.textContent=s.status; + }else{ + statusEl.textContent=STATUS_LABELS[appState]||STATUS_LABELS.disconnected; + } + } + updateReadouts(); + const b=s.battery; + if(b){ + $('#battery-title').textContent=`${b.voltage.toFixed(2)} V`; + $('#battery-percent').textContent=`${Math.round(b.percent)}% • ${b.cells}s`; + $('#battery-fill').style.width=`${b.percent}%`; + $('#battery-fill').style.background=b.low?'var(--red)':'var(--acid)'; + }else{ + $('#battery-title').textContent='—'; + $('#battery-percent').textContent='waiting'; + $('#battery-fill').style.width='0'; + } + renderMotors(s.servos); +} async function poll(){if(!bridgeReady)return;try{render(await api('poll'))}catch(e){}setTimeout(poll,250)} let modelLoadPromise=null; @@ -259,7 +473,6 @@ async function boot(){ if(booted||!window.pywebview?.api)return; booted=true; bridgeReady=true; - loadDefaultModel(); try{ const init=await api('initial_state'); batteryConfig=init.battery_config; @@ -267,8 +480,11 @@ async function boot(){ fillBatteryForm(); render(init.state); poll(); + // Populate the serial selector before starting the optional model load. + // A large cached mesh must never delay an operator seeing USB COM ports. + loadDefaultModel(); setInterval(async()=>{ - if(!connected){ + if(appState==='disconnected'){ try{populatePorts(await api('refresh_ports'))}catch(_){} } },2000); diff --git a/SUB Control/web/index.html b/SUB Control/web/index.html index c109a6d..5e0d258 100644 --- a/SUB Control/web/index.html +++ b/SUB Control/web/index.html @@ -22,23 +22,24 @@ -
vehicleno heartbeatSimpleROV-3 • differential drive
+
vehicleSimpleROV-33 thrusters • differential drive
-
disconnected
- + - +
@@ -71,21 +72,19 @@
- - - +
-

ready. select a USB port to begin.

logs
+

Disconnected — select a port and connect

logs
- connection

Link Settings

20 Hz is recommended for smooth intermediate commands on this link.

 + connection

Link Settings

20 Hz is recommended for smooth intermediate commands on this link.

 Power

Battery Settings

 configuration

Motor Outputs

Function and direction are read one parameter at a time to protect the low-bandwidth link.

OutputFunctionDirection
 diff --git a/SUB Control/web_ui.py b/SUB Control/web_ui.py index e66cef1..d57e3ff 100644 --- a/SUB Control/web_ui.py +++ b/SUB Control/web_ui.py @@ -24,12 +24,15 @@ class ControlApi: def __init__(self): self.events: queue.Queue = queue.Queue() self.worker = MavlinkWorker(self.events) + self.demo_mode = False self.lock = threading.Lock() self.state = { "connected": False, "link": "DISCONNECTED", "vehicle": "No heartbeat", "armed": False, + "mode": None, + "app_state": "disconnected", "status": "Ready. Select a USB port to begin.", "battery": None, "log_path": None, @@ -57,14 +60,26 @@ class ControlApi: def initial_state(self): return { - "ports": [p.device for p in list_ports.comports()], + "ports": self.refresh_ports(), "battery_config": self.battery_config, "state": self._snapshot(), "model_info": self.get_saved_model_info(), } def refresh_ports(self): - return [p.device for p in list_ports.comports()] + ports = [] + # Add physical and virtual serial COM ports + for p in list_ports.comports(): + desc = p.description or "" + if desc and desc != "n/a" and desc != p.device: + label = f"{p.device} ({desc})" + else: + label = p.device + ports.append({"device": p.device, "label": label, "type": "serial"}) + + # Built-in UI testing: no external process, port, or MAVLink socket. + ports.insert(0, {"device": "demo", "label": "Demo Mode (no hardware)", "type": "demo"}) + return ports def minimize_window(self): if self._window: @@ -202,22 +217,69 @@ class ControlApi: return None def connect(self, port, baud=57600, rate=20): + print(f"[web_ui] connect() called: port={port!r}, baud={baud!r}, rate={rate!r}") if not port: raise ValueError("Select a USB serial port") + if str(port).lower() in ("demo", "tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"): + self.demo_mode = True + self.servo_functions = {1: 33, 2: 34, 3: 35} + self.motor_directions = {1: 1, 2: 1, 3: 1} + with self.lock: + self.state.update( + connected=True, link="LINK OK", vehicle="DEMO • DISARMED", + armed=False, mode="MANUAL", status="Demo mode ready — no hardware connected.", + battery={"voltage": 15.8, "per_cell": 3.95, "percent": 72, + "cells": 4, "low": False}, + ) + for servo in range(1, 17): + function = self.servo_functions.get(servo, 0) + motor = motor_number_for_function(function) + self.state["servos"][str(servo)] = { + "function": function, "label": MOTOR_FUNCTIONS.get(function, "Unassigned"), + "motor": motor, "direction": self.motor_directions.get(motor) if motor else None, + } + return self._snapshot() self.worker.rate_hz = int(rate) self.worker.connect(str(port), int(baud)) with self.lock: self.state.update( connected=True, link="CONNECTING", status=f"Opening {port} at {baud} baud…", + armed=False, ) - return True + print(f"[web_ui] connect() dispatched to worker thread OK") + # Return only once the worker has either seen a heartbeat or reported a + # connection failure. This makes the button independent of the regular + # UI polling loop and prevents an indefinite visual "Connecting" state. + deadline = time.monotonic() + 8.0 + while time.monotonic() < deadline: + snapshot = self.poll() + if snapshot["app_state"] != "connecting": + return snapshot + time.sleep(0.05) + self.worker.disconnect() + time.sleep(0.1) + self.poll() + raise TimeoutError(f"No MAVLink heartbeat received from {port} within 8 seconds") def disconnect(self): + if self.demo_mode: + self.demo_mode = False + with self.lock: + self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat", + armed=False, mode=None, battery=None, status="Demo mode disconnected") + return True self.worker.disconnect() return True def set_axes(self, forward, turn, vertical): + if self.demo_mode: + with self.lock: + self.state["status"] = ( + f"Demo controls • surge {float(forward):+.2f} • " + f"yaw {float(turn):+.2f} • heave {float(vertical):+.2f}" + ) + return True self.worker.set_axes(Axes( max(-1.0, min(1.0, float(forward))), 0.0, @@ -232,6 +294,15 @@ class ControlApi: return value def command(self, name): + if self.demo_mode: + if name not in ("manual", "arm", "disarm"): + raise ValueError(f"Unknown command: {name}") + armed = name == "arm" if name != "manual" else self.state.get("armed", False) + with self.lock: + self.state.update(armed=armed, mode="MANUAL", + vehicle=f"DEMO • {'ARMED' if armed else 'DISARMED'}", + status=f"Demo: {'Manual mode' if name == 'manual' else name.capitalize()} confirmed") + return True if not self.worker.connection or not self.worker.target_system: if name == "manual": raise RuntimeError("Connect to vehicle to enable Manual mode") @@ -258,6 +329,10 @@ class ControlApi: if not 1 <= servo <= 16 or not 0 <= motor <= 12: raise ValueError("Invalid servo or motor") function = 0 if motor == 0 else (32 + motor if motor <= 8 else 73 + motor) + if self.demo_mode: + self.servo_functions[servo] = function + self._update_servo(servo) + return True self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function) return True @@ -269,6 +344,10 @@ class ControlApi: current = self.motor_directions.get(motor) if current is None: raise ValueError(f"MOT_{motor}_DIRECTION has not been read yet") + if self.demo_mode: + self.motor_directions[motor] = -1 if current == 1 else 1 + self._update_servo(servo) + return True self.worker.set_parameter(f"MOT_{motor}_DIRECTION", -1 if current == 1 else 1) return True @@ -312,9 +391,24 @@ class ControlApi: ) return self._snapshot() + def _derive_app_state(self) -> str: + if not self.state.get("connected"): + return "disconnected" + link = self.state.get("link", "") + if link == "CONNECTING": + return "connecting" + if link in ("DATA OK / HEARTBEAT DELAYED", "ALL LINK DATA LOST"): + return "link_degraded" + if self.state.get("armed"): + return "armed" + return "connected_disarmed" + def _snapshot(self): with self.lock: - return json.loads(json.dumps(self.state)) + snap = json.loads(json.dumps(self.state)) + snap["app_state"] = self._derive_app_state() + snap["armed"] = bool(self.state.get("armed", False)) + return snap def _consume(self, event): kind = event[0] @@ -324,6 +418,7 @@ class ControlApi: self.state.update( connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}", armed=bool(armed), + mode=str(mode).upper(), ) elif kind in ("status", "statustext"): self.state["status"] = event[1] @@ -355,9 +450,13 @@ class ControlApi: self.state["log_path"] = event[1] elif kind == "error": self.state["status"] = event[1] + self.state["last_error"] = event[1] elif kind == "disconnected": + last_err = self.state.get("last_error") + status_msg = last_err if last_err else "Disconnected" self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat", - armed=False, battery=None, status="Disconnected") + armed=False, mode=None, battery=None, status=status_msg) + self.state["last_error"] = None def _update_servo(self, servo): function = self.servo_functions.get(servo) @@ -380,22 +479,49 @@ def _apply_dwm_styling(window): hwnd = int(window.native.Handle.ToInt64()) if hwnd: import ctypes + user32 = ctypes.windll.user32 + dwmapi = ctypes.windll.dwmapi corner_pref = ctypes.c_int(2) # DWMWCP_ROUND - ctypes.windll.dwmapi.DwmSetWindowAttribute( + dwmapi.DwmSetWindowAttribute( ctypes.c_void_p(hwnd), 33, ctypes.byref(corner_pref), ctypes.sizeof(corner_pref) ) dark_mode = ctypes.c_int(1) # DWMWA_USE_IMMERSIVE_DARK_MODE - ctypes.windll.dwmapi.DwmSetWindowAttribute( + dwmapi.DwmSetWindowAttribute( ctypes.c_void_p(hwnd), 20, ctypes.byref(dark_mode), ctypes.sizeof(dark_mode) ) + user32.SetForegroundWindow(ctypes.c_void_p(hwnd)) except Exception: pass +def _get_window_size() -> tuple[int, int, tuple[int, int]]: + target_w, target_h = 1280, 750 + min_w, min_h = 960, 580 + try: + import ctypes + user32 = ctypes.windll.user32 + sw = user32.GetSystemMetrics(0) + sh = user32.GetSystemMetrics(1) + if sw > 0 and sh > 0: + target_w = min(target_w, max(min_w, int(sw * 0.92))) + target_h = min(target_h, max(min_h, int(sh * 0.90))) + min_w = min(min_w, target_w) + min_h = min(min_h, target_h) + except Exception: + pass + return target_w, target_h, (min_w, min_h) + + class AppHTTPHandler(http.server.SimpleHTTPRequestHandler): def log_message(self, format, *args): pass # Suppress HTTP request logs in console + def end_headers(self): + self.send_header("Cache-Control", "no-cache, no-store, must-revalidate") + self.send_header("Pragma", "no-cache") + self.send_header("Expires", "0") + super().end_headers() + def do_GET(self): unquoted = urllib.parse.unquote(self.path) if self.path in ("/model", "/model.stl", "/runtime/vehicle_model.stl") or "Main Assembly" in unquoted or "vehicle_model" in unquoted: @@ -409,7 +535,6 @@ class AppHTTPHandler(http.server.SimpleHTTPRequestHandler): self.send_header("Content-Type", "model/stl") self.send_header("Content-Length", str(len(data))) self.send_header("Access-Control-Allow-Origin", "*") - self.send_header("Cache-Control", "no-cache") self.end_headers() self.wfile.write(data) return @@ -429,10 +554,11 @@ def main(): api._migrate_legacy_model() project_root = Path(__file__).resolve().parent port = start_local_server(project_root) - app_url = f"http://127.0.0.1:{port}/web/index.html" + app_url = f"http://127.0.0.1:{port}/web/index.html?v={int(time.time())}" + win_w, win_h, min_dims = _get_window_size() window = webview.create_window( "SUB Control", url=app_url, js_api=api, - width=1400, height=808, min_size=(1125, 690), + width=win_w, height=win_h, min_size=min_dims, background_color="#111719", frameless=True, easy_drag=False, shadow=True, )