commit 2f6077f5d80ac4f7eef134f49029a3062a7f0f45 Author: PitresAli Date: Tue Aug 25 22:13:54 2026 +0200 Initial SUB Control application diff --git a/SUB Control/.gitignore b/SUB Control/.gitignore new file mode 100644 index 0000000..6dbe475 --- /dev/null +++ b/SUB Control/.gitignore @@ -0,0 +1,5 @@ +.venv/ +__pycache__/ +*.py[cod] +logs/ +model_settings.json diff --git a/SUB Control/README.md b/SUB Control/README.md new file mode 100644 index 0000000..00d27f8 --- /dev/null +++ b/SUB Control/README.md @@ -0,0 +1,102 @@ +# ArduSub Minimal Control + +A deliberately small, low-bandwidth MAVLink controller for bench-testing an +ArduSub vehicle over a USB serial connection. It sends no parameter-list +request and no telemetry-stream request. + +The desktop window uses a local HTML/CSS/JavaScript interface rendered by the +Windows WebView. Python remains the only launcher and owns the MAVLink worker, +arming, logging, output limiting, and parameter writes. It does not run a web +server, open a browser tab, or require an internet connection. + +The WebView is frameless and uses the compact title bar drawn by the interface. +The vehicle rail includes a local STL viewer: choose **Load STL**, then drag to +orbit, use the mouse wheel to zoom, or switch between shaded and edge-focused +rendering. The chosen file is remembered in `model_settings.json`; only its +local path is stored. STEP/STP CAD solids must first be tessellated/exported as +STL because the embedded renderer intentionally does not include a full CAD +kernel. + +## Safety first + +- Remove propellers for initial tests and restrain the vehicle. +- Verify the ArduSub frame and `SERVOx_FUNCTION`, motor direction, ESC range, + leak/failsafe, and emergency-stop arrangements before arming. +- The application starts disarmed and limits pilot demand to 20% by default. + Once armed, control movement is sent directly with no additional gate. +- Closing or disconnecting sends neutral controls and a disarm request, but a + physical power cutoff is still required. + +The controls are pilot-demand axes, not direct PWM values. ArduSub performs +the frame mixing and generates the configured ESC outputs. For ArduSub, +`MANUAL_CONTROL.z = 500` is neutral vertical thrust. + +## Run on Windows + +1. Install Python 3.11 or newer. +2. Open PowerShell in this folder and run `./run.ps1`. +3. Select the autopilot's COM port, then click **Connect**. Baud and control-rate + settings are under the small `•••` link button (normally 57600 baud / 20 Hz). +4. Confirm `ArduSub` and a live heartbeat are shown. +5. Click **Set MANUAL**, then **ARM** only when the bench is safe. +6. Move the controls. Once armed, no additional enable button is needed. + +Keyboard controls for SimpleROV-3: W/S forward/back, A/D yaw left/right, +Q/E alternative yaw control, and R/F rise/dive. +Releasing movement keys eases their controls back to neutral. + +## Three-thruster limitation + +The common ArduSub `SimpleROV-3` arrangement controls forward, yaw, and depth. +It cannot independently control lateral, roll, or pitch. Accordingly, the main +pad is mapped to forward/yaw, while the vertical slider controls depth thrust. + +## Link behavior + +- MAVLink GCS heartbeat: 1 Hz +- `MANUAL_CONTROL`: selectable 10 or 20 Hz (default 20 Hz). Lower rates are + intentionally excluded because they make short return ramps become abrupt + steps at the flight controller. +- No full parameter download +- On connection, inherited bulk telemetry streams are stopped. Only + `SYS_STATUS` at 1 Hz and `SERVO_OUTPUT_RAW` / `RC_CHANNELS` at 2 Hz are + requested, in addition to the normal heartbeat and event-driven status text. +- Arm/disarm uses `MAV_CMD_COMPONENT_ARM_DISARM` and displays `COMMAND_ACK` +- Link health is based on received autopilot heartbeats +- Battery voltage is read from `SYS_STATUS` at 1 Hz and shown as pack voltage, + estimated per-cell voltage, and a voltage-only percentage. Cell count, + capacity, C rating, full/empty cell voltages, and warning percentage are + configurable and saved locally in `battery_settings.json`. Capacity consumed + cannot be measured without a current sensor. +- Servo configuration scans `SERVO1_FUNCTION` through `SERVO16_FUNCTION` and + ArduSub's `MOT_1_DIRECTION` through `MOT_12_DIRECTION` one at a time, waiting + for each response and retrying missed replies. A selected output can be + assigned to Motor 1–12 or set unassigned. Motor direction is toggled with the + corresponding `MOT_n_DIRECTION` value while disarmed; `SERVOx_REVERSED` is + intentionally not used for Motor functions. It never downloads the full + parameter list. + The diagnostic scan also reads `SERVO1`–`SERVO3` MIN/TRIM/MAX and ArduSub's + joystick gain values into the local session log; these values remain + read-only in the application. +- Every session creates a local text log under `logs/`, including operator + actions, outgoing manual-control values and commands, acknowledgements, + status text, and all received MAVLink messages. This does not use ArduPilot's + onboard logger or add any link traffic. + +The car-style virtual joystick maps only to forward/reverse and continuous +differential turning; it never sends lateral motion. The joystick and vertical +slider ease back to center over 400 ms after mouse/key release, so neither can +be left commanding motion. +Heartbeat loss, disconnect, and disarm remain immediate safety stops and are +intentionally not eased. + +In addition, transmitted pilot demands have a small 4% input deadband and a +0.8-second full-range slew limit. This makes both acceleration and deceleration +arrive at the flight controller as a sequence of stable intermediate values. +Forward and yaw are normalized as a SimpleROV-3 motor pair before the output +limit is applied, preventing their mixer contributions from adding beyond the +selected horizontal limit. + +ArduPilot may ignore manual input when `MAV_GCS_SYSID` does not match this +application's system ID (default 255), or when `MAV_OPTIONS` does not allow +MAVLink input. Those parameters must already be correctly configured. diff --git a/SUB Control/app.py b/SUB Control/app.py new file mode 100644 index 0000000..8fd3fdc --- /dev/null +++ b/SUB Control/app.py @@ -0,0 +1,1219 @@ +from __future__ import annotations + +import queue +import json +import os +import threading +import time +import tkinter as tk +import customtkinter as ctk +from collections import deque +from dataclasses import dataclass +from datetime import datetime +from pathlib import Path +from tkinter import messagebox, ttk + +from pymavlink import mavutil +from serial.tools import list_ports + + +NEUTRAL_Z = 500 # ArduSub legacy MANUAL_CONTROL convention: 0..1000. +MOTOR_FUNCTIONS = { + **{33 + index: f"Motor {index + 1}" for index in range(8)}, + **{82 + index: f"Motor {index + 9}" for index in range(4)}, +} + + +def motor_number_for_function(function: int | None) -> int | None: + if function is None: + return None + if 33 <= function <= 40: + return function - 32 + if 82 <= function <= 85: + return function - 73 + return None + + +@dataclass +class Axes: + forward: float = 0.0 + lateral: float = 0.0 + vertical: float = 0.0 + yaw: float = 0.0 + + +class MavlinkWorker: + def __init__(self, events: queue.Queue): + self.events = events + self.connection = None + self.stop_event = threading.Event() + self.lock = threading.Lock() + self.thread = None + self.axes = Axes() + self.output_axes = Axes() + self.last_control_at = 0.0 + # Smooth ESC commands without adding a perceptible one-second lag. + # At 20 Hz this produces five small steps from neutral to full command. + self.ramp_seconds = 0.25 + self.gain = 0.20 + self.rate_hz = 20 + self.target_system = 0 + self.target_component = 0 + self.last_heartbeat = 0.0 + self.last_rx = 0.0 + self.armed = False + self.config_paused = False + self.param_scan = deque() + self.param_waiting = None + self.param_deadline = 0.0 + self.param_retries = 0 + self.param_types = {} + self.pending_sets = {} + self.log_lock = threading.Lock() + self.log_file = None + self.log_path = None + + def connect(self, port: str, baud: int): + if self.thread and self.thread.is_alive(): + 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}") + self.events.put(("log_path", str(self.log_path))) + self.thread = threading.Thread( + target=self._run, args=(port, baud), daemon=True + ) + self.thread.start() + + def _run(self, port: str, baud: int): + try: + conn = mavutil.mavlink_connection( + port, baud=baud, source_system=255, source_component=190, + autoreconnect=False, force_connected=True, + ) + self.connection = conn + self.events.put(("status", f"Port open; waiting for heartbeat at {baud} baud…")) + next_gcs_hb = 0.0 + next_control = 0.0 + while not self.stop_event.is_set(): + now = time.monotonic() + if now >= next_gcs_hb: + self.log("TX_HEARTBEAT", "GCS") + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, 0, mavutil.mavlink.MAV_STATE_ACTIVE, + ) + next_gcs_hb = now + 1.0 + if self.target_system and now >= next_control: + self._send_control() + next_control = now + (1.0 / max(1, self.rate_hz)) + if self.target_system: + self._tick_param_scan(now) + + msg = conn.recv_match(blocking=False) + if msg: + self._handle_message(msg) + else: + time.sleep(0.01) + except Exception as exc: + self.events.put(("error", f"Connection error: {exc}")) + finally: + self._safe_stop() + if self.connection: + try: + self.connection.close() + except Exception: + pass + self.connection = None + self.target_system = 0 + self.armed = False + self.config_paused = False + self.log("SESSION", "disconnected") + with self.log_lock: + if self.log_file: + self.log_file.close() + self.log_file = None + self.events.put(("disconnected", "Disconnected")) + + def log(self, category: str, message: str): + timestamp = datetime.now().astimezone().isoformat(timespec="milliseconds") + with self.log_lock: + if self.log_file: + self.log_file.write(f"{timestamp} [{category}] {message}\n") + + def _handle_message(self, msg): + msg_type = msg.get_type() + if msg_type == "BAD_DATA": + self.log("RX_BAD_DATA", repr(getattr(msg, "data", b""))) + return + self.last_rx = time.monotonic() + try: + payload = json.dumps(msg.to_dict(), default=str, separators=(",", ":")) + except Exception: + payload = str(msg) + self.log("RX", payload) + if msg_type == "HEARTBEAT" and msg.get_srcComponent() == 1: + first = not self.target_system + self.target_system = msg.get_srcSystem() + self.target_component = msg.get_srcComponent() + self.last_heartbeat = time.monotonic() + armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + self.armed = armed + self.config_paused = armed + try: + mode = mavutil.mode_string_v10(msg) + except Exception: + mode = str(msg.custom_mode) + autopilot = mavutil.mavlink.enums["MAV_AUTOPILOT"].get(msg.autopilot) + ap_name = autopilot.name if autopilot else str(msg.autopilot) + self.events.put(("heartbeat", self.target_system, ap_name, mode, armed)) + if first: + self.events.put(("status", "Heartbeat received; controls are neutral.")) + self._configure_low_bandwidth_streams() + self._start_servo_scan() + elif msg_type == "COMMAND_ACK": + result = mavutil.mavlink.enums["MAV_RESULT"].get(msg.result) + result_name = result.name if result else str(msg.result) + self.events.put(("ack", int(msg.command), result_name)) + elif msg_type == "STATUSTEXT": + text = msg.text + if isinstance(text, bytes): + text = text.decode(errors="replace") + self.events.put(("statustext", text.rstrip("\x00"))) + elif msg_type == "SYS_STATUS": + voltage_mv = int(msg.voltage_battery) + if 0 < voltage_mv < 65535: + self.events.put(("battery", voltage_mv / 1000.0)) + elif msg_type == "PARAM_VALUE": + param_id = msg.param_id + if isinstance(param_id, bytes): + param_id = param_id.decode(errors="replace") + param_id = param_id.rstrip("\x00") + self.param_types[param_id] = msg.param_type + if param_id == self.param_waiting: + self.param_waiting = None + self.param_deadline = time.monotonic() + 0.10 + if param_id in self.pending_sets: + wanted = self.pending_sets[param_id] + if int(round(msg.param_value)) == wanted: + del self.pending_sets[param_id] + self.events.put(("param_saved", param_id, wanted)) + if param_id.startswith("SERVO") and param_id.endswith("_FUNCTION"): + self.events.put(("servo_param", param_id, int(round(msg.param_value)))) + elif param_id.startswith("MOT_") and param_id.endswith("_DIRECTION"): + parts = param_id.split("_") + if len(parts) == 3 and parts[1].isdigit(): + self.events.put(("motor_direction", int(parts[1]), + int(round(msg.param_value)))) + + def _start_servo_scan(self): + names = [f"SERVO{number}_FUNCTION" for number in range(1, 17)] + # ArduSub reverses mixer motors with MOT_n_DIRECTION (1 normal, + # -1 reversed). SERVOx_REVERSED does not reverse Motor functions. + names.extend(f"MOT_{number}_DIRECTION" for number in range(1, 13)) + # Capture the exact PWM scaling that affects the three SimpleROV-3 + # outputs, plus ArduSub's own joystick gain. These are read-only here + # and make direction asymmetry diagnosable from the session log. + names.extend( + f"SERVO{number}_{suffix}" + for number in range(1, 4) + for suffix in ("MIN", "TRIM", "MAX") + ) + names.extend(("JS_GAIN_MIN", "JS_GAIN_DEFAULT", "JS_GAIN_MAX")) + self.param_scan = deque(names) + self.param_waiting = None + self.param_deadline = time.monotonic() + self.param_retries = 0 + + def _configure_low_bandwidth_streams(self): + """Stop inherited high-rate streams and request only diagnostic essentials.""" + if not self.connection: + return + self.log("TX_STREAM_CONFIG", "stop MAV_DATA_STREAM_ALL") + self.connection.mav.request_data_stream_send( + self.target_system, self.target_component, + mavutil.mavlink.MAV_DATA_STREAM_ALL, 0, 0, + ) + # Heartbeat and STATUSTEXT do not need to be requested. Keep only + # output/input evidence needed to diagnose controls, at 2 Hz each. + for message_id, interval_us, name in ( + (mavutil.mavlink.MAVLINK_MSG_ID_SYS_STATUS, 1_000_000, "SYS_STATUS"), + (mavutil.mavlink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500_000, "SERVO_OUTPUT_RAW"), + (mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS, 500_000, "RC_CHANNELS"), + ): + self.log("TX_STREAM_CONFIG", f"{name} interval_us={interval_us}") + self.connection.mav.command_long_send( + self.target_system, self.target_component, + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + 0, message_id, interval_us, 0, 0, 0, 0, 0, + ) + self.events.put(("status", "Low-bandwidth telemetry configured; reading servo setup…")) + + def _tick_param_scan(self, now): + if not self.connection or self.config_paused or now < self.param_deadline: + return + if self.param_waiting is not None: + if self.param_retries < 2: + self.param_retries += 1 + self._request_param(self.param_waiting) + self.param_deadline = now + 1.5 + return + self.events.put(("param_missing", self.param_waiting)) + self.param_waiting = None + if self.param_scan: + self.param_waiting = self.param_scan.popleft() + self.param_retries = 0 + self._request_param(self.param_waiting) + self.param_deadline = now + 1.5 + elif self.param_waiting is None: + self.events.put(("param_scan_complete",)) + self.param_deadline = float("inf") + + def _request_param(self, name: str): + self.log("TX_PARAM_REQUEST", name) + self.connection.mav.param_request_read_send( + self.target_system, self.target_component, name.encode("ascii"), -1 + ) + + def set_parameter(self, name: str, value: int): + if not self.connection or not self.target_system: + raise RuntimeError("No autopilot heartbeat") + if self.armed or self.config_paused: + raise RuntimeError("Disarm before changing motor configuration") + param_type = self.param_types.get(name) + if param_type is None: + raise RuntimeError(f"{name} has not been read from the autopilot") + self.pending_sets[name] = int(value) + self.log("TX_PARAM_SET", f"{name}={value} type={param_type}") + self.connection.mav.param_set_send( + self.target_system, self.target_component, name.encode("ascii"), + float(value), param_type, + ) + self.events.put(("status", f"Writing {name}={value}; waiting for confirmation…")) + + def _send_control(self, force_neutral: bool = False): + conn = self.connection + if not conn or not self.target_system: + return + with self.lock: + active = not force_neutral + target = self.axes if active else Axes() + gain = self.gain + now = time.monotonic() + if force_neutral: + self.output_axes = Axes() + else: + dt = (now - self.last_control_at) if self.last_control_at else (1.0 / max(1, self.rate_hz)) + max_change = min(1.0, dt / max(0.1, self.ramp_seconds)) + self.output_axes = Axes( + self._approach(self.output_axes.forward, target.forward, max_change), + self._approach(self.output_axes.lateral, target.lateral, max_change), + self._approach(self.output_axes.vertical, target.vertical, max_change), + self._approach(self.output_axes.yaw, target.yaw, max_change), + ) + self.last_control_at = now + axes = self.output_axes + # SimpleROV-3 mixes horizontal motors from forward +/- yaw. Normalize + # the pair so combining the two axes cannot exceed the selected limit. + horizontal_mix_peak = max( + 1.0, + abs(axes.forward + axes.yaw), + abs(axes.forward - axes.yaw), + ) + limited_forward = axes.forward / horizontal_mix_peak + limited_yaw = axes.yaw / horizontal_mix_peak + x = round(limited_forward * gain * 1000) + y = round(axes.lateral * gain * 1000) + r = round(limited_yaw * gain * 1000) + z = round(NEUTRAL_Z + axes.vertical * gain * 500) + self.log( + "TX_MANUAL_CONTROL", + f"x={x} y={y} z={z} r={r} active={active} " + f"target=({target.forward:.3f},{target.lateral:.3f},{target.vertical:.3f},{target.yaw:.3f})", + ) + conn.mav.manual_control_send(self.target_system, x, y, z, r, 0) + + @staticmethod + def _approach(current: float, target: float, max_change: float) -> float: + difference = target - current + if abs(difference) <= max_change: + return target + return current + (max_change if difference > 0 else -max_change) + + def command_arm(self, arm: bool): + conn = self.connection + if not conn or not self.target_system: + raise RuntimeError("No autopilot heartbeat") + if not arm: + self._send_control(force_neutral=True) + else: + # Give the ARM request priority over the low-rate parameter scan. + self.config_paused = True + self.log("TX_COMMAND", "ARM" if arm else "DISARM") + conn.mav.command_long_send( + self.target_system, self.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1.0 if arm else 0.0, 0, 0, 0, 0, 0, 0, + ) + + def set_manual_mode(self): + conn = self.connection + if not conn or not self.target_system: + raise RuntimeError("No autopilot heartbeat") + modes = conn.mode_mapping() or {} + manual = modes.get("MANUAL") + if manual is None: + raise RuntimeError("MANUAL mode is not advertised by this firmware") + self.log("TX_MODE", f"MANUAL custom_mode={manual}") + conn.mav.set_mode_send( + self.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + manual, + ) + + def set_axes(self, axes: Axes): + def conditioned(value): + # Ignore tiny pointer/key return noise, then quantize to stable 1% steps. + if abs(value) < 0.04: + return 0.0 + return round(max(-1.0, min(1.0, value)), 2) + + with self.lock: + self.axes = Axes( + conditioned(axes.forward), conditioned(axes.lateral), + conditioned(axes.vertical), conditioned(axes.yaw), + ) + + def disconnect(self): + self._send_control(force_neutral=True) + if self.connection and self.target_system: + try: + self.command_arm(False) + except Exception: + pass + self.stop_event.set() + + def _safe_stop(self): + if self.connection and self.target_system: + try: + self._send_control(force_neutral=True) + self.connection.mav.command_long_send( + self.target_system, self.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 0, 0, 0, 0, 0, 0, 0, + ) + except Exception: + pass + + +class Joystick(tk.Canvas): + def __init__(self, master, callback, **kwargs): + self.size = 276 + self.center_point = self.size / 2 + self.radius = 108 + self.knob_radius = 24 + super().__init__(master, width=self.size, height=self.size, bg="#f5f5f2", + highlightthickness=0, bd=0, **kwargs) + self.callback = callback + self.value = (0.0, 0.0) + self.return_job = None + c, r, k = self.center_point, self.radius, self.knob_radius + self.create_oval(c-r, c-r, c+r, c+r, fill="#e9e9e5", outline="#d8d8d2", width=2) + self.create_oval(c-70, c-70, c+70, c+70, outline="#deded8", width=1) + self.create_line(c, c-r+12, c, c+r-12, fill="#d2d2cc", dash=(3, 6)) + self.create_line(c-r+12, c, c+r-12, c, fill="#d2d2cc", dash=(3, 6)) + self.create_text(c, 13, text="FORWARD", fill="#8a8a84", font=("Helvetica", 8, "bold")) + self.create_text(c, self.size-13, text="REVERSE", fill="#8a8a84", font=("Helvetica", 8, "bold")) + self.create_text(25, c, text="TURN\nLEFT", fill="#8a8a84", font=("Helvetica", 8, "bold")) + self.create_text(self.size-25, c, text="TURN\nRIGHT", fill="#8a8a84", font=("Helvetica", 8, "bold")) + self.knob = self.create_oval(c-k, c-k, c+k, c+k, + fill="#575c61", outline="#42474b", width=2) + self.bind("", self._move) + self.bind("", self._move) + self.bind("", self._release) + + def _move(self, event): + if self.return_job is not None: + self.after_cancel(self.return_job) + self.return_job = None + dx, dy = event.x - self.center_point, event.y - self.center_point + length = max(1.0, (dx * dx + dy * dy) ** 0.5) + if length > self.radius: + dx, dy = dx * self.radius / length, dy * self.radius / length + self.set_immediate(dx / self.radius, -dy / self.radius) + + def set_immediate(self, x: float, y: float): + if self.return_job is not None: + self.after_cancel(self.return_job) + self.return_job = None + self.value = (x, y) + c, k = self.center_point, self.knob_radius + dx, dy = x * self.radius, -y * self.radius + self.coords(self.knob, c-k+dx, c-k+dy, c+k+dx, c+k+dy) + self.callback() + + def _release(self, _event=None): + start_x, start_y = self.value + started = time.monotonic() + duration = 0.40 + + def ease_back(): + elapsed = time.monotonic() - started + progress = min(1.0, elapsed / duration) + # Smoothstep has zero slope at both ends, avoiding a motor kick. + eased = progress * progress * (3.0 - 2.0 * progress) + remaining = 1.0 - eased + x, y = start_x * remaining, start_y * remaining + self.value = (x, y) + c, k = self.center_point, self.knob_radius + dx, dy = x * self.radius, -y * self.radius + self.coords(self.knob, c-k+dx, c-k+dy, c+k+dx, c+k+dy) + self.callback() + if progress < 1.0: + self.return_job = self.after(20, ease_back) + else: + self.return_job = None + + if self.return_job is not None: + self.after_cancel(self.return_job) + ease_back() + + def center(self): + if self.return_job is not None: + self.after_cancel(self.return_job) + self.return_job = None + self.value = (0.0, 0.0) + c, k = self.center_point, self.knob_radius + self.coords(self.knob, c-k, c-k, c+k, c+k) + self.callback() + + +class SpringScale(ctk.CTkSlider): + """A Scale that smoothly returns to zero when the pointer is released.""" + + def __init__(self, master, **kwargs): + self.callback = kwargs.pop("command", None) + kwargs.pop("showvalue", None) + kwargs.pop("bg", None) + kwargs.pop("troughcolor", None) + kwargs.pop("activebackground", None) + kwargs.pop("highlightthickness", None) + kwargs.pop("bd", None) + kwargs.pop("sliderrelief", None) + if "orient" in kwargs: + kwargs["orientation"] = kwargs.pop("orient") + length = kwargs.pop("length", 200) + if kwargs.get("orientation", "horizontal") == "vertical": + kwargs.setdefault("height", length) + kwargs.setdefault("width", 18) + else: + kwargs.setdefault("width", length) + kwargs.setdefault("height", 18) + super().__init__(master, command=self._changed, corner_radius=10, + button_corner_radius=10, button_length=22, + fg_color="#dbe3ee", progress_color="#60a5fa", + button_color="#2563eb", button_hover_color="#1d4ed8", + **kwargs) + self.return_job = None + self.bind("", self._cancel_return, add="+") + self.bind("", self._released, add="+") + + def _changed(self, value): + if self.callback: + self.callback(value) + + def _set_and_notify(self, value): + self.set(value) + if self.callback: + self.callback(value) + + def _cancel_return(self, _event=None): + if self.return_job is not None: + self.after_cancel(self.return_job) + self.return_job = None + + def _released(self, _event=None): + # Wait until Tk's native Scale binding has applied its final value. + self.after_idle(self.spring_to_center) + + def set_immediate(self, value): + self._cancel_return() + self._set_and_notify(value) + + def spring_to_center(self): + self._cancel_return() + start = float(self.get()) + if abs(start) < 0.5: + self._set_and_notify(0) + return + started = time.monotonic() + duration = 0.40 + + def ease_back(): + progress = min(1.0, (time.monotonic() - started) / duration) + eased = progress * progress * (3.0 - 2.0 * progress) + self._set_and_notify(start * (1.0 - eased)) + if progress < 1.0: + self.return_job = self.after(20, ease_back) + else: + self._set_and_notify(0) + self.return_job = None + + ease_back() + + +class App(tk.Tk): + def __init__(self): + super().__init__() + self.title("ArduSub Minimal Control") + self.geometry("1040x800") + self.minsize(920, 720) + self.events = queue.Queue() + self.worker = MavlinkWorker(self.events) + self.keys = set() + self.keyboard_yaw = 0 + self.keyboard_vertical = 0 + self.keyboard_planar = (0, 0) + self.servo_functions = {} + self.motor_directions = {} + self.battery_config_path = Path(__file__).resolve().parent / "battery_settings.json" + self.battery_config = { + "cells": 4, + "capacity_mah": 5500, + "c_rating": 35, + "full_v_per_cell": 4.20, + "empty_v_per_cell": 3.30, + "warning_percent": 20, + } + self.battery_voltage = None + self.battery_voltage_smoothed = None + self._load_battery_settings() + self._configure_styles() + self.protocol("WM_DELETE_WINDOW", self.close) + self._build() + self.refresh_ports() + self.bind_all("", self._key_down) + self.bind_all("", self._key_up) + self.after(50, self._poll) + self.after(500, self._link_watchdog) + + def _configure_styles(self): + self.configure(bg="#e8e9e7") + style = ttk.Style(self) + try: + style.theme_use("clam") + except tk.TclError: + pass + style.configure("TFrame", background="#fbfbf9") + style.configure("TLabel", background="#fbfbf9", foreground="#202224", + font=("Helvetica", 9)) + style.configure("TLabelframe", background="#f4f6f8", bordercolor="#d8dee8", + borderwidth=1, relief="solid") + style.configure("TLabelframe.Label", background="#f4f6f8", foreground="#475569", + font=("Segoe UI", 9, "bold")) + style.configure("TButton", font=("Segoe UI", 9), padding=(10, 7), + background="#ffffff", foreground="#1e293b", bordercolor="#cbd5e1") + style.map("TButton", background=[("active", "#e8eef7")]) + style.configure("Primary.TButton", background="#2563eb", foreground="#ffffff", + bordercolor="#2563eb", font=("Segoe UI", 9, "bold")) + style.map("Primary.TButton", background=[("active", "#1d4ed8")]) + style.configure("Danger.TButton", background="#dc2626", foreground="#ffffff", + bordercolor="#dc2626", font=("Segoe UI", 9, "bold")) + style.map("Danger.TButton", background=[("active", "#b91c1c")]) + style.configure("Title.TLabel", font=("Segoe UI", 18, "bold"), foreground="#0f172a") + style.configure("Subtitle.TLabel", font=("Segoe UI", 9), foreground="#64748b") + style.configure("Status.TLabel", font=("Segoe UI", 9, "bold")) + style.configure("Footer.TLabel", background="#e9eef5", foreground="#475569", + font=("Segoe UI", 9)) + style.configure("Treeview", rowheight=28, background="#ffffff", fieldbackground="#ffffff", + foreground="#26282a", bordercolor="#e1e1dc", font=("Helvetica", 9)) + style.configure("Treeview.Heading", font=("Helvetica", 9, "bold"), padding=(6, 6), + background="#f0f0ed", foreground="#4c4f52") + style.map("Treeview", background=[("selected", "#dbeafe")], + foreground=[("selected", "#1e3a8a")]) + style.configure("Horizontal.TProgressbar", troughcolor="#e2e8f0", background="#22c55e", + bordercolor="#e2e8f0", lightcolor="#22c55e", darkcolor="#22c55e") + + def _console_button(self, master, text, command, kind="neutral", width=112): + colors = { + "neutral": ("#f4f4f1", "#e9e9e4", "#343739", "#deded8"), + "primary": ("#dcebd5", "#cde2c4", "#27401f", "#c4dcb9"), + "danger": ("#f7dfdc", "#f1cfcb", "#7b3029", "#edcec9"), + } + color, hover, text_color, border = colors[kind] + return ctk.CTkButton( + master, text=text, command=command, width=width, height=34, + corner_radius=11, fg_color=color, hover_color=hover, + text_color=text_color, border_color=border, border_width=1, + font=ctk.CTkFont("Helvetica", 12, "bold" if kind != "neutral" else "normal"), + bg_color="transparent", + ) + + def _console_combo(self, master, values, width): + return ctk.CTkComboBox( + master, values=[str(value) for value in values], width=width, height=34, + corner_radius=10, border_width=1, border_color="#cbd5e1", + fg_color="#ffffff", button_color="#e8eef7", + button_hover_color="#dbeafe", dropdown_fg_color="#ffffff", + dropdown_hover_color="#dbeafe", text_color="#1e293b", + bg_color="transparent", state="readonly", + ) + + def _build(self): + shell = ctk.CTkFrame(self, fg_color="#f8f8f6", corner_radius=24, + border_width=1, border_color="#d5d6d3") + shell.pack(fill="both", expand=True, padx=18, pady=18) + shell.grid_rowconfigure(0, weight=1) + shell.grid_columnconfigure(1, weight=1) + + rail = ctk.CTkFrame(shell, width=248, fg_color="#282d30", corner_radius=22) + rail.grid(row=0, column=0, sticky="nsew") + rail.grid_propagate(False) + workspace = ctk.CTkFrame(shell, fg_color="#fbfbf9", corner_radius=22) + workspace.grid(row=0, column=1, sticky="nsew") + + ctk.CTkLabel(rail, text="SIMPLE ROV / 03", text_color="#f4f4f1", + font=ctk.CTkFont("Helvetica", 13, "bold")).pack( + anchor="w", padx=24, pady=(24, 0)) + ctk.CTkLabel(rail, text="ArduSub vehicle", text_color="#8f9699", + font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w", padx=24, pady=(2, 14)) + + vehicle_art = tk.Canvas(rail, width=204, height=250, bg="#282d30", + highlightthickness=0, bd=0) + vehicle_art.pack(padx=20, pady=(5, 4)) + vehicle_art.create_oval(47, 27, 157, 223, fill="#353b3e", outline="#555c60", width=2) + vehicle_art.create_oval(68, 49, 136, 201, fill="#303538", outline="#444a4e") + vehicle_art.create_oval(80, 102, 124, 146, fill="#5e666a", outline="#788084", width=2) + vehicle_art.create_oval(21, 57, 62, 100, fill="#202426", outline="#62696d", width=2) + vehicle_art.create_oval(142, 57, 183, 100, fill="#202426", outline="#62696d", width=2) + vehicle_art.create_line(42, 79, 72, 93, fill="#757d81", width=3) + vehicle_art.create_line(162, 79, 132, 93, fill="#757d81", width=3) + vehicle_art.create_text(102, 236, text="TOP VIEW • 3 THRUSTERS", + fill="#7e8689", font=("Helvetica", 8, "bold")) + + ctk.CTkLabel(rail, text="Vehicle", text_color="#959c9f", + font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w", padx=24, pady=(5, 0)) + self.vehicle_label = ctk.CTkLabel(rail, text="No heartbeat", text_color="#f4f4f1", + font=ctk.CTkFont("Helvetica", 14, "bold")) + self.vehicle_label.pack(anchor="w", padx=24) + + rail_bottom = ctk.CTkFrame(rail, fg_color="transparent") + rail_bottom.pack(side="bottom", fill="x", padx=24, pady=22) + self.battery_summary = ctk.CTkLabel( + rail_bottom, text="Battery —", text_color="#f4f4f1", + font=ctk.CTkFont("Helvetica", 13, "bold"), anchor="w" + ) + self.battery_summary.pack(fill="x") + self.battery_meta = ctk.CTkLabel( + rail_bottom, text="Waiting for voltage", text_color="#92999c", + font=ctk.CTkFont("Helvetica", 9), anchor="w" + ) + self.battery_meta.pack(fill="x", pady=(0, 5)) + self.battery_bar = ctk.CTkProgressBar( + rail_bottom, height=7, corner_radius=4, fg_color="#454b4e", + progress_color="#c4e46c" + ) + self.battery_bar.set(0) + self.battery_bar.pack(fill="x", pady=(0, 18)) + self.link_label = ctk.CTkLabel( + rail_bottom, text="● DISCONNECTED", text_color="#f08a82", + font=ctk.CTkFont("Helvetica", 10, "bold"), anchor="w" + ) + self.link_label.pack(fill="x") + + header = ctk.CTkFrame(workspace, fg_color="transparent") + header.pack(fill="x", padx=28, pady=(24, 14)) + title = ctk.CTkFrame(header, fg_color="transparent") + title.pack(side="left") + ctk.CTkLabel(title, text="Drive", text_color="#17191a", + font=ctk.CTkFont("Helvetica", 24, "bold")).pack(anchor="w") + ctk.CTkLabel(title, text="Differential steering • MANUAL control", + text_color="#8a8c8e", font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w") + + top = ctk.CTkFrame(header, fg_color="transparent") + top.pack(side="right") + self.port = self._console_combo(top, [], 190) + self.port.grid(row=0, column=0, padx=4) + self._console_button(top, "↻", self.refresh_ports, width=38).grid(row=0, column=1, padx=4) + self.baud = self._console_combo(top, (9600, 19200, 38400, 57600, 115200), 105) + self.baud.set("57600") + self.rate = self._console_combo(top, (10, 20), 72) + self.rate.set("20") + self.link_settings_button = self._console_button( + top, "•••", self._toggle_link_settings, width=38 + ) + self.link_settings_button.grid(row=0, column=2, padx=4) + self.connect_button = self._console_button( + top, "Connect", self.toggle_connect, kind="primary", width=110 + ) + self.connect_button.grid(row=0, column=3, padx=(4, 0)) + + separator = ctk.CTkFrame(workspace, height=1, fg_color="#e7e7e3") + separator.pack(fill="x", padx=28) + + controls = ctk.CTkFrame(workspace, fg_color="transparent") + controls.pack(fill="both", expand=True, padx=28, pady=18) + controls.grid_rowconfigure(0, weight=1) + controls.grid_columnconfigure(0, weight=1) + controls.grid_columnconfigure(1, minsize=214) + + drive_card = ctk.CTkFrame(controls, fg_color="#f5f5f2", corner_radius=18) + drive_card.grid(row=0, column=0, sticky="nsew", padx=(0, 12)) + ctk.CTkLabel(drive_card, text="STEER + THROTTLE", text_color="#66696b", + font=ctk.CTkFont("Helvetica", 10, "bold")).pack(pady=(18, 0)) + self.joystick = Joystick(drive_card, self._axes_changed) + self.joystick.pack(expand=True, pady=(4, 0)) + ctk.CTkLabel(drive_card, text="W / S drive • A / D steer", + text_color="#969896", font=ctk.CTkFont("Helvetica", 9)).pack(pady=(0, 16)) + + heave_card = ctk.CTkFrame(controls, fg_color="#f5f5f2", corner_radius=18) + heave_card.grid(row=0, column=1, sticky="nsew") + ctk.CTkLabel(heave_card, text="VERTICAL", text_color="#66696b", + font=ctk.CTkFont("Helvetica", 10, "bold")).pack(pady=(18, 2)) + ctk.CTkLabel(heave_card, text="RISE", text_color="#969896", + font=ctk.CTkFont("Helvetica", 8, "bold")).pack() + self.vertical = SpringScale( + heave_card, from_=100, to=-100, orient="vertical", length=230, + command=lambda _: self._axes_changed() + ) + self.vertical.set(0) + self.vertical.pack(expand=True, pady=8) + ctk.CTkLabel(heave_card, text="DIVE", text_color="#969896", + font=ctk.CTkFont("Helvetica", 8, "bold")).pack() + ctk.CTkLabel(heave_card, text="R / F • springs to neutral", + text_color="#969896", font=ctk.CTkFont("Helvetica", 9)).pack(pady=(4, 16)) + + self.battery_cells = tk.StringVar(value=str(self.battery_config["cells"])) + self.battery_capacity = tk.StringVar(value=str(self.battery_config["capacity_mah"])) + self.battery_c_rating = tk.StringVar(value=str(self.battery_config["c_rating"])) + self.battery_full_cell = tk.StringVar(value=str(self.battery_config["full_v_per_cell"])) + self.battery_empty_cell = tk.StringVar(value=str(self.battery_config["empty_v_per_cell"])) + self.battery_warning = tk.StringVar(value=str(self.battery_config["warning_percent"])) + fields = ( + ("Cells (S)", self.battery_cells, 5), + ("Capacity mAh", self.battery_capacity, 8), + ("C rating", self.battery_c_rating, 5), + ("Full V/cell", self.battery_full_cell, 6), + ("Empty V/cell", self.battery_empty_cell, 6), + ("Warn %", self.battery_warning, 5), + ) + self.battery_settings_frame = ctk.CTkFrame(workspace, fg_color="#f0f0ed", corner_radius=14) + for column, (label, variable, width) in enumerate(fields): + group = ctk.CTkFrame(self.battery_settings_frame, fg_color="transparent") + group.grid(row=0, column=column, padx=(0, 8), sticky="w") + ctk.CTkLabel(group, text=label, text_color="#66696b", + font=ctk.CTkFont("Helvetica", 9)).pack(anchor="w") + ctk.CTkEntry(group, textvariable=variable, width=max(62, width * 9), height=30, + corner_radius=8, border_color="#d0d0cb").pack(anchor="w") + self._console_button(self.battery_settings_frame, "Apply", self._apply_battery_settings, + kind="primary", width=82).grid( + row=0, column=6, padx=(8, 0), sticky="sew" + ) + self.battery_settings_frame.pack_forget() + + actions = ctk.CTkFrame(workspace, fg_color="transparent") + actions.pack(fill="x", padx=28, pady=(0, 14)) + self._console_button(actions, "MANUAL", self.set_manual, width=94).pack(side="left", padx=3) + self._console_button(actions, "ARM", self.arm, kind="primary", width=82).pack(side="left", padx=3) + self._console_button(actions, "DISARM", self.disarm, kind="danger", width=92).pack(side="left", padx=3) + ctk.CTkLabel(actions, text="Output", text_color="#777a7c", + font=ctk.CTkFont("Helvetica", 10)).pack(side="left", padx=(18, 4)) + self.gain = ctk.CTkSlider( + actions, from_=5, to=50, number_of_steps=45, width=170, height=18, + command=self._gain_changed, corner_radius=10, button_corner_radius=10, + button_length=22, fg_color="#dbe3ee", progress_color="#60a5fa", + button_color="#2563eb", button_hover_color="#1d4ed8", + bg_color="transparent", + ) + self.gain.set(20) + self.gain.pack(side="left") + self.gain_text = ctk.CTkLabel(actions, text="20%", text_color="#55585a", + font=ctk.CTkFont("Helvetica", 10, "bold")) + self.gain_text.pack(side="left") + self._console_button(actions, "Motor setup", self._toggle_motor_setup, width=112).pack( + side="right", padx=3 + ) + self._console_button(actions, "Battery", self._toggle_battery_settings, width=88).pack( + side="right", padx=3 + ) + + assignments = ctk.CTkFrame(workspace, fg_color="#f0f0ed", corner_radius=14) + self.assignments_frame = assignments + self.servo_tree = ttk.Treeview( + assignments, columns=("output", "function", "reversed"), + show="headings", height=5, selectmode="browse", + ) + self.servo_tree.heading("output", text="Output") + self.servo_tree.heading("function", text="Function") + self.servo_tree.heading("reversed", text="Motor reversed") + self.servo_tree.column("output", width=90, anchor="center", stretch=False) + self.servo_tree.column("function", width=220, anchor="w") + self.servo_tree.column("reversed", width=90, anchor="center", stretch=False) + for number in range(1, 17): + self.servo_tree.insert("", "end", iid=str(number), + values=(f"SERVO{number}", "Waiting…", "Waiting…")) + self.servo_tree.pack(side="left", fill="x", expand=True) + scrollbar = ttk.Scrollbar(assignments, orient="vertical", command=self.servo_tree.yview) + scrollbar.pack(side="left", fill="y") + self.servo_tree.configure(yscrollcommand=scrollbar.set) + + edit = ctk.CTkFrame(assignments, fg_color="transparent") + edit.pack(side="right", fill="y") + ctk.CTkLabel(edit, text="Selected output function", text_color="#55585a").pack(anchor="w") + motor_choices = ["Unassigned (0)"] + [f"Motor {number}" for number in range(1, 13)] + self.motor_function = self._console_combo(edit, motor_choices, 165) + self.motor_function.set("Motor 1") + self.motor_function.pack(fill="x", pady=(2, 5)) + self._console_button(edit, "Apply function", self.apply_servo_function, width=165).pack(fill="x", pady=2) + self._console_button(edit, "Reverse motor", self.toggle_servo_reversed, width=165).pack(fill="x", pady=2) + ctk.CTkLabel(edit, text="Changes require DISARMED", text_color="#a16207").pack(pady=(8, 0)) + + log_frame = ctk.CTkFrame(workspace, fg_color="transparent") + log_frame.pack(fill="x", padx=28, pady=(0, 12)) + self.log_frame = log_frame + self.log_path_text = tk.StringVar(value="Session log starts when connected") + ctk.CTkLabel(log_frame, textvariable=self.log_path_text, anchor="w", text_color="#8b8d8f", + font=ctk.CTkFont("Helvetica", 9)).pack(side="left", fill="x", expand=True) + self._console_button(log_frame, "Open logs", self.open_logs_folder, width=96).pack(side="right") + + self.status_text = tk.StringVar(value="Ready. Controls are neutral; vehicle is not connected.") + ctk.CTkLabel(workspace, textvariable=self.status_text, anchor="w", height=34, + corner_radius=12, fg_color="#eeeeea", text_color="#65686a", + font=ctk.CTkFont("Helvetica", 9)).pack(fill="x", padx=28, pady=(0, 18)) + + def refresh_ports(self): + ports = [p.device for p in list_ports.comports()] + self.port.configure(values=ports) + if ports and self.port.get() not in ports: + self.port.set(ports[0]) + + def _toggle_battery_settings(self): + if self.battery_settings_frame.winfo_ismapped(): + self.battery_settings_frame.pack_forget() + else: + self.battery_settings_frame.pack( + fill="x", padx=28, pady=(0, 12), before=self.log_frame + ) + + def _toggle_link_settings(self): + if self.baud.winfo_ismapped(): + self.baud.grid_remove() + self.rate.grid_remove() + else: + self.baud.grid(row=1, column=0, sticky="e", padx=4, pady=(6, 0)) + self.rate.grid(row=1, column=1, sticky="w", padx=4, pady=(6, 0)) + + def _toggle_motor_setup(self): + if self.assignments_frame.winfo_ismapped(): + self.assignments_frame.pack_forget() + else: + self.assignments_frame.pack( + fill="x", padx=28, pady=(0, 12), before=self.log_frame + ) + + def _load_battery_settings(self): + try: + saved = json.loads(self.battery_config_path.read_text(encoding="utf-8")) + for key in self.battery_config: + if key in saved: + self.battery_config[key] = saved[key] + except (OSError, ValueError, TypeError): + pass + + def _apply_battery_settings(self): + try: + config = { + "cells": int(self.battery_cells.get()), + "capacity_mah": int(self.battery_capacity.get()), + "c_rating": float(self.battery_c_rating.get()), + "full_v_per_cell": float(self.battery_full_cell.get()), + "empty_v_per_cell": float(self.battery_empty_cell.get()), + "warning_percent": int(self.battery_warning.get()), + } + if not 1 <= config["cells"] <= 12: + raise ValueError("cell count must be between 1 and 12") + if config["capacity_mah"] <= 0 or config["c_rating"] <= 0: + raise ValueError("capacity and C rating must be positive") + if not config["full_v_per_cell"] > config["empty_v_per_cell"] > 0: + raise ValueError("full V/cell must be greater than empty V/cell") + if not 0 <= config["warning_percent"] <= 100: + raise ValueError("warning percentage must be between 0 and 100") + self.battery_config = config + self.battery_config_path.write_text( + json.dumps(config, indent=2) + "\n", encoding="utf-8" + ) + self._update_battery_display() + self.status_text.set("Battery settings saved") + except (ValueError, OSError) as exc: + messagebox.showerror("Invalid battery settings", str(exc)) + + def _battery_percent(self, voltage): + cells = self.battery_config["cells"] + per_cell = voltage / cells + empty = self.battery_config["empty_v_per_cell"] + full = self.battery_config["full_v_per_cell"] + return max(0.0, min(100.0, (per_cell - empty) * 100.0 / (full - empty))) + + def _update_battery_display(self): + voltage = self.battery_voltage_smoothed + if voltage is None: + self.battery_summary.configure(text="Battery —", text_color="#f4f4f1") + self.battery_meta.configure(text="Waiting for voltage") + self.battery_bar.set(0) + return + cells = self.battery_config["cells"] + percent = self._battery_percent(voltage) + per_cell = voltage / cells + warning = percent <= self.battery_config["warning_percent"] + self.battery_summary.configure( + text=f"Battery {percent:.0f}%", + text_color="#ffaaa2" if warning else "#f4f4f1", + ) + self.battery_meta.configure( + text=f"{voltage:.2f} V • {per_cell:.2f} V/cell • {cells}S" + + (" • LOW" if warning else "") + ) + self.battery_bar.configure(progress_color="#f08a82" if warning else "#c4e46c") + self.battery_bar.set(percent / 100.0) + + def toggle_connect(self): + if self.worker.thread and self.worker.thread.is_alive(): + self.worker.disconnect() + self.connect_button.configure(state="disabled") + return + if not self.port.get(): + messagebox.showerror("No port", "Select a USB serial COM port.") + return + self.worker.rate_hz = int(self.rate.get()) + self.worker.connect(self.port.get(), int(self.baud.get())) + self.connect_button.configure(text="Disconnect") + self.status_text.set("Opening serial port…") + + def _axes_changed(self): + turn, forward = self.joystick.value + self.worker.set_axes(Axes(forward, 0.0, + float(self.vertical.get()) / 100, + turn)) + + def _gain_changed(self, value): + value = int(float(value)) + self.worker.gain = value / 100 + self.gain_text.configure(text=f"{value}%") + + def _key_down(self, event): + key = event.keysym.lower() + if key in "wasdqerf": + self.keys.add(key) + self._apply_keys() + + def _key_up(self, event): + key = event.keysym.lower() + self.keys.discard(key) + self._apply_keys() + + def _apply_keys(self): + turn = (1 if ("d" in self.keys or "e" in self.keys) else 0) - ( + 1 if ("a" in self.keys or "q" in self.keys) else 0 + ) + forward = (1 if "w" in self.keys else 0) - (1 if "s" in self.keys else 0) + vertical = (1 if "r" in self.keys else 0) - (1 if "f" in self.keys else 0) + planar = (turn, forward) + if planar != self.keyboard_planar: + if turn or forward: + self.joystick.set_immediate(float(turn), float(forward)) + else: + self.joystick._release() + self.keyboard_planar = planar + if vertical != self.keyboard_vertical: + if vertical: + self.vertical.set_immediate(vertical * 100) + else: + self.vertical.spring_to_center() + self.keyboard_vertical = vertical + self._axes_changed() + + def arm(self): + if not messagebox.askyesno("Arm vehicle", "Propellers removed or vehicle securely restrained?\n\nArm now?"): + return + try: + self.worker.log("OPERATOR", "clicked ARM") + self.worker.command_arm(True) + self.status_text.set("Arm command sent; waiting for ACK/heartbeat…") + except Exception as exc: + messagebox.showerror("Cannot arm", str(exc)) + + def disarm(self): + try: + self.worker.log("OPERATOR", "clicked DISARM") + self.worker.command_arm(False) + self.status_text.set("Disarm command sent; neutral sent") + except Exception as exc: + messagebox.showerror("Cannot disarm", str(exc)) + + def set_manual(self): + try: + self.worker.log("OPERATOR", "clicked SET MANUAL") + self.worker.set_manual_mode() + self.status_text.set("MANUAL mode request sent") + except Exception as exc: + messagebox.showerror("Cannot set mode", str(exc)) + + def open_logs_folder(self): + log_dir = Path(__file__).resolve().parent / "logs" + log_dir.mkdir(exist_ok=True) + os.startfile(log_dir) + + def _selected_servo(self): + selection = self.servo_tree.selection() + if not selection: + raise RuntimeError("Select a SERVO output in the table first") + return int(selection[0]) + + def apply_servo_function(self): + try: + servo = self._selected_servo() + choice = self.motor_function.get() + if choice.startswith("Motor "): + motor = int(choice.split()[1]) + function = 32 + motor if motor <= 8 else 73 + motor + else: + function = 0 + description = MOTOR_FUNCTIONS.get(function, "Unassigned") + if not messagebox.askyesno( + "Change output function", + f"Set SERVO{servo}_FUNCTION to {description} ({function})?\n\n" + "This changes the autopilot configuration.", + ): + return + self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function) + except Exception as exc: + messagebox.showerror("Cannot change function", str(exc)) + + def toggle_servo_reversed(self): + try: + servo = self._selected_servo() + function = self.servo_functions.get(servo) + motor = motor_number_for_function(function) + if motor is None: + raise RuntimeError( + f"SERVO{servo} is not assigned to an ArduSub Motor function" + ) + current = self.motor_directions.get(motor) + if current is None: + raise RuntimeError(f"MOT_{motor}_DIRECTION has not been read yet") + new_value = -1 if current == 1 else 1 + if not messagebox.askyesno( + "Reverse motor output", + f"SERVO{servo} is Motor {motor}.\n" + f"Set MOT_{motor}_DIRECTION to {new_value}?\n\n" + "Test direction with propellers removed after changing this.", + ): + return + self.worker.set_parameter(f"MOT_{motor}_DIRECTION", new_value) + except Exception as exc: + messagebox.showerror("Cannot reverse output", str(exc)) + + def _refresh_servo_row(self, number): + function = self.servo_functions.get(number) + motor = motor_number_for_function(function) + direction = self.motor_directions.get(motor) if motor is not None else None + if function is None: + function_text = "Waiting…" + else: + function_text = f"{MOTOR_FUNCTIONS.get(function, 'Other / unassigned')} ({function})" + if motor is None: + reversed_text = "—" + elif direction is None: + reversed_text = "Waiting…" + else: + reversed_text = "Yes" if direction == -1 else "No" + self.servo_tree.item(str(number), values=(f"SERVO{number}", function_text, reversed_text)) + + def _poll(self): + try: + while True: + event = self.events.get_nowait() + kind = event[0] + if kind == "heartbeat": + _, system, autopilot, mode, armed = event + self.vehicle_label.configure( + text=f"{mode} • {'ARMED' if armed else 'DISARMED'}" + ) + self.link_label.configure(text="● LINK OK", text_color="#c4e46c") + elif kind in ("status", "statustext"): + self.status_text.set(event[1]) + elif kind == "ack": + self.status_text.set(f"Command {event[1]} ACK: {event[2]}") + elif kind == "servo_param": + _, param_id, value = event + number = int(param_id[5:param_id.index("_")]) + self.servo_functions[number] = value + self._refresh_servo_row(number) + elif kind == "motor_direction": + _, motor, value = event + self.motor_directions[motor] = value + for number in range(1, 17): + if motor_number_for_function(self.servo_functions.get(number)) == motor: + self._refresh_servo_row(number) + elif kind == "battery": + self.battery_voltage = event[1] + if self.battery_voltage_smoothed is None: + self.battery_voltage_smoothed = self.battery_voltage + else: + self.battery_voltage_smoothed = ( + 0.75 * self.battery_voltage_smoothed + 0.25 * self.battery_voltage + ) + self._update_battery_display() + elif kind == "param_saved": + self.status_text.set(f"Confirmed: {event[1]}={event[2]}") + elif kind == "param_missing": + self.status_text.set(f"No response for {event[1]}; continuing scan…") + elif kind == "param_scan_complete": + count = sum(value in MOTOR_FUNCTIONS for value in self.servo_functions.values()) + self.status_text.set(f"Servo scan complete: {count} motor output(s) assigned") + elif kind == "log_path": + self.log_path_text.set(f"Log: {event[1]}") + elif kind == "error": + self.status_text.set(event[1]) + messagebox.showerror("MAVLink", event[1]) + elif kind == "disconnected": + self.connect_button.configure(text="Connect", state="normal") + self.link_label.configure(text="● DISCONNECTED", text_color="#f08a82") + self.vehicle_label.configure(text="No heartbeat") + self.battery_voltage = None + self.battery_voltage_smoothed = None + self._update_battery_display() + self.servo_functions.clear() + self.motor_directions.clear() + for number in range(1, 17): + self.servo_tree.item(str(number), values=(f"SERVO{number}", "Waiting…", "Waiting…")) + except queue.Empty: + pass + self.after(50, self._poll) + + def _link_watchdog(self): + if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 5.0: + if time.monotonic() - self.worker.last_rx < 2.0: + text = "● DATA OK / HEARTBEAT DELAYED" + else: + text = "● ALL LINK DATA LOST" + self.link_label.configure(text=text, text_color="#f08a82") + self.worker._send_control(force_neutral=True) + self.after(500, self._link_watchdog) + + def close(self): + self.worker.disconnect() + self.after(150, self.destroy) + + +if __name__ == "__main__": + # The production interface is CSS/HTML rendered by the local WebView. + # The Tk classes remain in this module as a fallback and to keep the + # MAVLink worker import-compatible with older installations. + from web_ui import main as run_web_interface + + run_web_interface() diff --git a/SUB Control/battery_settings.json b/SUB Control/battery_settings.json new file mode 100644 index 0000000..f8db5a8 --- /dev/null +++ b/SUB Control/battery_settings.json @@ -0,0 +1,8 @@ +{ + "cells": 4, + "capacity_mah": 5500, + "c_rating": 35.0, + "full_v_per_cell": 4.2, + "empty_v_per_cell": 3.3, + "warning_percent": 20 +} diff --git a/SUB Control/requirements.txt b/SUB Control/requirements.txt new file mode 100644 index 0000000..0843753 --- /dev/null +++ b/SUB Control/requirements.txt @@ -0,0 +1,4 @@ +pymavlink>=2.4.49,<3 +pyserial>=3.5,<4 +customtkinter==5.2.2 +pywebview==6.2.1 diff --git a/SUB Control/run.ps1 b/SUB Control/run.ps1 new file mode 100644 index 0000000..54284e2 --- /dev/null +++ b/SUB Control/run.ps1 @@ -0,0 +1,8 @@ +$ErrorActionPreference = "Stop" + +if (-not (Test-Path -LiteralPath ".venv\Scripts\python.exe")) { + py -m venv .venv +} + +& .venv\Scripts\python.exe -m pip install -r requirements.txt +& .venv\Scripts\python.exe app.py diff --git a/SUB Control/web/app.js b/SUB Control/web/app.js new file mode 100644 index 0000000..21098b1 --- /dev/null +++ b/SUB Control/web/app.js @@ -0,0 +1,40 @@ +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 api=(name,...args)=>window.pywebview.api[name](...args); +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)} + +const joy=$('#joystick'),stick=$('#stick');let joyDrag=false,returnAnim=0; +function setJoy(x,y,send=true){const r=joy.clientWidth*.34,n=Math.hypot(x,y);if(n>1){x/=n;y/=n}control.turn=x;control.forward=-y;stick.style.transform=`translate(calc(-50% + ${x*r}px),calc(-50% + ${y*r}px))`;updateReadouts();if(send)sendAxes()} +function joyEvent(e){const b=joy.getBoundingClientRect();setJoy((e.clientX-b.left-b.width/2)/(b.width*.34),(e.clientY-b.top-b.height/2)/(b.height*.34))} +joy.addEventListener('pointerdown',e=>{joyDrag=true;cancelAnimationFrame(returnAnim);joy.setPointerCapture(e.pointerId);joyEvent(e)});joy.addEventListener('pointermove',e=>{if(joyDrag)joyEvent(e)});joy.addEventListener('pointerup',()=>{joyDrag=false;easeJoy()}); +function easeJoy(){const sx=control.turn,sy=-control.forward,start=performance.now();function frame(now){const p=Math.min(1,(now-start)/400),smooth=p*p*(3-2*p);setJoy(sx*(1-smooth),sy*(1-smooth));if(p<1)returnAnim=requestAnimationFrame(frame)}returnAnim=requestAnimationFrame(frame)} + +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 heaveEvent(e){const b=heave.getBoundingClientRect();setHeave((b.top+b.height/2-e.clientY)/(b.height*.43))} +heave.addEventListener('pointerdown',e=>{heaveDrag=true;cancelAnimationFrame(heaveAnim);heave.setPointerCapture(e.pointerId);heaveEvent(e)});heave.addEventListener('pointermove',e=>{if(heaveDrag)heaveEvent(e)});heave.addEventListener('pointerup',()=>{heaveDrag=false;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){cancelAnimationFrame(returnAnim);setJoy(turn,-forward)}else if(!joyDrag)easeJoy();if(vertical){cancelAnimationFrame(heaveAnim);setHeave(vertical)}else if(!heaveDrag)easeHeave()} +addEventListener('keydown',e=>{if(/^(INPUT|SELECT)$/.test(e.target.tagName))return;const k=e.key.toLowerCase();if('wasdqerf'.includes(k)&&!keys.has(k)){keys.add(k);applyKeys()}});addEventListener('keyup',e=>{const k=e.key.toLowerCase();if(keys.delete(k))applyKeys()}); +addEventListener('blur',()=>{keys.clear();if(!joyDrag)easeJoy();if(!heaveDrag)easeHeave()}); + +function openModal(id){document.getElementById(id)?.showModal()}$$('[data-open]').forEach(b=>b.onclick=()=>openModal(b.dataset.open));$$('[data-close]').forEach(b=>b.onclick=()=>b.closest('dialog').close()); +$('#window-min').onclick=()=>api('minimize_window');$('#window-max').onclick=()=>api('toggle_maximize_window');$('#window-close').onclick=()=>api('close_window');$('.window-drag').ondblclick=()=>api('toggle_maximize_window'); +$('#load-model').onclick=async()=>{try{const model=await api('choose_model');if(model)window.rovModel.load(model)}catch(e){toast(e.message,true)}};$('#model-style').onclick=e=>{const edges=window.rovModel.toggleStyle();e.currentTarget.textContent=edges?'Shaded':'Edges'}; +$('#refresh').onclick=async()=>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=()=>api('command','manual').catch(e=>toast(e.message,true));$('#arm').onclick=()=>openModal('arm-dialog');$('#confirm-arm').onclick=()=>{api('command','arm').catch(e=>toast(e.message,true));$('#arm-dialog').close()};$('#disarm').onclick=()=>api('command','disarm').catch(e=>toast(e.message,true));$('#logs').onclick=()=>api('open_logs'); +$('#gain').oninput=e=>{$('#gain-value').textContent=`${e.target.value}%`;api('set_gain',+e.target.value)}; +$('#battery-form').onsubmit=async e=>{e.preventDefault();const data=Object.fromEntries(new FormData(e.target));try{batteryConfig=await api('save_battery',data);$('#battery-modal').close();toast('Battery settings saved')}catch(err){toast(err.message,true)}}; +function fillBatteryForm(){for(const [k,v] of Object.entries(batteryConfig)){const el=$(`#battery-form [name=${k}]`);if(el)el.value=v}} +function renderMotors(servos){const signature=JSON.stringify(servos);if(signature===servoSignature)return;servoSignature=signature;const list=$('#motor-list');list.innerHTML='';for(let i=1;i<=16;i++){const s=servos[String(i)]||{},row=document.createElement('div');row.className='motor-row';const opts=['',...Array.from({length:12},(_,n)=>``)].join('');row.innerHTML=`SERVO${i}${s.direction==null?'—':s.direction===-1?'Reversed':'Normal'}`;row.querySelector('select').onchange=e=>api('set_servo_function',i,+e.target.value).catch(x=>toast(x.message,true));row.querySelector('button').onclick=()=>api('reverse_motor',i).catch(x=>toast(x.message,true));list.append(row)}} +function render(s){connected=s.connected;$('#connect').textContent=connected?'Disconnect':'Connect';$('#link').textContent=s.link;$('.link-state').classList.toggle('online',s.link==='LINK OK');$('#vehicle').textContent=s.vehicle;$('#status').textContent=s.status;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 booted=false; +async function boot(){if(booted||!window.pywebview?.api)return;booted=true;bridgeReady=true;try{const init=await api('initial_state');batteryConfig=init.battery_config;populatePorts(init.ports);fillBatteryForm();render(init.state);const saved=await api('saved_model');if(saved)window.rovModel.load(saved);poll();setInterval(async()=>{if(!connected)try{populatePorts(await api('refresh_ports'))}catch(_){}},2000)}catch(e){booted=false;bridgeReady=false;toast(`Python bridge: ${e.message}`,true);setTimeout(waitForBridge,500)}} +function waitForBridge(){if(window.pywebview?.api)boot();else setTimeout(waitForBridge,50)} +addEventListener('pywebviewready',boot);if(document.readyState==='loading')addEventListener('DOMContentLoaded',waitForBridge);else waitForBridge(); diff --git a/SUB Control/web/index.html b/SUB Control/web/index.html new file mode 100644 index 0000000..b974c24 --- /dev/null +++ b/SUB Control/web/index.html @@ -0,0 +1,119 @@ + + + + + + Simple ROV Control + + + +
+
SSimple ROV ControlBuilt-in vehicle
+
+ + + +
+
+
+ + +
+
+
PILOT CONSOLE

Drive

+
+ + + + +
+
+ +
+
+
STEER + THROTTLEDifferential drive
W A S D
+
+
+ FORWARDREVERSE + TURN LEFTTURN RIGHT +
+
+
0% surge0% turn
+
+ +
+
VERTICALCenter thruster
R / F
+
+ RISE +
+ DIVE +
+
0% vertical
+
+
+ +
+
+ + + +
+ + + +
+

Ready. Select a USB port to begin.

+
+
+ +
+ + SAFETY CHECK

Arm the vehicle?

Remove propellers or securely restrain the ROV before enabling motor output.

 + CONNECTION

Link settings

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

 + POWER

Battery settings

 + CONFIGURATION

Motor outputs

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

OutputFunctionDirection
 + + + + + diff --git a/SUB Control/web/model-viewer.js b/SUB Control/web/model-viewer.js new file mode 100644 index 0000000..7f0fa42 --- /dev/null +++ b/SUB Control/web/model-viewer.js @@ -0,0 +1,79 @@ +(() => { + const canvas = document.querySelector('#model-canvas'); + const stage = document.querySelector('.rov-stage'); + const ctx = canvas.getContext('2d'); + let vertices = null, yaw = -.55, pitch = -.42, zoom = 1, edgesOnly = false; + let dragging = false, lastX = 0, lastY = 0, frame = 0; + + function parseStl(buffer) { + const view = new DataView(buffer); + let raw = []; + const binaryCount = buffer.byteLength >= 84 ? view.getUint32(80, true) : 0; + const isBinary = binaryCount > 0 && 84 + binaryCount * 50 <= buffer.byteLength; + if (isBinary) { + const stride = Math.max(1, Math.ceil(binaryCount / 14000)); + for (let t = 0; t < binaryCount; t += stride) { + const offset = 84 + t * 50 + 12; + for (let v = 0; v < 3; v++) { + raw.push(view.getFloat32(offset + v * 12, true)); + raw.push(view.getFloat32(offset + v * 12 + 4, true)); + raw.push(view.getFloat32(offset + v * 12 + 8, true)); + } + } + } else { + const text = new TextDecoder().decode(buffer); + const matches = [...text.matchAll(/vertex\s+([-+\deE.]+)\s+([-+\deE.]+)\s+([-+\deE.]+)/gi)]; + if (matches.length < 3) throw new Error('This file does not contain a readable STL mesh'); + const stride = Math.max(1, Math.ceil(matches.length / 42000)); + for (let i = 0; i + 2 < matches.length; i += 3 * stride) { + for (let j = 0; j < 3; j++) raw.push(+matches[i+j][1], +matches[i+j][2], +matches[i+j][3]); + } + } + if (raw.length < 9) throw new Error('The STL mesh is empty'); + let minX=Infinity,minY=Infinity,minZ=Infinity,maxX=-Infinity,maxY=-Infinity,maxZ=-Infinity; + for(let i=0;ia.z-b.z); + ctx.lineJoin='round';ctx.lineWidth=.65; + for(const t of tris){const ax=cx+t.a[0]*size,ay=cy-t.a[1]*size,bx=cx+t.b[0]*size,by=cy-t.b[1]*size,cx2=cx+t.c[0]*size,cy2=cy-t.c[1]*size;const nz=(bx-ax)*(cy2-ay)-(by-ay)*(cx2-ax),light=Math.max(0,Math.min(1,.25+Math.abs(nz)/(size*size)*5));ctx.beginPath();ctx.moveTo(ax,ay);ctx.lineTo(bx,by);ctx.lineTo(cx2,cy2);ctx.closePath();ctx.fillStyle=edgesOnly?'rgba(79,96,102,.035)':`rgba(${55+light*80},${67+light*82},${72+light*84},.88)`;ctx.strokeStyle=edgesOnly?'rgba(183,210,216,.72)':'rgba(171,193,198,.25)';ctx.fill();ctx.stroke()} + } + function requestDraw(){if(!frame)frame=requestAnimationFrame(draw)} + + canvas.addEventListener('pointerdown',e=>{dragging=true;lastX=e.clientX;lastY=e.clientY;canvas.setPointerCapture(e.pointerId)}); + canvas.addEventListener('pointermove',e=>{if(!dragging)return;yaw+=(e.clientX-lastX)*.012;pitch=Math.max(-1.45,Math.min(1.45,pitch+(e.clientY-lastY)*.012));lastX=e.clientX;lastY=e.clientY;requestDraw()}); + canvas.addEventListener('pointerup',()=>dragging=false); + canvas.addEventListener('wheel',e=>{e.preventDefault();zoom=Math.max(.55,Math.min(2.2,zoom*(e.deltaY>0?.9:1.1)));requestDraw()},{passive:false}); + addEventListener('resize',requestDraw); + + window.rovModel = { + load(payload) { + try { + const bytes=Uint8Array.from(atob(payload.data),c=>c.charCodeAt(0)); + vertices=parseStl(bytes.buffer);stage.classList.add('model-loaded'); + document.querySelector('#model-style').disabled=false; + document.querySelector('#model-file-name').textContent=payload.name; + yaw=-.55;pitch=-.42;zoom=1;requestDraw(); + } catch (error) { window.showToast?.(error.message,true); } + }, + toggleStyle() { edgesOnly=!edgesOnly;requestDraw();return edgesOnly; } + }; +})(); diff --git a/SUB Control/web/styles.css b/SUB Control/web/styles.css new file mode 100644 index 0000000..d2510f2 --- /dev/null +++ b/SUB Control/web/styles.css @@ -0,0 +1,12 @@ +:root{color-scheme:dark;--bg:#0d1113;--rail:#161c1f;--panel:#f1f3f0;--card:#fff;--ink:#141719;--muted:#747a7d;--line:#dfe3de;--acid:#c9f277;--cyan:#72d8e8;--amber:#f2c675;--red:#f0958b;--shadow:0 25px 70px #0007;font-family:Inter,"SF Pro Display","Helvetica Neue",Arial,sans-serif} +.window-bar{height:38px;display:flex;align-items:center;justify-content:space-between;background:#111719;color:#aeb6b8;border-bottom:1px solid #ffffff10;user-select:none}.window-drag{display:flex;align-items:center;align-self:stretch;flex:1;gap:9px;padding-left:13px}.window-drag strong{font-size:10px;letter-spacing:.04em;color:#d8dddc}.window-drag i{font-size:9px;font-style:normal;color:#667175}.mini-mark{display:grid;place-items:center;width:19px;height:19px;border-radius:5px;background:var(--acid);color:#1d2818;font-size:9px;font-weight:900}.window-actions{display:flex;height:100%}.window-actions button{width:46px;height:100%;border:0;border-radius:0;background:transparent;color:#9ca5a7;cursor:pointer;font-size:15px}.window-actions button:hover{background:#ffffff0c;color:#fff}.window-actions .window-close:hover{background:#c84c43;color:#fff} +*{box-sizing:border-box}html,body{height:100%;margin:0;overflow:hidden;background:#111719;font-size:14px}button,select,input{font:inherit}.app-shell{width:100%;height:calc(100% - 38px);margin:0;display:grid;grid-template-columns:270px 1fr;border:0;border-radius:0;overflow:hidden;background:var(--panel)} +.vehicle-rail{position:relative;display:flex;flex-direction:column;padding:25px 24px 22px;background:linear-gradient(155deg,#1c2428,#111618);color:#f4f6f3;overflow:hidden}.brand{display:flex;align-items:center;gap:10px;letter-spacing:.08em;font-size:12px;font-weight:700}.brand small{margin-left:auto;color:#758086}.brand-mark{display:grid;place-items:center;width:27px;height:27px;border-radius:9px;background:var(--acid);color:#1d2818;font-weight:900}.eyebrow{font-size:9px;letter-spacing:.16em;color:#899295;font-weight:750}.vehicle-rail>.eyebrow{margin-top:30px}.rov-stage{position:relative;height:min(41vh,330px);min-height:240px;display:grid;place-items:center}.rov-stage svg{position:relative;width:210px;height:270px;z-index:2}.water-ring{position:absolute;border:1px solid #78939d24;border-radius:50%;transform:scaleY(.5)}.water-ring.r1{width:230px;height:230px}.water-ring.r2{width:165px;height:165px;border-color:#9ec7d235}.vehicle-name{display:flex;flex-direction:column;gap:5px}.vehicle-name small,.battery-card small{font-size:9px;letter-spacing:.14em;color:#778186;font-style:normal}.vehicle-name strong{font-size:17px}.vehicle-name span{font-size:10px;color:#7d878b}.rail-bottom{margin-top:auto}.battery-card{display:grid;grid-template-columns:1fr auto;width:100%;padding:0 0 19px;text-align:left;color:inherit;border:0;border-bottom:1px solid #ffffff17;cursor:pointer}.battery-card>span{display:flex;flex-direction:column;gap:3px}.battery-card strong{font-size:19px}.battery-card i{align-self:end;color:#8c979b;font-size:11px;font-style:normal}.battery-card b{grid-column:1/-1;height:5px;margin-top:10px;background:#353d40;border-radius:20px;overflow:hidden}.battery-card em{display:block;width:0;height:100%;background:var(--acid);border-radius:inherit;transition:width .35s}.ghost{background:none}.link-state{display:flex;align-items:center;gap:9px;margin-top:17px;font-size:10px;letter-spacing:.08em;color:#a9b0b2}.link-state span{width:7px;height:7px;border-radius:50%;background:var(--red);box-shadow:0 0 0 4px #f0958b18}.link-state.online span{background:var(--acid);box-shadow:0 0 0 4px #c9f27718} +.workspace{min-width:0;display:flex;flex-direction:column;padding:26px 30px 18px;color:var(--ink);background:linear-gradient(145deg,#f7f8f6,#ecefeb)}.topbar{display:flex;align-items:center;justify-content:space-between;padding-bottom:20px;border-bottom:1px solid var(--line)}h1{margin:3px 0 0;font-size:30px;letter-spacing:-.04em}.connection{display:flex;align-items:center;gap:7px}select,input{border:1px solid #d7dbd6;background:#fff;color:#25292b;border-radius:11px;outline:0}.connection select{width:155px;height:38px;padding:0 12px}.button,.icon-button{height:38px;border:1px solid transparent;border-radius:12px;padding:0 16px;color:#28302a;background:#e9ece8;cursor:pointer;transition:transform .12s,background .12s,box-shadow .12s}.button:hover,.icon-button:hover{transform:translateY(-1px);box-shadow:0 5px 14px #28322c18}.icon-button{width:39px;padding:0;font-weight:700}.connect-button{background:#1d2421;color:white}.drive-grid{display:grid;grid-template-columns:minmax(380px,1fr) 230px;gap:16px;min-height:0;flex:1;padding:18px 0}.control-card{display:flex;flex-direction:column;min-height:0;padding:18px 20px 14px;border:1px solid #e0e3df;border-radius:22px;background:#fff;box-shadow:0 12px 34px #3039340b}.control-card header{display:flex;justify-content:space-between;align-items:start}.control-card header div{display:flex;flex-direction:column;gap:3px}.control-card small,.modal>small{font-size:9px;letter-spacing:.14em;color:#8b9190;font-weight:750}.control-card header strong{font-size:15px}.key-hint{padding:6px 9px;border-radius:8px;background:#f0f2ef;color:#838887;font-size:9px;font-weight:700;letter-spacing:.08em}.control-card footer{display:flex;justify-content:center;gap:25px;color:#7e8482;font-size:10px}.control-card footer span{display:flex;align-items:center;gap:4px}.control-card footer b{color:#353a38}.dot{width:6px;height:6px;border-radius:50%;background:var(--cyan)}.dot.yaw{background:var(--amber)}.dot.depth{background:#bc9feb} +.joystick{--size:min(36vh,300px);position:relative;width:var(--size);height:var(--size);min-width:220px;min-height:220px;margin:auto;border:1px solid #d5d9d5;border-radius:50%;background:radial-gradient(circle at 45% 38%,#f8f9f7 0 20%,#ecefeb 70%);box-shadow:inset 0 3px 8px #fff,inset 0 -8px 18px #2630280c;user-select:none;touch-action:none}.joystick:after{content:"";position:absolute;inset:23%;border:1px solid #dce0dc;border-radius:50%}.axis{position:absolute;background:#ccd1cc}.vertical-axis{top:8%;bottom:8%;left:50%;width:1px}.horizontal-axis{left:8%;right:8%;top:50%;height:1px}.axis-label{position:absolute;color:#979c99;font-size:8px;font-weight:750;letter-spacing:.12em}.forward{top:7px;left:50%;transform:translateX(-50%)}.reverse{bottom:7px;left:50%;transform:translateX(-50%)}.left{left:6px;top:50%;transform:translateY(-50%) rotate(-90deg)}.right{right:3px;top:50%;transform:translateY(-50%) rotate(90deg)}.stick-shadow{position:absolute;left:50%;top:50%;width:68px;height:68px;border-radius:50%;background:#21282422;filter:blur(9px);transform:translate(-45%,-35%)}.stick{position:absolute;z-index:3;left:50%;top:50%;width:62px;height:62px;border-radius:50%;transform:translate(-50%,-50%);background:linear-gradient(145deg,#4b5558,#252c2f);border:1px solid #111719;box-shadow:inset 0 2px 2px #ffffff38,0 10px 18px #18201d40;will-change:transform}.stick span{position:absolute;inset:18px;border-radius:50%;border:1px solid #a9b3b544} +.vertical-control{display:flex;flex:1;min-height:240px;flex-direction:column;align-items:center;gap:8px;padding:16px 0 12px;color:#989d9a;font-size:8px;font-weight:750;letter-spacing:.12em}.heave-track{position:relative;width:38px;flex:1;min-height:180px;border-radius:22px;background:linear-gradient(90deg,#e3e6e2,#f3f4f2 50%,#dfe3de);border:1px solid #d8dcd7;box-shadow:inset 0 3px 7px #9ca59f25;touch-action:none}.heave-center{position:absolute;left:7px;right:7px;top:50%;height:1px;background:#adb4af}.heave-fill{position:absolute;left:12px;right:12px;top:50%;height:0;background:#bc9feb88}.heave-knob{position:absolute;left:50%;top:50%;width:56px;height:35px;border-radius:18px;background:linear-gradient(145deg,#4b5558,#252c2f);border:1px solid #151a1d;box-shadow:0 8px 14px #1d252238,inset 0 1px #ffffff36;transform:translate(-50%,-50%);will-change:top} +.command-bar{display:flex;align-items:center;gap:10px;min-height:57px;padding:10px 12px;border:1px solid #dde1dc;border-radius:17px;background:#fff}.commands{display:flex;gap:7px}.subtle{background:#f0f2ef;border-color:#e4e7e2}.arm{background:#ddf1bd;color:#29371f}.danger{background:#f7dfdc;color:#7b3029}.output-limit{display:flex;flex:1;align-items:center;gap:12px;margin:0 10px;color:#747a77;font-size:10px}.output-limit span{white-space:nowrap}.output-limit b{color:#222724}.output-limit input{width:100%;accent-color:#27302b}.status-line{display:flex;align-items:center;gap:9px;min-height:27px;color:#747b78;font-size:10px}.status-line span{width:6px;height:6px;border-radius:50%;background:#96a09a}.status-line p{white-space:nowrap;overflow:hidden;text-overflow:ellipsis} +.modal{width:min(620px,calc(100vw - 40px));padding:27px;border:1px solid #ffffff24;border-radius:24px;background:#f7f8f5;color:#1c2021;box-shadow:0 35px 100px #000b}.modal::backdrop{background:#071012b5;backdrop-filter:blur(8px)}.modal h2{margin:5px 0 17px;font-size:25px}.modal-close{position:absolute;top:18px;right:18px;width:34px;height:34px;border:0;border-radius:50%;background:#e8ebe6;color:#555;cursor:pointer;font-size:20px}.form-grid{display:grid;gap:13px}.form-grid.two{grid-template-columns:1fr 1fr}.form-grid.three{grid-template-columns:repeat(3,1fr)}.form-grid label{display:flex;flex-direction:column;gap:6px;color:#737976;font-size:10px}.form-grid input,.form-grid select{width:100%;height:40px;padding:0 11px}.note{color:#7a807d;font-size:11px;line-height:1.5}.modal-actions{display:flex;justify-content:flex-end;gap:8px;margin-top:22px}.modal-actions.wide{grid-column:1/-1}.motor-modal{width:min(760px,calc(100vw - 40px))}.motor-head,.motor-row{display:grid;grid-template-columns:70px 1fr 110px 94px;align-items:center;gap:9px}.motor-head{padding:8px 10px;color:#969b99;font-size:9px;letter-spacing:.1em}.motor-list{max-height:390px;overflow:auto}.motor-row{min-height:47px;padding:5px 10px;border-top:1px solid #e3e6e1}.motor-row select{height:34px;padding:0 9px}.motor-row .button{height:32px;padding:0 10px}.toast{position:fixed;z-index:20;left:50%;bottom:28px;padding:11px 16px;border-radius:12px;background:#202725;color:#fff;box-shadow:0 10px 35px #0006;transform:translate(-50%,25px);opacity:0;pointer-events:none;transition:.2s}.toast.show{transform:translate(-50%,0);opacity:1}.toast.error{background:#77352f} +@media(max-width:950px){.app-shell{grid-template-columns:225px minmax(0,1fr)}.vehicle-rail{padding-left:18px;padding-right:18px}.drive-grid{grid-template-columns:minmax(330px,1fr) 190px}.workspace{padding-left:22px;padding-right:22px}.connection select{width:120px}}@media(max-height:700px){.rov-stage{height:240px}.drive-grid{padding:12px 0}.joystick{--size:min(32vh,250px)}.workspace{padding-top:18px}} +.control-card{border-radius:14px}.command-bar{border-radius:12px}.button,.icon-button,.connection select,select,input{border-radius:9px}.modal{border-radius:16px}.brand-mark{border-radius:7px} +.rov-stage #model-canvas{display:none;position:absolute;inset:0;width:100%;height:100%;z-index:3;cursor:grab;touch-action:none}.rov-stage #model-canvas:active{cursor:grabbing}.rov-stage.model-loaded #default-model{display:none}.rov-stage.model-loaded #model-canvas{display:block}.model-tools{position:absolute;z-index:5;left:50%;bottom:5px;display:flex;gap:5px;transform:translateX(-50%);opacity:0;transition:opacity .18s}.rov-stage:hover .model-tools{opacity:1}.model-tools button{height:27px;padding:0 9px;border:1px solid #ffffff18;border-radius:7px;background:#20282bcc;color:#aab3b5;font-size:8px;letter-spacing:.06em;cursor:pointer;backdrop-filter:blur(7px)}.model-tools button:hover{color:#fff;background:#303a3e}.model-tools button:disabled{opacity:.35;cursor:default} diff --git a/SUB Control/web_ui.py b/SUB Control/web_ui.py new file mode 100644 index 0000000..3303a43 --- /dev/null +++ b/SUB Control/web_ui.py @@ -0,0 +1,298 @@ +from __future__ import annotations + +import json +import os +import queue +import threading +import time +import base64 +from pathlib import Path + +import webview +from serial.tools import list_ports + +from app import Axes, MOTOR_FUNCTIONS, MavlinkWorker, motor_number_for_function + + +class ControlApi: + """Small, local-only bridge between the CSS UI and the MAVLink worker.""" + + def __init__(self): + self.events: queue.Queue = queue.Queue() + self.worker = MavlinkWorker(self.events) + self.lock = threading.Lock() + self.state = { + "connected": False, + "link": "DISCONNECTED", + "vehicle": "No heartbeat", + "armed": False, + "status": "Ready. Select a USB port to begin.", + "battery": None, + "log_path": None, + "servos": {str(i): {"function": None, "direction": None} for i in range(1, 17)}, + } + self.servo_functions: dict[int, int] = {} + self.motor_directions: dict[int, int] = {} + self.battery_path = Path(__file__).resolve().parent / "battery_settings.json" + self.model_path = Path(__file__).resolve().parent / "model_settings.json" + self.window = None + self.maximized = False + self.battery_config = { + "cells": 4, "capacity_mah": 5500, "c_rating": 35, + "full_v_per_cell": 4.2, "empty_v_per_cell": 3.3, + "warning_percent": 20, + } + try: + self.battery_config.update(json.loads(self.battery_path.read_text(encoding="utf-8"))) + except (OSError, ValueError, TypeError): + pass + + def initial_state(self): + return { + "ports": [p.device for p in list_ports.comports()], + "battery_config": self.battery_config, + "state": self._snapshot(), + } + + def refresh_ports(self): + return [p.device for p in list_ports.comports()] + + def minimize_window(self): + if self.window: + self.window.minimize() + return True + + def toggle_maximize_window(self): + if not self.window: + return False + if self.maximized: + self.window.restore() + else: + self.window.maximize() + self.maximized = not self.maximized + return self.maximized + + def close_window(self): + if self.window: + self.window.destroy() + return True + + def choose_model(self): + if not self.window: + raise RuntimeError("Window is not ready") + selected = self.window.create_file_dialog( + webview.OPEN_DIALOG, + allow_multiple=False, + file_types=("STL mesh (*.stl)", "STEP model (*.step;*.stp)"), + ) + if not selected: + return None + path = Path(selected[0]) + if path.suffix.lower() in (".step", ".stp"): + raise ValueError( + "STEP is a CAD solid and must be tessellated first. Export it as a binary STL, then load that file." + ) + if path.suffix.lower() != ".stl": + raise ValueError("Choose an STL mesh") + if path.stat().st_size > 30 * 1024 * 1024: + raise ValueError("The STL is larger than 30 MB; export a lighter mesh") + self.model_path.write_text(json.dumps({"path": str(path)}, indent=2) + "\n", encoding="utf-8") + return self._model_payload(path) + + def saved_model(self): + try: + path = Path(json.loads(self.model_path.read_text(encoding="utf-8"))["path"]) + if path.is_file() and path.suffix.lower() == ".stl" and path.stat().st_size <= 30 * 1024 * 1024: + return self._model_payload(path) + except (OSError, ValueError, TypeError, KeyError): + pass + return None + + @staticmethod + def _model_payload(path): + return {"name": path.name, "data": base64.b64encode(path.read_bytes()).decode("ascii")} + + def connect(self, port, baud=57600, rate=20): + if not port: + raise ValueError("Select a USB serial port") + 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…", + ) + return True + + def disconnect(self): + self.worker.disconnect() + return True + + def set_axes(self, forward, turn, vertical): + self.worker.set_axes(Axes( + max(-1.0, min(1.0, float(forward))), + 0.0, + max(-1.0, min(1.0, float(vertical))), + max(-1.0, min(1.0, float(turn))), + )) + return True + + def set_gain(self, value): + value = max(5, min(50, int(value))) + self.worker.gain = value / 100.0 + return value + + def command(self, name): + if name == "manual": + self.worker.log("OPERATOR", "clicked SET MANUAL") + self.worker.set_manual_mode() + elif name == "arm": + self.worker.log("OPERATOR", "clicked ARM") + self.worker.command_arm(True) + elif name == "disarm": + self.worker.log("OPERATOR", "clicked DISARM") + self.worker.command_arm(False) + else: + raise ValueError("Unknown command") + return True + + def set_servo_function(self, servo, motor): + servo, motor = int(servo), int(motor) + 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) + self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function) + return True + + def reverse_motor(self, servo): + servo = int(servo) + motor = motor_number_for_function(self.servo_functions.get(servo)) + if motor is None: + raise ValueError(f"SERVO{servo} is not assigned to a motor") + current = self.motor_directions.get(motor) + if current is None: + raise ValueError(f"MOT_{motor}_DIRECTION has not been read yet") + self.worker.set_parameter(f"MOT_{motor}_DIRECTION", -1 if current == 1 else 1) + return True + + def save_battery(self, config): + clean = { + "cells": int(config["cells"]), + "capacity_mah": int(config["capacity_mah"]), + "c_rating": float(config["c_rating"]), + "full_v_per_cell": float(config["full_v_per_cell"]), + "empty_v_per_cell": float(config["empty_v_per_cell"]), + "warning_percent": int(config["warning_percent"]), + } + if clean["cells"] < 1 or clean["capacity_mah"] <= 0 or clean["c_rating"] <= 0: + raise ValueError("Battery cells, capacity and C rating must be positive") + if not clean["full_v_per_cell"] > clean["empty_v_per_cell"] > 0: + raise ValueError("Full voltage per cell must be greater than empty voltage") + if not 0 <= clean["warning_percent"] <= 100: + raise ValueError("Warning percentage must be between 0 and 100") + self.battery_config = clean + self.battery_path.write_text(json.dumps(clean, indent=2) + "\n", encoding="utf-8") + return clean + + def open_logs(self): + path = Path(__file__).resolve().parent / "logs" + path.mkdir(exist_ok=True) + os.startfile(path) + return True + + def poll(self): + while True: + try: + event = self.events.get_nowait() + except queue.Empty: + break + self._consume(event) + if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 5: + with self.lock: + self.state["link"] = ( + "DATA OK / HEARTBEAT DELAYED" + if time.monotonic() - self.worker.last_rx < 2 else "ALL LINK DATA LOST" + ) + return self._snapshot() + + def _snapshot(self): + with self.lock: + return json.loads(json.dumps(self.state)) + + def _consume(self, event): + kind = event[0] + with self.lock: + if kind == "heartbeat": + _, system, autopilot, mode, armed = event + self.state.update( + connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}", + armed=bool(armed), + ) + elif kind in ("status", "statustext"): + self.state["status"] = event[1] + elif kind == "ack": + self.state["status"] = f"Command {event[1]} ACK: {event[2]}" + elif kind == "battery": + voltage = float(event[1]) + cells = self.battery_config["cells"] + per_cell = voltage / cells + low, high = self.battery_config["empty_v_per_cell"], self.battery_config["full_v_per_cell"] + percent = max(0, min(100, (per_cell - low) * 100 / (high - low))) + self.state["battery"] = { + "voltage": voltage, "per_cell": per_cell, "percent": percent, + "cells": cells, "low": percent <= self.battery_config["warning_percent"], + } + elif kind == "servo_param": + _, param_id, value = event + servo = int(param_id[5:param_id.index("_")]) + self.servo_functions[servo] = int(value) + self._update_servo(servo) + elif kind == "motor_direction": + _, motor, value = event + self.motor_directions[int(motor)] = int(value) + for servo in range(1, 17): + self._update_servo(servo) + elif kind == "param_saved": + self.state["status"] = f"Confirmed: {event[1]}={event[2]}" + elif kind == "log_path": + self.state["log_path"] = event[1] + elif kind == "error": + self.state["status"] = event[1] + elif kind == "disconnected": + self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat", + armed=False, battery=None, status="Disconnected") + + def _update_servo(self, servo): + function = self.servo_functions.get(servo) + motor = motor_number_for_function(function) + self.state["servos"][str(servo)] = { + "function": function, + "label": MOTOR_FUNCTIONS.get(function, "Unassigned") if function is not None else "Waiting…", + "motor": motor, + "direction": self.motor_directions.get(motor) if motor else None, + } + + def close(self): + self.worker.disconnect() + + +def main(): + api = ControlApi() + ui_path = Path(__file__).resolve().parent / "web" / "index.html" + window = webview.create_window( + "Simple ROV Control", url=str(ui_path), js_api=api, + width=1120, height=760, min_size=(900, 650), + background_color="#111719", frameless=True, easy_drag=False, + shadow=True, + ) + api.window = window + window.events.maximized += lambda: setattr(api, "maximized", True) + window.events.restored += lambda: setattr(api, "maximized", False) + try: + webview.start(debug=False) + finally: + api.close() + + +if __name__ == "__main__": + main()