Add reliable built-in demo mode
This commit is contained in:
+52
-6
@@ -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])
|
||||
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
}
|
||||
+267
-51
@@ -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='';
|
||||
|
||||
window.setJoyControl=(x,y)=>{control.turn=x;control.forward=-y;updateReadouts();sendAxes()};
|
||||
const allList=[];
|
||||
const seen=new Set();
|
||||
|
||||
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&&elapsed<ARM_HOLD_MS&&elapsed>70){
|
||||
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&&elapsed<ENGAGE_HOLD_MS&&elapsed>70){
|
||||
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=['<option value="0">Unassigned</option>',...Array.from({length:12},(_,n)=>`<option value="${n+1}" ${s.motor===n+1?'selected':''}>Motor ${n+1}</option>`)].join('');row.innerHTML=`<strong>SERVO${i}</strong><select aria-label="SERVO${i} function">${opts}</select><span>${s.direction==null?'—':s.direction===-1?'Reversed':'Normal'}</span><button class="button subtle">Reverse</button>`;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);
|
||||
|
||||
@@ -22,23 +22,24 @@
|
||||
<input id="model-input" type="file" accept=".stl,.STL" hidden>
|
||||
</div>
|
||||
</div>
|
||||
<div class="vehicle-name"><small>vehicle</small><strong id="vehicle">no heartbeat</strong><span>SimpleROV-3 • differential drive</span></div>
|
||||
<div class="vehicle-name"><small>vehicle</small><strong>SimpleROV-3</strong><span>3 thrusters • differential drive</span></div>
|
||||
<div class="rail-bottom">
|
||||
<button class="battery-card ghost" data-open="battery-modal">
|
||||
<span><small>Battery</small><strong id="battery-title">—</strong></span><i id="battery-percent">waiting</i>
|
||||
<b><em id="battery-fill"></em></b>
|
||||
</button>
|
||||
<div class="link-state"><span id="link-dot"></span><strong id="link">disconnected</strong></div>
|
||||
</div>
|
||||
</aside>
|
||||
|
||||
<section class="workspace">
|
||||
<header class="topbar">
|
||||
<div class="connection">
|
||||
<select id="port" aria-label="Serial port"><option value="">Select COM port…</option></select>
|
||||
<select id="port" aria-label="Serial port">
|
||||
<option value="demo" selected>Demo Mode (no hardware)</option>
|
||||
</select>
|
||||
<button id="refresh" class="icon-button" title="Refresh ports" aria-label="Refresh ports"><svg viewBox="0 0 24 24" aria-hidden="true"><path d="M20 7v5h-5M4 17v-5h5M18.5 9A7 7 0 0 0 6.8 6.8L4 10m16 4-2.8 3.2A7 7 0 0 1 5.5 15"/></svg></button>
|
||||
<button class="icon-button" data-open="link-modal" title="Link settings" aria-label="Link settings"><svg class="dots-icon" viewBox="0 0 24 24" aria-hidden="true"><circle cx="5" cy="12" r="1.25"/><circle cx="12" cy="12" r="1.25"/><circle cx="19" cy="12" r="1.25"/></svg></button>
|
||||
<button id="connect" class="button connect-button">Connect</button>
|
||||
<button id="connect" class="button connect-button" aria-label="Toggle connection"><span class="btn-spinner" aria-hidden="true"></span><span class="btn-label">Connect</span></button>
|
||||
</div>
|
||||
<div class="topbar-spacer pywebview-drag-region"></div>
|
||||
<div class="window-actions">
|
||||
@@ -71,21 +72,19 @@
|
||||
<section class="bottom-actions">
|
||||
<section class="command-bar">
|
||||
<div class="commands">
|
||||
<button id="manual" class="button subtle"><span>Manual</span></button>
|
||||
<button id="arm" class="button arm" aria-label="Hold to arm" title="Hold to arm (F2)"><span class="arm-copy"><span class="arm-label">Arm</span><span class="arm-holding-label">Holding…</span></span></button>
|
||||
<button id="disarm" class="button danger"><span>Disarm</span></button>
|
||||
<button id="engage" class="button subtle" aria-label="Engage" title="Engage"><span class="arm-copy"><span id="engage-label" class="arm-label">Engage</span><span class="arm-holding-label">Engaging…</span></span></button>
|
||||
</div>
|
||||
<label class="output-limit"><span>output limit <b id="gain-value">20%</b></span><div class="range-pill"><button id="gain-down" type="button" aria-label="Decrease output limit">‹</button><input id="gain" type="range" min="5" max="50" value="20"><button id="gain-up" type="button" aria-label="Increase output limit">›</button></div></label>
|
||||
<button class="button subtle" data-open="motors-modal">Motor Setup</button>
|
||||
</section>
|
||||
</section>
|
||||
<div class="status-line"><span></span><p id="status">ready. select a USB port to begin.</p><a id="logs" class="logs-link" title="Open session logs" aria-label="Open session logs">logs</a></div>
|
||||
<div class="status-line"><span id="status-dot" class="status-dot"></span><p id="status-text">Disconnected — select a port and connect</p><a id="logs" class="logs-link" title="Open session logs" aria-label="Open session logs">logs</a></div>
|
||||
</section>
|
||||
</main>
|
||||
|
||||
<div id="toast" class="toast"></div>
|
||||
|
||||
<dialog id="link-modal" class="modal"><button class="modal-close" data-close>×</button><small>connection</small><h2>Link Settings</h2><div class="form-grid two"><label><span>Baud rate</span><select id="baud"><option>9600</option><option>19200</option><option>38400</option><option selected>57600</option><option>115200</option></select></label><label><span>Control rate</span><select id="rate"><option>10</option><option selected>20</option></select></label></div><p class="note">20 Hz is recommended for smooth intermediate commands on this link.</p></dialog>
|
||||
<dialog id="link-modal" class="modal"><button class="modal-close" data-close>×</button><small>connection</small><h2>Link Settings</h2><div class="form-grid two"><label><span>Baud rate</span><select id="baud"><option>9600</option><option>19200</option><option>38400</option><option selected>57600</option><option>115200</option></select></label><label><span>Control rate</span><select id="rate"><option>10</option><option selected>20</option></select></label></div><div style="margin-top:14px;"><label style="display:flex;flex-direction:column;gap:6px;"><span style="font-size:11px;font-weight:500;color:var(--muted)">Custom Port / Endpoint</span><div style="display:flex;gap:8px;"><input id="custom-port-input" placeholder="e.g. COM3 or tcp:127.0.0.1:5760" style="flex:1;height:32px;padding:0 12px;font-size:11px;"><button type="button" id="custom-port-btn" class="button subtle" style="white-space:nowrap;">Use Port</button></div></label></div><p class="note" style="margin-top:14px;">20 Hz is recommended for smooth intermediate commands on this link.</p></dialog>
|
||||
<dialog id="battery-modal" class="modal"><button class="modal-close" data-close>×</button><small>Power</small><h2>Battery Settings</h2><form id="battery-form" class="form-grid three"><label><span>Cells (S)</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="cells" type="number" min="1" step="1"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><label><span>Capacity (mAh)</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="capacity_mah" type="number" min="1" step="50"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><label><span>C rating</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="c_rating" type="number" min="1" step=".1"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><label><span>Full V / cell</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="full_v_per_cell" type="number" step=".01"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><label><span>Empty V / cell</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="empty_v_per_cell" type="number" step=".01"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><label><span>Warning %</span><div class="number-pill"><button type="button" class="num-step down" tabindex="-1">‹</button><input name="warning_percent" type="number" min="0" max="100" step="1"><button type="button" class="num-step up" tabindex="-1">›</button></div></label><div class="modal-actions wide"><button type="button" class="button subtle" data-close>Cancel</button><button class="button connect-button">Save Settings</button></div></form></dialog>
|
||||
<dialog id="motors-modal" class="modal motor-modal"><button class="modal-close" data-close>×</button><small>configuration</small><h2>Motor Outputs</h2><p class="note">Function and direction are read one parameter at a time to protect the low-bandwidth link.</p><div class="motor-head"><span>Output</span><span>Function</span><span>Direction</span><span></span></div><div id="motor-list" class="motor-list"></div></dialog>
|
||||
|
||||
|
||||
+136
-10
@@ -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,
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user