Build Argo V2 control mappings and telemetry dashboard
Add configurable Xbox motion and action bindings, frame-aware six-axis MAVLink controls, sensor logging, camera viewing and recording, and a faster startup path. Synchronize both app copies and document companion hardware setup and verification limits.
This commit is contained in:
+814
@@ -0,0 +1,814 @@
|
||||
"""Optional Tk frontend. Imported only when explicitly requested."""
|
||||
import queue
|
||||
import json
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
import tkinter as tk
|
||||
import customtkinter as ctk
|
||||
from pathlib import Path
|
||||
from tkinter import messagebox, ttk
|
||||
from serial.tools import list_ports
|
||||
from app import Axes, MavlinkWorker, MOTOR_FUNCTIONS, NEUTRAL_Z, normalize_endpoint
|
||||
|
||||
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)
|
||||
Reference in New Issue
Block a user