1289 lines
58 KiB
Python
1289 lines
58 KiB
Python
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
|
|
|
|
|
|
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()
|
|
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.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):
|
|
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} control_hz={self.rate_hz}")
|
|
self.events.put(("log_path", str(self.log_path)))
|
|
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")
|
|
conn.mav.heartbeat_send(
|
|
mavutil.mavlink.MAV_TYPE_GCS,
|
|
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
|
|
0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
|
|
)
|
|
print(f"[MavlinkWorker._run] Initial heartbeat sent, entering recv loop")
|
|
next_gcs_hb = time.monotonic() + 1.0
|
|
next_control = 0.0
|
|
while not self.stop_event.is_set():
|
|
now = time.monotonic()
|
|
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:
|
|
print(f"[MavlinkWorker._run] EXCEPTION: {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 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
|
|
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.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)
|
|
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")
|
|
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:
|
|
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
|
|
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, 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.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("<Button-1>", self._move)
|
|
self.bind("<B1-Motion>", self._move)
|
|
self.bind("<ButtonRelease-1>", 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("<ButtonPress-1>", self._cancel_return, add="+")
|
|
self.bind("<ButtonRelease-1>", 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("<KeyPress>", self._key_down)
|
|
self.bind_all("<KeyRelease>", 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()
|