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