"""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()