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 # ESC_TELEMETRY_1_TO_4 uses a MAVLink 2 message ID. Select the v2 dialect # before importing pymavlink so received ESC packets can be decoded/logged. os.environ["MAVLINK20"] = "1" 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 def normalize_endpoint(port: str) -> str: p = str(port).strip() if not p: return p lower = p.lower() if "sim" in lower or "5760" in lower or lower in ("tcp", "tcp:", "tcp:127.0.0.1", "127.0.0.1"): return "tcp:127.0.0.1:5760" if "14550" in lower or lower in ("udp", "udp:", "udpin", "udpin:"): return "udpin:127.0.0.1:14550" if p.startswith("127.0.0.1:") or p.startswith("localhost:"): num = p.split(":")[1] if num == "14550": return f"udpin:{p}" return f"tcp:{p}" if ":" in p and not p.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:")): return f"tcp:{p}" return p class MavlinkWorker: def __init__(self, events: queue.Queue): self.events = events self.connection = None self.stop_event = threading.Event() self.lock = threading.Lock() # pymavlink writes are not thread-safe. The web/API thread can issue # commands while this worker sends heartbeats and controls, so serialize # every packet before it reaches a low-bandwidth serial link. self.tx_lock = threading.Lock() self.thread = None self.axes = Axes() self.output_axes = Axes() self.last_control_at = 0.0 # Enabled by the web bridge only; the legacy Tk frontend is unchanged. self.require_input_lease = False self.last_input_at = 0.0 self.last_input_sequence = -1 self.input_timeout = 1.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.independent_motor_scaling = False self.motor_gains = {motor: 20 for motor in range(1, 13)} 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 # Keep configuration traffic off the link during an arm/disarm # transition, including the wait for the vehicle's confirmation. self.command_quiet_until = 0.0 self.disarm_requested_until = 0.0 self.pending_arm_state = None self.next_arm_retry_at = 0.0 self.arm_retry_deadline = 0.0 self.arm_confirmation = 0 self.stream_config = deque() self.next_stream_config_at = 0.0 # Low-rate TIMESYNC echoes measure PC↔FC round trips with minimal traffic. self.ping_rate_hz = 1 self.heavy_quality_test = None self.heavy_pending_pings = {} self.heavy_quality_latest = None self.telemetry_rates_hz = { "SYS_STATUS": 1, "BATTERY_STATUS": 1, "SERVO_OUTPUT_RAW": 2, "RC_CHANNELS": 2, "ESC_TELEMETRY_1_TO_4": 1, } self.ping_sequence = 0 self.next_ping_at = 0.0 self.pending_pings = {} self.ping_results = deque(maxlen=300) self.bad_frame_times = deque() self.bad_frame_bytes = deque() self.rx_sequence_by_source = {} self.rx_delivery_results = deque(maxlen=1024) self.next_quality_publish_at = 0.0 self.param_scan = deque() self.param_waiting = None self.param_deadline = 0.0 self.param_retries = 0 self.param_types = {} self.pending_sets = {} self.motor_test_lock = threading.Lock() self.motor_test_request = None self.motor_test_mode = False self.motor_test_acknowledged = False self.motor_test_last_motor = None self.log_lock = threading.Lock() self.log_file = None self.log_path = None self.console_lines = deque(maxlen=500) self.console_rx_lines = deque(maxlen=300) self.console_tx_lines = deque(maxlen=300) def connect(self, port: str, baud: int): print(f"[MavlinkWorker] connect() called: port={port!r}, baud={baud}") if self.thread and self.thread.is_alive(): print(f"[MavlinkWorker] Thread already alive, ignoring connect()") return self.stop_event.clear() log_dir = Path(__file__).resolve().parent / "logs" log_dir.mkdir(exist_ok=True) self.log_path = log_dir / f"ardusub_{datetime.now():%Y%m%d_%H%M%S}.log" self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1) normalized = normalize_endpoint(port) print(f"[MavlinkWorker] normalized endpoint: {normalized!r}") self.log("SESSION", f"start port={normalized} (input={port}) baud={baud} " f"control_hz={self.rate_hz} rx=MAVLink2 tx=MAVLink2") self.events.put(("log_path", str(self.log_path))) self.last_heartbeat = 0.0 self.last_rx = 0.0 self.next_quality_publish_at = 0.0 self.thread = threading.Thread( target=self._run, args=(normalized, baud), daemon=True ) self.thread.start() print(f"[MavlinkWorker] worker thread started") def _run(self, port: str, baud: int): print(f"[MavlinkWorker._run] ENTERED: port={port!r}, baud={baud}") try: print(f"[MavlinkWorker._run] Creating mavlink_connection...") conn = mavutil.mavlink_connection( port, baud=baud, source_system=255, source_component=190, # Fail a missing TCP simulator immediately. pymavlink otherwise # retries the refused socket several times and leaves the UI in # "Opening" long after the actual failure. autoreconnect=False, retries=0, force_connected=True, ) print(f"[MavlinkWorker._run] mavlink_connection created: {type(conn).__name__}") self.connection = conn if port.startswith("tcp:"): self.events.put(("status", f"Connected to {port}; waiting for vehicle heartbeat…")) else: self.events.put(("status", f"Port {port} open; waiting for heartbeat at {baud} baud…")) # Send initial GCS heartbeat immediately self.log("TX_HEARTBEAT", "GCS initial") self._send_mav(conn, "heartbeat_send", mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, mavutil.mavlink.MAV_STATE_ACTIVE, ) print(f"[MavlinkWorker._run] Initial heartbeat sent, entering recv loop") next_gcs_hb = time.monotonic() + 1.0 next_control = 0.0 while not self.stop_event.is_set(): now = time.monotonic() if now >= next_gcs_hb: self.log("TX_HEARTBEAT", "GCS") self._send_mav(conn, "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 # ArduSub cannot use MANUAL_CONTROL while disarmed. Keeping a # 20 Hz neutral stream alive here only competes with ARM and # its acknowledgement on constrained links. motor_test_mode = self._tick_motor_test(now) if self.target_system and self.armed and now >= next_control: if not motor_test_mode: self._send_control() # The first disarm packet is preceded by a neutral command. # Until disarm is confirmed, keep sending neutral at a # lower rate instead of consuming 20 Hz on the weak link. control_hz = min(self.rate_hz, 5) if now < self.disarm_requested_until else self.rate_hz next_control = now + (1.0 / max(1, control_hz)) elif not self.armed: next_control = now if self.target_system: self._tick_arm_retry(now) self._tick_link_probe(now) self._tick_stream_config(now) self._tick_param_scan(now) if now >= self.next_quality_publish_at: self._publish_link_quality() self.next_quality_publish_at = now + 1.0 msg = conn.recv_match(blocking=False) if msg: self._handle_message(msg) else: time.sleep(0.01) except Exception as exc: print(f"[MavlinkWorker._run] EXCEPTION: {exc}") self.events.put(("error", f"Connection error: {exc}")) finally: if self.heavy_quality_test is not None: self.cancel_heavy_quality_test() 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.command_quiet_until = 0.0 self.disarm_requested_until = 0.0 self.pending_arm_state = None self.next_arm_retry_at = 0.0 self.arm_retry_deadline = 0.0 self.arm_confirmation = 0 with self.motor_test_lock: self.motor_test_request = None self.motor_test_mode = False self.motor_test_last_motor = None self.stream_config.clear() self.pending_pings.clear() self.ping_results.clear() self.bad_frame_times.clear() self.bad_frame_bytes.clear() self.rx_sequence_by_source.clear() self.rx_delivery_results.clear() 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") line = f"{timestamp} [{category}] {message}" with self.log_lock: self.console_lines.append(line[:2000]) if category.startswith("RX") or category == "ONBOARD_LOG_ERROR": self.console_rx_lines.append(line[:2000]) elif category.startswith("TX") or category == "OPERATOR": self.console_tx_lines.append(line[:2000]) if self.log_file: self.log_file.write(line + "\n") def read_console(self, limit=150): with self.log_lock: limit = max(1, min(300, int(limit))) return { "rx": list(self.console_rx_lines)[-limit:], "tx": list(self.console_tx_lines)[-limit:], } def _send_mav(self, connection, method: str, *args): """Serialize MAVLink 2 writes and protect the start of each burst.""" with self.tx_lock: # This full-duplex module pair loses the opening bytes of some # PC-to-FC bursts after the line has been idle. Put a valid, # expendable GCS heartbeat first so the following packet remains # intact and the FC also sees a healthy GCS heartbeat stream. if method != "heartbeat_send" and hasattr(connection.mav, "heartbeat_send"): connection.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, mavutil.mavlink.MAV_STATE_ACTIVE, ) getattr(connection.mav, method)(*args) def _handle_message(self, msg): msg_type = msg.get_type() if msg_type == "BAD_DATA": now = time.monotonic() self.bad_frame_times.append(now) self.bad_frame_bytes.append((now, len(getattr(msg, "data", b"")))) self._trim_bad_frames(now) self.log("RX_BAD_DATA", repr(getattr(msg, "data", b""))) return self.last_rx = time.monotonic() self._track_rx_delivery(msg) try: payload = json.dumps(msg.to_dict(), default=str, separators=(",", ":")) except Exception: payload = str(msg) self.log("RX", payload) if msg_type == "TIMESYNC" and int(getattr(msg, "tc1", 0)) != 0: token = int(getattr(msg, "ts1", -1)) heavy_probe = self.heavy_pending_pings.pop(token, None) if heavy_probe is not None: stage_index, sent_at = heavy_probe self._record_heavy_probe(stage_index, (time.monotonic() - sent_at) * 1000.0) self._publish_heavy_quality() sent_at = None else: sent_at = self.pending_pings.pop(token, None) if sent_at is not None: rtt_ms = (time.monotonic() - sent_at) * 1000.0 self.ping_results.append((time.monotonic(), rtt_ms)) self.log("RX_LINK_PROBE", f"timesync={token} rtt_ms={rtt_ms:.1f}") self._publish_link_quality() elif msg_type == "HEARTBEAT" and getattr(msg, "type", 0) != mavutil.mavlink.MAV_TYPE_GCS: if getattr(msg, "autopilot", mavutil.mavlink.MAV_AUTOPILOT_INVALID) == mavutil.mavlink.MAV_AUTOPILOT_INVALID: return if self.target_system and (msg.get_srcSystem(), msg.get_srcComponent()) != (self.target_system, self.target_component): return 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 if not armed: with self.motor_test_lock: was_testing = self.motor_test_mode self.motor_test_request = None self.motor_test_mode = False if was_testing: self.events.put(("motor_test_stopped", "Vehicle disarmed")) if self.pending_arm_state is not None and armed == self.pending_arm_state: self.pending_arm_state = None if not armed: self.disarm_requested_until = 0.0 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, self.last_heartbeat)) if first: self.next_ping_at = time.monotonic() self.events.put(("status", "Heartbeat received; controls are neutral.")) self._configure_low_bandwidth_streams() self._start_servo_scan() elif msg_type == "COMMAND_ACK": if (msg.get_srcSystem(), msg.get_srcComponent()) != (self.target_system, self.target_component): return result = mavutil.mavlink.enums["MAV_RESULT"].get(msg.result) result_name = result.name if result else str(msg.result) if int(msg.command) == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL: self.log("RX_STREAM_CONFIG_ACK", result_name) return if int(msg.command) == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST: self.log("RX_MOTOR_TEST_ACK", result_name) if result_name in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS"): if not self.motor_test_acknowledged: self.motor_test_acknowledged = True self.events.put(("motor_test_ack", result_name)) else: with self.motor_test_lock: self.motor_test_request = None self.motor_test_mode = False self.events.put(("motor_test_rejected", result_name)) return if (int(msg.command) == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM and result_name not in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS")): self.pending_arm_state = None self.events.put(("ack", int(msg.command), result_name, self.last_rx)) elif msg_type == "STATUSTEXT": text = msg.text if isinstance(text, bytes): text = text.decode(errors="replace") text = text.rstrip("\x00") if "/APM/LOGS" in text and "Failed" in text: self.log("ONBOARD_LOG_ERROR", text) self.events.put(("statustext", text)) elif msg_type == "SYS_STATUS": voltage_mv = int(msg.voltage_battery) current_ca = int(msg.current_battery) remaining = int(msg.battery_remaining) self.events.put(("battery", { "voltage": voltage_mv / 1000.0 if 0 < voltage_mv < 65535 else None, "current_a": current_ca / 100.0 if current_ca >= 0 else None, "remaining_percent": remaining if 0 <= remaining <= 100 else None, })) elif msg_type == "BATTERY_STATUS": voltages = [int(value) for value in getattr(msg, "voltages", [])] valid_voltages = [value for value in voltages if 0 < value < 65535] voltage = sum(valid_voltages) / 1000.0 if valid_voltages else None current_ca = int(msg.current_battery) consumed = int(msg.current_consumed) remaining = int(msg.battery_remaining) self.events.put(("battery", { "voltage": voltage, "current_a": current_ca / 100.0 if current_ca >= 0 else None, "consumed_mah": consumed if consumed >= 0 else None, "remaining_percent": remaining if 0 <= remaining <= 100 else None, })) elif msg_type == "SERVO_OUTPUT_RAW": base = int(getattr(msg, "port", 0)) * 8 + 1 outputs = {} for index in range(1, 9): raw = int(getattr(msg, f"servo{index}_raw", 0)) outputs[str(base + index - 1)] = raw if 0 < raw < 65535 else None self.events.put(("servo_output", outputs)) 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 _trim_bad_frames(self, now): while self.bad_frame_times and now - self.bad_frame_times[0] > 20.0: self.bad_frame_times.popleft() def _track_rx_delivery(self, msg): """Estimate downlink loss from MAVLink's per-source packet sequence.""" try: source = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) sequence = int(msg.get_seq()) except (AttributeError, TypeError, ValueError): return if self.target_system and source != (self.target_system, self.target_component): return previous = self.rx_sequence_by_source.get(source) self.rx_sequence_by_source[source] = sequence if previous is None: return missing = (sequence - previous - 1) & 0xFF # A large backwards jump is a reset/reordered packet, not 200 losses. if missing > 127: return timestamp = time.monotonic() self.rx_delivery_results.extend((timestamp, False) for _ in range(missing)) self.rx_delivery_results.append((timestamp, True)) def _publish_link_quality(self): now = time.monotonic() self._trim_bad_frames(now) while self.ping_results and now - self.ping_results[0][0] > 30.0: self.ping_results.popleft() while self.rx_delivery_results and now - self.rx_delivery_results[0][0] > 30.0: self.rx_delivery_results.popleft() while self.bad_frame_bytes and now - self.bad_frame_bytes[0][0] > 20.0: self.bad_frame_bytes.popleft() samples = [sample for _, sample in self.ping_results] received = [sample for sample in samples if sample is not None] total = len(samples) loss_percent = 100.0 * (total - len(received)) / total if total else None ordered_rtt = sorted(received) latency_ms = (sum(ordered_rtt) / len(ordered_rtt)) if ordered_rtt else None median_rtt_ms = ordered_rtt[len(ordered_rtt) // 2] if ordered_rtt else None p95_rtt_ms = ordered_rtt[min(len(ordered_rtt) - 1, (len(ordered_rtt) * 95 + 99) // 100 - 1)] if ordered_rtt else None rx_received = sum(1 for _, delivered in self.rx_delivery_results if delivered) rx_missing = sum(1 for _, delivered in self.rx_delivery_results if not delivered) rx_loss_percent = 100.0 * rx_missing / (rx_received + rx_missing) if rx_received else None rx_corrupt_bytes = sum(byte_count for _, byte_count in self.bad_frame_bytes) heartbeat_age = (now - self.last_heartbeat) if self.last_heartbeat else None if total < 10 or rx_received < 10 or heartbeat_age is None: grade = "testing" elif (heartbeat_age > 3 or loss_percent > 15 or (rx_loss_percent is not None and rx_loss_percent > 5) or len(self.bad_frame_bytes) >= 5): grade = "poor" elif (loss_percent > 3 or (rx_loss_percent is not None and rx_loss_percent > 1) or (p95_rtt_ms is not None and p95_rtt_ms > 500) or self.bad_frame_bytes): grade = "fair" elif (loss_percent > 0.5 or (rx_loss_percent is not None and rx_loss_percent > 0.2) or (p95_rtt_ms is not None and p95_rtt_ms > 200)): grade = "good" else: grade = "excellent" self.events.put(("link_quality", { "grade": grade, "latency_ms": round(latency_ms, 1) if latency_ms is not None else None, "rtt_median_ms": round(median_rtt_ms, 1) if median_rtt_ms is not None else None, "rtt_p95_ms": round(p95_rtt_ms, 1) if p95_rtt_ms is not None else None, "loss_percent": round(loss_percent, 1) if loss_percent is not None else None, "probe_loss_percent": round(loss_percent, 1) if loss_percent is not None else None, "probe_samples": total, "probe_replies": len(received), "rx_packets_30s": rx_received, "rx_sequence_loss_percent": round(rx_loss_percent, 1) if rx_loss_percent is not None else None, "rx_corrupt_frames_20s": len(self.bad_frame_bytes), "rx_corrupt_bytes_20s": rx_corrupt_bytes, "samples": total, "corrupt_frames_20s": len(self.bad_frame_times), "heartbeat_age_s": round(heartbeat_age, 1) if heartbeat_age is not None else None, "probe_supported": total > 0, })) def _send_motor_test(self, motor, throttle_percent): if not self.connection or not self.target_system: raise RuntimeError("No autopilot heartbeat") self.log("TX_MOTOR_TEST", f"motor={motor} throttle={throttle_percent}%") self._send_mav(self.connection, "command_int_send", self.target_system, self.target_component, mavutil.mavlink.MAV_FRAME_GLOBAL, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, 0, 0, float(motor), 0.0, float(throttle_percent), 1.0, 1, 0, 0, ) def start_motor_test(self, motor, throttle_percent): motor, throttle_percent = int(motor), int(throttle_percent) if not 1 <= motor <= 12 or not 0 <= throttle_percent <= 100: raise ValueError("Choose a motor and a test throttle from 0% to 100%") if not self.connection or not self.target_system: raise RuntimeError("Connect to the vehicle before testing a motor") if not self.armed or time.monotonic() - self.last_heartbeat > 2.0: raise RuntimeError("Arm the vehicle and confirm fresh telemetry before motor testing") with self.motor_test_lock: if self.motor_test_mode: raise RuntimeError("Finish the current motor test before starting another") self.motor_test_request = {"motor": motor, "throttle": throttle_percent, "next_at": 0.0} self.motor_test_last_motor = motor self.motor_test_mode = True self.motor_test_acknowledged = False self.set_axes(Axes()) self.log("OPERATOR", f"held motor test for motor {motor} at {throttle_percent}%") return True def stop_motor_test(self): with self.motor_test_lock: request = self.motor_test_request if request is not None: self.motor_test_last_motor = request["motor"] motor = self.motor_test_last_motor was_testing = self.motor_test_mode self.motor_test_request = None if was_testing and motor and self.connection and self.target_system: self._send_motor_test(motor, 0) return True def _tick_motor_test(self, now): motor = throttle = None with self.motor_test_lock: request = self.motor_test_request active = self.motor_test_mode if request is not None and not self.armed: self.motor_test_request = None self.motor_test_mode = False active = False self.events.put(("motor_test_stopped", "Vehicle disarmed")) elif request is not None and now >= request["next_at"]: request["next_at"] = now + 0.25 motor, throttle = request["motor"], request["throttle"] else: motor = throttle = None if motor is not None: self._send_motor_test(motor, throttle) return active def _tick_link_probe(self, now): if self.heavy_quality_test is not None: self._tick_heavy_quality(now) return expired = [sequence for sequence, sent_at in self.pending_pings.items() if now - sent_at >= 2.5] for sequence in expired: self.pending_pings.pop(sequence, None) self.ping_results.append((now, None)) self.log("LINK_PROBE_TIMEOUT", f"timesync={sequence}") if expired: self._publish_link_quality() if (now < self.next_ping_at or now < self.command_quiet_until or not self.connection or not self.target_system): return token = time.monotonic_ns() self.pending_pings[token] = now self.log("TX_LINK_PROBE", f"timesync={token}") # ArduPilot implements TIMESYNC even on builds that omit PING. A # request has tc1=0; the response mirrors our unique ts1 token. self._send_mav(self.connection, "timesync_send", 0, token) self.next_ping_at = now + (1.0 / max(1, self.ping_rate_hz)) def start_heavy_quality_test(self): if not self.connection or not self.target_system: raise RuntimeError("Connect to the flight controller before testing the link") if self.armed: raise RuntimeError("The heavy link test is available while the vehicle is disarmed") if self.pending_arm_state is not None or time.monotonic() < self.command_quiet_until: raise RuntimeError("Wait for the current vehicle command to finish") if self.heavy_quality_test is not None: raise RuntimeError("A heavy link test is already running") self.pending_pings.clear() self.heavy_pending_pings.clear() self.heavy_quality_latest = { "running": True, "stage": 1, "stages": 3, "rate_hz": 1, "status": "Running", "results": [], } self.heavy_quality_test = { "rates": (1, 5, 10), "duration_s": 10.0, "stage_index": 0, "stage_started": time.monotonic(), "next_probe_at": time.monotonic(), "stages": [{"rate_hz": rate, "sent": 0, "replies": 0, "timeouts": 0, "rtts": []} for rate in (1, 5, 10)], } self.log("TX_HEAVY_TEST", "started rates=1,5,10Hz stage_seconds=10") self._publish_heavy_quality() return self.heavy_quality_latest def cancel_heavy_quality_test(self): if self.heavy_quality_test is None: return self.heavy_quality_latest self.heavy_pending_pings.clear() test = self.heavy_quality_test self.heavy_quality_test = None self.heavy_quality_latest = { "running": False, "stage": 0, "stages": len(test["rates"]), "rate_hz": None, "status": "Cancelled", "results": self._heavy_results(test), } self.next_ping_at = time.monotonic() self.log("TX_HEAVY_TEST", "cancelled") self.events.put(("heavy_quality", self.heavy_quality_latest)) return self.heavy_quality_latest @staticmethod def _heavy_results(test): results = [] for stage in test["stages"]: rtts = sorted(stage["rtts"]) replies = stage["replies"] resolved = replies + stage["timeouts"] results.append({ "rate_hz": stage["rate_hz"], "sent": stage["sent"], "replies": replies, "loss_percent": round(100.0 * stage["timeouts"] / resolved, 1) if resolved else None, "median_rtt_ms": round(rtts[len(rtts) // 2], 1) if rtts else None, "p95_rtt_ms": round(rtts[min(len(rtts) - 1, (len(rtts) * 95 + 99) // 100 - 1)], 1) if rtts else None, }) return results def _record_heavy_probe(self, stage_index, rtt_ms=None): test = self.heavy_quality_test if test is None or not 0 <= stage_index < len(test["stages"]): return stage = test["stages"][stage_index] if rtt_ms is None: stage["timeouts"] += 1 else: stage["replies"] += 1 stage["rtts"].append(rtt_ms) def _publish_heavy_quality(self): test = self.heavy_quality_test if test is None: return index = test["stage_index"] stage = test["stages"][index] self.heavy_quality_latest = { "running": True, "stage": index + 1, "stages": len(test["rates"]), "rate_hz": stage["rate_hz"], "sent": stage["sent"], "replies": stage["replies"], "timeouts": stage["timeouts"], "status": f"Stage {index + 1} of {len(test['rates'])}: {stage['rate_hz']} Hz", "results": self._heavy_results(test), } self.events.put(("heavy_quality", self.heavy_quality_latest)) def _tick_heavy_quality(self, now): test = self.heavy_quality_test if test is None: return for token, (stage_index, sent_at) in list(self.heavy_pending_pings.items()): if now - sent_at >= 2.5: self.heavy_pending_pings.pop(token, None) self._record_heavy_probe(stage_index) self.log("HEAVY_PROBE_TIMEOUT", f"rate={test['rates'][stage_index]}Hz timesync={token}") index = test["stage_index"] stage_pending = any(stage_index == index for stage_index, _ in self.heavy_pending_pings.values()) if now - test["stage_started"] >= test["duration_s"] and not stage_pending: index += 1 if index >= len(test["rates"]): results = self._heavy_results(test) self.heavy_quality_test = None self.heavy_quality_latest = { "running": False, "stage": 3, "stages": 3, "rate_hz": None, "status": "Complete", "results": results, } self.next_ping_at = now self.log("TX_HEAVY_TEST", "complete") self.events.put(("heavy_quality", self.heavy_quality_latest)) return test["stage_index"] = index test["stage_started"] = now test["next_probe_at"] = now self._publish_heavy_quality() index = test["stage_index"] stage = test["stages"][index] if now >= test["next_probe_at"] and now - test["stage_started"] < test["duration_s"]: token = time.monotonic_ns() self.heavy_pending_pings[token] = (index, now) stage["sent"] += 1 self.log("TX_HEAVY_PROBE", f"rate={stage['rate_hz']}Hz timesync={token}") self._send_mav(self.connection, "timesync_send", 0, token) test["next_probe_at"] = now + 1.0 / stage["rate_hz"] if now >= self.next_quality_publish_at: self._publish_heavy_quality() self.next_quality_publish_at = now + 1.0 def set_link_rates(self, ping_hz, telemetry_rates): self.ping_rate_hz = max(1, min(10, int(ping_hz))) for name in self.telemetry_rates_hz: if name in telemetry_rates: self.telemetry_rates_hz[name] = max(0, min(10, int(telemetry_rates[name]))) if self.connection and self.target_system: self._configure_low_bandwidth_streams() 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): """Pace optional telemetry changes; never burst them at arm time.""" if not self.connection: return self.log("TX_STREAM_CONFIG", "stop MAV_DATA_STREAM_ALL") self._send_mav(self.connection, "request_data_stream_send", self.target_system, self.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 0, 0, ) # ATTITUDE and VFR_HUD interval commands were accepted by this FC. # AHRS2 is also unneeded, but an acknowledgement has not been seen. # NAMED_VALUE_FLOAT explicitly returned DENIED, so do not retry it. # A single interval command is sometimes lost, so send two paced # passes. This does not change persistent parameters or onboard logs. unwanted = ( "ATTITUDE", "AHRS2", "VFR_HUD", ) self.stream_config = deque( (name, getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}"), -1) for _ in range(2) for name in unwanted ) for name, rate_hz in self.telemetry_rates_hz.items(): interval_us = -1 if rate_hz == 0 else round(1_000_000 / rate_hz) self.stream_config.append(( name, getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}"), interval_us, )) self.next_stream_config_at = time.monotonic() self.events.put(("status", "Applying low-bandwidth telemetry rates…")) def _tick_stream_config(self, now): if (not self.connection or not self.target_system or self.armed or now < self.command_quiet_until or now < self.next_stream_config_at or not self.stream_config): return name, message_id, interval_us = self.stream_config.popleft() self.log("TX_STREAM_CONFIG", f"{name} interval_us={interval_us}") self._send_mav(self.connection, "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.next_stream_config_at = now + 0.5 def _tick_param_scan(self, now): if (not self.connection or self.config_paused or self.stream_config or now < self.next_stream_config_at or now < self.command_quiet_until 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._send_mav(self.connection, "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._send_mav(self.connection, "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: now = time.monotonic() expired = self.require_input_lease and ( not self.armed or now - self.last_heartbeat > 3.0 or now - self.last_input_at > self.input_timeout) if expired or force_neutral: # Never replay a stored command after telemetry/UI recovery. self.axes = Axes() self.output_axes = Axes() active = not (force_neutral or expired) target = self.axes if active else Axes() gain = self.gain independent_motor_scaling = self.independent_motor_scaling motor_gains = dict(self.motor_gains) 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 if independent_motor_scaling: # The desktop controller's SimpleROV-3 profile maps motors 1/2 # to surge/turn and motor 3 to vertical thrust. Convert the two # independently scaled horizontal demands back into MANUAL_CONTROL # axes so ArduSub's configured mixer remains in charge. motor_1 = (limited_forward + limited_yaw) * motor_gains.get(1, 100) / 100.0 motor_2 = (limited_forward - limited_yaw) * motor_gains.get(2, 100) / 100.0 limited_forward = (motor_1 + motor_2) / 2.0 limited_yaw = (motor_1 - motor_2) / 2.0 vertical_gain = motor_gains.get(3, 100) / 100.0 x = round(limited_forward * 1000) y = round(axes.lateral * 1000) r = round(limited_yaw * 1000) z = round(NEUTRAL_Z + axes.vertical * vertical_gain * 500) else: 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})", ) self._send_mav(conn, "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 arm and self.heavy_quality_test is not None: self.cancel_heavy_quality_test() now = time.monotonic() self.command_quiet_until = now + 6.0 if not arm: self.disarm_requested_until = now + 6.0 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") self.pending_arm_state = bool(arm) self.next_arm_retry_at = now + 0.25 self.arm_retry_deadline = now + 5.0 self.arm_confirmation = 0 self._send_arm_packet(arm) def _send_arm_packet(self, arm: bool): conn = self.connection if not conn or not self.target_system: return self._send_mav(conn, "command_long_send", self.target_system, self.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, self.arm_confirmation, 1.0 if arm else 0.0, 0, 0, 0, 0, 0, 0, ) def _tick_arm_retry(self, now): if self.pending_arm_state is None or now < self.next_arm_retry_at: return if now >= self.arm_retry_deadline: self.log("TX_COMMAND_TIMEOUT", "ARM" if self.pending_arm_state else "DISARM") self.pending_arm_state = None return self.log("TX_COMMAND_RETRY", "ARM" if self.pending_arm_state else "DISARM") self.arm_confirmation = min(255, self.arm_confirmation + 1) self._send_arm_packet(self.pending_arm_state) self.next_arm_retry_at = now + 0.25 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}") self._send_mav(conn, "set_mode_send", self.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, manual, ) def set_motor_scaling(self, independent, gains): clean = {motor: max(0, min(100, int(gains.get(motor, 100)))) for motor in range(1, 13)} with self.lock: self.independent_motor_scaling = bool(independent) self.motor_gains = clean def motor_output_scale(self, motor): with self.lock: return (self.motor_gains.get(int(motor), 100) / 100.0 if self.independent_motor_scaling else self.gain) def set_axes(self, axes: Axes, sequence=None): 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: if sequence is not None: if sequence <= self.last_input_sequence: return self.last_input_sequence = sequence self.last_input_at = time.monotonic() 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._send_mav(self.connection, "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("SUB 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="SUB CONTROL / 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()] for sim in ("tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"): if sim not in ports: ports.append(sim) self.port.configure(values=ports) if ports and self.port.get() not in ports: self.port.set(ports[0]) 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[:5] 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()