Add reliable built-in demo mode
This commit is contained in:
+52
-6
@@ -42,6 +42,25 @@ class Axes:
|
|||||||
yaw: float = 0.0
|
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:
|
class MavlinkWorker:
|
||||||
def __init__(self, events: queue.Queue):
|
def __init__(self, events: queue.Queue):
|
||||||
self.events = events
|
self.events = events
|
||||||
@@ -74,29 +93,52 @@ class MavlinkWorker:
|
|||||||
self.log_path = None
|
self.log_path = None
|
||||||
|
|
||||||
def connect(self, port: str, baud: int):
|
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():
|
if self.thread and self.thread.is_alive():
|
||||||
|
print(f"[MavlinkWorker] Thread already alive, ignoring connect()")
|
||||||
return
|
return
|
||||||
self.stop_event.clear()
|
self.stop_event.clear()
|
||||||
log_dir = Path(__file__).resolve().parent / "logs"
|
log_dir = Path(__file__).resolve().parent / "logs"
|
||||||
log_dir.mkdir(exist_ok=True)
|
log_dir.mkdir(exist_ok=True)
|
||||||
self.log_path = log_dir / f"ardusub_{datetime.now():%Y%m%d_%H%M%S}.log"
|
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_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.events.put(("log_path", str(self.log_path)))
|
||||||
self.thread = threading.Thread(
|
self.thread = threading.Thread(
|
||||||
target=self._run, args=(port, baud), daemon=True
|
target=self._run, args=(normalized, baud), daemon=True
|
||||||
)
|
)
|
||||||
self.thread.start()
|
self.thread.start()
|
||||||
|
print(f"[MavlinkWorker] worker thread started")
|
||||||
|
|
||||||
def _run(self, port: str, baud: int):
|
def _run(self, port: str, baud: int):
|
||||||
|
print(f"[MavlinkWorker._run] ENTERED: port={port!r}, baud={baud}")
|
||||||
try:
|
try:
|
||||||
|
print(f"[MavlinkWorker._run] Creating mavlink_connection...")
|
||||||
conn = mavutil.mavlink_connection(
|
conn = mavutil.mavlink_connection(
|
||||||
port, baud=baud, source_system=255, source_component=190,
|
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.connection = conn
|
||||||
self.events.put(("status", f"Port open; waiting for heartbeat at {baud} baud…"))
|
if port.startswith("tcp:"):
|
||||||
next_gcs_hb = 0.0
|
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
|
next_control = 0.0
|
||||||
while not self.stop_event.is_set():
|
while not self.stop_event.is_set():
|
||||||
now = time.monotonic()
|
now = time.monotonic()
|
||||||
@@ -120,6 +162,7 @@ class MavlinkWorker:
|
|||||||
else:
|
else:
|
||||||
time.sleep(0.01)
|
time.sleep(0.01)
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
|
print(f"[MavlinkWorker._run] EXCEPTION: {exc}")
|
||||||
self.events.put(("error", f"Connection error: {exc}"))
|
self.events.put(("error", f"Connection error: {exc}"))
|
||||||
finally:
|
finally:
|
||||||
self._safe_stop()
|
self._safe_stop()
|
||||||
@@ -156,7 +199,7 @@ class MavlinkWorker:
|
|||||||
except Exception:
|
except Exception:
|
||||||
payload = str(msg)
|
payload = str(msg)
|
||||||
self.log("RX", payload)
|
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
|
first = not self.target_system
|
||||||
self.target_system = msg.get_srcSystem()
|
self.target_system = msg.get_srcSystem()
|
||||||
self.target_component = msg.get_srcComponent()
|
self.target_component = msg.get_srcComponent()
|
||||||
@@ -887,6 +930,9 @@ class App(tk.Tk):
|
|||||||
|
|
||||||
def refresh_ports(self):
|
def refresh_ports(self):
|
||||||
ports = [p.device for p in list_ports.comports()]
|
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)
|
self.port.configure(values=ports)
|
||||||
if ports and self.port.get() not in ports:
|
if ports and self.port.get() not in ports:
|
||||||
self.port.set(ports[0])
|
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 $=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);
|
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`)});
|
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}))});
|
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)}
|
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;
|
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 populatePorts(ports){
|
||||||
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)}}
|
const p=$('#port');
|
||||||
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)}
|
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)}
|
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;
|
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))}
|
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 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()}
|
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(!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()});
|
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()});
|
addEventListener('blur',()=>{keys.clear();window.joystick3D?.easeBack();if(!heaveDrag)easeHeave()});
|
||||||
|
|
||||||
function openModal(id){
|
function openModal(id){
|
||||||
@@ -119,66 +162,237 @@ if(rovStage){
|
|||||||
if(file)handleStlFile(file);
|
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)}};
|
$('#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'))};
|
||||||
$('#manual').onclick=async()=>{try{await api('command','manual');toast('Manual mode command sent')}catch(e){toast(e.message,true)}};
|
$('#connect').onclick=async()=>{
|
||||||
const armButton=$('#arm');
|
const btn=$('#connect');
|
||||||
let armHoldTimer=null, armStart=0, armCharged=false;
|
const label=btn.querySelector('.btn-label')||btn;
|
||||||
const ARM_HOLD_MS=1000;
|
const p=$('#port');
|
||||||
|
const port=p?p.value:'';
|
||||||
|
|
||||||
function updateArmProgress(){
|
if(appState==='connecting'){
|
||||||
if(!armStart)return;
|
label.textContent='Canceling…';
|
||||||
const elapsed=performance.now()-armStart;
|
try{ await api('disconnect'); }catch(_){}
|
||||||
const progress=Math.min(1,elapsed/ARM_HOLD_MS);
|
return;
|
||||||
armButton.style.setProperty('--arm-progress',`${progress*100}%`);
|
}
|
||||||
|
|
||||||
|
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){
|
if(progress>=1){
|
||||||
armCharged=true;
|
engageCharged=true;
|
||||||
stopArmHold(false);
|
stopEngageHold(false);
|
||||||
armButton.classList.add('charged');
|
engageButton.classList.add('charged');
|
||||||
setTimeout(()=>armButton.classList.remove('charged'),600);
|
setTimeout(()=>engageButton.classList.remove('charged'),600);
|
||||||
api('command','arm').then(()=>toast('Vehicle armed')).catch(e=>toast(e.message,true));
|
api('command','arm').then(()=>toast('Vehicle armed')).catch(e=>toast(e.message,true));
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
armHoldTimer=requestAnimationFrame(updateArmProgress);
|
engageHoldTimer=requestAnimationFrame(updateEngageProgress);
|
||||||
}
|
}
|
||||||
|
|
||||||
function startArmHold(e){
|
function startEngageHold(e){
|
||||||
if(armStart||armCharged)return;
|
if(appState!=='connected_disarmed'||!engageButton?.classList.contains('arm'))return;
|
||||||
|
if(engageStart||engageCharged)return;
|
||||||
if(e&&e.button!==undefined&&e.button!==0)return;
|
if(e&&e.button!==undefined&&e.button!==0)return;
|
||||||
armStart=performance.now();
|
engageStart=performance.now();
|
||||||
armCharged=false;
|
engageCharged=false;
|
||||||
armButton.classList.add('charging');
|
engageButton.classList.add('charging');
|
||||||
armButton.style.setProperty('--arm-progress','0%');
|
engageButton.style.setProperty('--arm-progress','0%');
|
||||||
armHoldTimer=requestAnimationFrame(updateArmProgress);
|
engageHoldTimer=requestAnimationFrame(updateEngageProgress);
|
||||||
}
|
}
|
||||||
|
|
||||||
function stopArmHold(wasCancelled=true){
|
function stopEngageHold(wasCancelled=true){
|
||||||
if(!armStart)return;
|
if(!engageStart)return;
|
||||||
const elapsed=performance.now()-armStart;
|
const elapsed=performance.now()-engageStart;
|
||||||
cancelAnimationFrame(armHoldTimer);
|
cancelAnimationFrame(engageHoldTimer);
|
||||||
armHoldTimer=null;
|
engageHoldTimer=null;
|
||||||
armStart=0;
|
engageStart=0;
|
||||||
armButton.classList.remove('charging');
|
engageButton.classList.remove('charging');
|
||||||
armButton.style.setProperty('--arm-progress','0%');
|
engageButton.style.setProperty('--arm-progress','0%');
|
||||||
if(wasCancelled&&!armCharged&&elapsed<ARM_HOLD_MS&&elapsed>70){
|
if(wasCancelled&&!engageCharged&&elapsed<ENGAGE_HOLD_MS&&elapsed>70){
|
||||||
toast('Hold for 1s to Arm vehicle');
|
toast('Hold for 1s to Engage vehicle');
|
||||||
}
|
}
|
||||||
armCharged=false;
|
engageCharged=false;
|
||||||
}
|
}
|
||||||
|
|
||||||
armButton.addEventListener('pointerdown',startArmHold);
|
if(engageButton){
|
||||||
armButton.addEventListener('pointerup',()=>stopArmHold(true));
|
engageButton.addEventListener('pointerdown',startEngageHold);
|
||||||
armButton.addEventListener('pointerleave',()=>stopArmHold(true));
|
engageButton.addEventListener('pointerup',()=>stopEngageHold(true));
|
||||||
armButton.addEventListener('pointercancel',()=>stopArmHold(true));
|
engageButton.addEventListener('pointerleave',()=>stopEngageHold(true));
|
||||||
armButton.addEventListener('contextmenu',e=>e.preventDefault());
|
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');
|
$('#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)}
|
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);
|
$('#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)}};
|
$('#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 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 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)}
|
async function poll(){if(!bridgeReady)return;try{render(await api('poll'))}catch(e){}setTimeout(poll,250)}
|
||||||
let modelLoadPromise=null;
|
let modelLoadPromise=null;
|
||||||
@@ -259,7 +473,6 @@ async function boot(){
|
|||||||
if(booted||!window.pywebview?.api)return;
|
if(booted||!window.pywebview?.api)return;
|
||||||
booted=true;
|
booted=true;
|
||||||
bridgeReady=true;
|
bridgeReady=true;
|
||||||
loadDefaultModel();
|
|
||||||
try{
|
try{
|
||||||
const init=await api('initial_state');
|
const init=await api('initial_state');
|
||||||
batteryConfig=init.battery_config;
|
batteryConfig=init.battery_config;
|
||||||
@@ -267,8 +480,11 @@ async function boot(){
|
|||||||
fillBatteryForm();
|
fillBatteryForm();
|
||||||
render(init.state);
|
render(init.state);
|
||||||
poll();
|
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()=>{
|
setInterval(async()=>{
|
||||||
if(!connected){
|
if(appState==='disconnected'){
|
||||||
try{populatePorts(await api('refresh_ports'))}catch(_){}
|
try{populatePorts(await api('refresh_ports'))}catch(_){}
|
||||||
}
|
}
|
||||||
},2000);
|
},2000);
|
||||||
|
|||||||
@@ -22,23 +22,24 @@
|
|||||||
<input id="model-input" type="file" accept=".stl,.STL" hidden>
|
<input id="model-input" type="file" accept=".stl,.STL" hidden>
|
||||||
</div>
|
</div>
|
||||||
</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">
|
<div class="rail-bottom">
|
||||||
<button class="battery-card ghost" data-open="battery-modal">
|
<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>
|
<span><small>Battery</small><strong id="battery-title">—</strong></span><i id="battery-percent">waiting</i>
|
||||||
<b><em id="battery-fill"></em></b>
|
<b><em id="battery-fill"></em></b>
|
||||||
</button>
|
</button>
|
||||||
<div class="link-state"><span id="link-dot"></span><strong id="link">disconnected</strong></div>
|
|
||||||
</div>
|
</div>
|
||||||
</aside>
|
</aside>
|
||||||
|
|
||||||
<section class="workspace">
|
<section class="workspace">
|
||||||
<header class="topbar">
|
<header class="topbar">
|
||||||
<div class="connection">
|
<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 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 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>
|
||||||
<div class="topbar-spacer pywebview-drag-region"></div>
|
<div class="topbar-spacer pywebview-drag-region"></div>
|
||||||
<div class="window-actions">
|
<div class="window-actions">
|
||||||
@@ -71,21 +72,19 @@
|
|||||||
<section class="bottom-actions">
|
<section class="bottom-actions">
|
||||||
<section class="command-bar">
|
<section class="command-bar">
|
||||||
<div class="commands">
|
<div class="commands">
|
||||||
<button id="manual" class="button subtle"><span>Manual</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>
|
||||||
<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>
|
|
||||||
</div>
|
</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>
|
<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>
|
<button class="button subtle" data-open="motors-modal">Motor Setup</button>
|
||||||
</section>
|
</section>
|
||||||
</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>
|
</section>
|
||||||
</main>
|
</main>
|
||||||
|
|
||||||
<div id="toast" class="toast"></div>
|
<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="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>
|
<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):
|
def __init__(self):
|
||||||
self.events: queue.Queue = queue.Queue()
|
self.events: queue.Queue = queue.Queue()
|
||||||
self.worker = MavlinkWorker(self.events)
|
self.worker = MavlinkWorker(self.events)
|
||||||
|
self.demo_mode = False
|
||||||
self.lock = threading.Lock()
|
self.lock = threading.Lock()
|
||||||
self.state = {
|
self.state = {
|
||||||
"connected": False,
|
"connected": False,
|
||||||
"link": "DISCONNECTED",
|
"link": "DISCONNECTED",
|
||||||
"vehicle": "No heartbeat",
|
"vehicle": "No heartbeat",
|
||||||
"armed": False,
|
"armed": False,
|
||||||
|
"mode": None,
|
||||||
|
"app_state": "disconnected",
|
||||||
"status": "Ready. Select a USB port to begin.",
|
"status": "Ready. Select a USB port to begin.",
|
||||||
"battery": None,
|
"battery": None,
|
||||||
"log_path": None,
|
"log_path": None,
|
||||||
@@ -57,14 +60,26 @@ class ControlApi:
|
|||||||
|
|
||||||
def initial_state(self):
|
def initial_state(self):
|
||||||
return {
|
return {
|
||||||
"ports": [p.device for p in list_ports.comports()],
|
"ports": self.refresh_ports(),
|
||||||
"battery_config": self.battery_config,
|
"battery_config": self.battery_config,
|
||||||
"state": self._snapshot(),
|
"state": self._snapshot(),
|
||||||
"model_info": self.get_saved_model_info(),
|
"model_info": self.get_saved_model_info(),
|
||||||
}
|
}
|
||||||
|
|
||||||
def refresh_ports(self):
|
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):
|
def minimize_window(self):
|
||||||
if self._window:
|
if self._window:
|
||||||
@@ -202,22 +217,69 @@ class ControlApi:
|
|||||||
return None
|
return None
|
||||||
|
|
||||||
def connect(self, port, baud=57600, rate=20):
|
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:
|
if not port:
|
||||||
raise ValueError("Select a USB serial 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.rate_hz = int(rate)
|
||||||
self.worker.connect(str(port), int(baud))
|
self.worker.connect(str(port), int(baud))
|
||||||
with self.lock:
|
with self.lock:
|
||||||
self.state.update(
|
self.state.update(
|
||||||
connected=True, link="CONNECTING",
|
connected=True, link="CONNECTING",
|
||||||
status=f"Opening {port} at {baud} baud…",
|
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):
|
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()
|
self.worker.disconnect()
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def set_axes(self, forward, turn, vertical):
|
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(
|
self.worker.set_axes(Axes(
|
||||||
max(-1.0, min(1.0, float(forward))),
|
max(-1.0, min(1.0, float(forward))),
|
||||||
0.0,
|
0.0,
|
||||||
@@ -232,6 +294,15 @@ class ControlApi:
|
|||||||
return value
|
return value
|
||||||
|
|
||||||
def command(self, name):
|
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 not self.worker.connection or not self.worker.target_system:
|
||||||
if name == "manual":
|
if name == "manual":
|
||||||
raise RuntimeError("Connect to vehicle to enable Manual mode")
|
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:
|
if not 1 <= servo <= 16 or not 0 <= motor <= 12:
|
||||||
raise ValueError("Invalid servo or motor")
|
raise ValueError("Invalid servo or motor")
|
||||||
function = 0 if motor == 0 else (32 + motor if motor <= 8 else 73 + 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)
|
self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
@@ -269,6 +344,10 @@ class ControlApi:
|
|||||||
current = self.motor_directions.get(motor)
|
current = self.motor_directions.get(motor)
|
||||||
if current is None:
|
if current is None:
|
||||||
raise ValueError(f"MOT_{motor}_DIRECTION has not been read yet")
|
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)
|
self.worker.set_parameter(f"MOT_{motor}_DIRECTION", -1 if current == 1 else 1)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
@@ -312,9 +391,24 @@ class ControlApi:
|
|||||||
)
|
)
|
||||||
return self._snapshot()
|
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):
|
def _snapshot(self):
|
||||||
with self.lock:
|
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):
|
def _consume(self, event):
|
||||||
kind = event[0]
|
kind = event[0]
|
||||||
@@ -324,6 +418,7 @@ class ControlApi:
|
|||||||
self.state.update(
|
self.state.update(
|
||||||
connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}",
|
connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}",
|
||||||
armed=bool(armed),
|
armed=bool(armed),
|
||||||
|
mode=str(mode).upper(),
|
||||||
)
|
)
|
||||||
elif kind in ("status", "statustext"):
|
elif kind in ("status", "statustext"):
|
||||||
self.state["status"] = event[1]
|
self.state["status"] = event[1]
|
||||||
@@ -355,9 +450,13 @@ class ControlApi:
|
|||||||
self.state["log_path"] = event[1]
|
self.state["log_path"] = event[1]
|
||||||
elif kind == "error":
|
elif kind == "error":
|
||||||
self.state["status"] = event[1]
|
self.state["status"] = event[1]
|
||||||
|
self.state["last_error"] = event[1]
|
||||||
elif kind == "disconnected":
|
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",
|
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):
|
def _update_servo(self, servo):
|
||||||
function = self.servo_functions.get(servo)
|
function = self.servo_functions.get(servo)
|
||||||
@@ -380,22 +479,49 @@ def _apply_dwm_styling(window):
|
|||||||
hwnd = int(window.native.Handle.ToInt64())
|
hwnd = int(window.native.Handle.ToInt64())
|
||||||
if hwnd:
|
if hwnd:
|
||||||
import ctypes
|
import ctypes
|
||||||
|
user32 = ctypes.windll.user32
|
||||||
|
dwmapi = ctypes.windll.dwmapi
|
||||||
corner_pref = ctypes.c_int(2) # DWMWCP_ROUND
|
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)
|
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
|
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)
|
ctypes.c_void_p(hwnd), 20, ctypes.byref(dark_mode), ctypes.sizeof(dark_mode)
|
||||||
)
|
)
|
||||||
|
user32.SetForegroundWindow(ctypes.c_void_p(hwnd))
|
||||||
except Exception:
|
except Exception:
|
||||||
pass
|
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):
|
class AppHTTPHandler(http.server.SimpleHTTPRequestHandler):
|
||||||
def log_message(self, format, *args):
|
def log_message(self, format, *args):
|
||||||
pass # Suppress HTTP request logs in console
|
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):
|
def do_GET(self):
|
||||||
unquoted = urllib.parse.unquote(self.path)
|
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:
|
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-Type", "model/stl")
|
||||||
self.send_header("Content-Length", str(len(data)))
|
self.send_header("Content-Length", str(len(data)))
|
||||||
self.send_header("Access-Control-Allow-Origin", "*")
|
self.send_header("Access-Control-Allow-Origin", "*")
|
||||||
self.send_header("Cache-Control", "no-cache")
|
|
||||||
self.end_headers()
|
self.end_headers()
|
||||||
self.wfile.write(data)
|
self.wfile.write(data)
|
||||||
return
|
return
|
||||||
@@ -429,10 +554,11 @@ def main():
|
|||||||
api._migrate_legacy_model()
|
api._migrate_legacy_model()
|
||||||
project_root = Path(__file__).resolve().parent
|
project_root = Path(__file__).resolve().parent
|
||||||
port = start_local_server(project_root)
|
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(
|
window = webview.create_window(
|
||||||
"SUB Control", url=app_url, js_api=api,
|
"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,
|
background_color="#111719", frameless=True, easy_drag=False,
|
||||||
shadow=True,
|
shadow=True,
|
||||||
)
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user