554 lines
23 KiB
Python
554 lines
23 KiB
Python
"""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()
|