stsL/argo
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects 1 Releases Wiki Activity
Files
8ccd7689f70ec869b7e6064e84ed674805bd02c5
argo/legacy_ui.py
T
PitresAli 8ccd7689f7 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.
2026-10-22 20:22:05 +02:00

815 lines
38 KiB
Python
Raw Blame History

"""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 View Git Blame Copy Permalink