1851 lines
86 KiB
Python
1851 lines
86 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
|
|
|
|
# ESC_TELEMETRY_1_TO_4 uses a MAVLink 2 message ID. Select the v2 dialect
|
|
# before importing pymavlink so received ESC packets can be decoded/logged.
|
|
os.environ["MAVLINK20"] = "1"
|
|
from pymavlink import mavutil
|
|
from serial.tools import list_ports
|
|
|
|
|
|
NEUTRAL_Z = 500 # ArduSub legacy MANUAL_CONTROL convention: 0..1000.
|
|
MOTOR_FUNCTIONS = {
|
|
**{33 + index: f"Motor {index + 1}" for index in range(8)},
|
|
**{82 + index: f"Motor {index + 9}" for index in range(4)},
|
|
}
|
|
|
|
|
|
def motor_number_for_function(function: int | None) -> int | None:
|
|
if function is None:
|
|
return None
|
|
if 33 <= function <= 40:
|
|
return function - 32
|
|
if 82 <= function <= 85:
|
|
return function - 73
|
|
return None
|
|
|
|
|
|
@dataclass
|
|
class Axes:
|
|
forward: float = 0.0
|
|
lateral: float = 0.0
|
|
vertical: float = 0.0
|
|
yaw: float = 0.0
|
|
|
|
|
|
def normalize_endpoint(port: str) -> str:
|
|
p = str(port).strip()
|
|
if not p:
|
|
return p
|
|
lower = p.lower()
|
|
if "sim" in lower or "5760" in lower or lower in ("tcp", "tcp:", "tcp:127.0.0.1", "127.0.0.1"):
|
|
return "tcp:127.0.0.1:5760"
|
|
if "14550" in lower or lower in ("udp", "udp:", "udpin", "udpin:"):
|
|
return "udpin:127.0.0.1:14550"
|
|
if p.startswith("127.0.0.1:") or p.startswith("localhost:"):
|
|
num = p.split(":")[1]
|
|
if num == "14550":
|
|
return f"udpin:{p}"
|
|
return f"tcp:{p}"
|
|
if ":" in p and not p.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:")):
|
|
return f"tcp:{p}"
|
|
return p
|
|
|
|
|
|
class MavlinkWorker:
|
|
def __init__(self, events: queue.Queue):
|
|
self.events = events
|
|
self.connection = None
|
|
self.stop_event = threading.Event()
|
|
self.lock = threading.Lock()
|
|
# pymavlink writes are not thread-safe. The web/API thread can issue
|
|
# commands while this worker sends heartbeats and controls, so serialize
|
|
# every packet before it reaches a low-bandwidth serial link.
|
|
self.tx_lock = threading.Lock()
|
|
self.thread = None
|
|
self.axes = Axes()
|
|
self.output_axes = Axes()
|
|
self.last_control_at = 0.0
|
|
# Enabled by the web bridge only; the legacy Tk frontend is unchanged.
|
|
self.require_input_lease = False
|
|
self.last_input_at = 0.0
|
|
self.last_input_sequence = -1
|
|
self.input_timeout = 1.0
|
|
# Smooth ESC commands without adding a perceptible one-second lag.
|
|
# At 20 Hz this produces five small steps from neutral to full command.
|
|
self.ramp_seconds = 0.25
|
|
self.gain = 0.20
|
|
self.independent_motor_scaling = False
|
|
self.motor_gains = {motor: 20 for motor in range(1, 13)}
|
|
self.rate_hz = 20
|
|
self.target_system = 0
|
|
self.target_component = 0
|
|
self.last_heartbeat = 0.0
|
|
self.last_rx = 0.0
|
|
self.armed = False
|
|
self.config_paused = False
|
|
# Keep configuration traffic off the link during an arm/disarm
|
|
# transition, including the wait for the vehicle's confirmation.
|
|
self.command_quiet_until = 0.0
|
|
self.disarm_requested_until = 0.0
|
|
self.pending_arm_state = None
|
|
self.next_arm_retry_at = 0.0
|
|
self.arm_retry_deadline = 0.0
|
|
self.arm_confirmation = 0
|
|
self.stream_config = deque()
|
|
self.next_stream_config_at = 0.0
|
|
# Low-rate TIMESYNC echoes measure PC↔FC round trips with minimal traffic.
|
|
self.ping_rate_hz = 1
|
|
self.heavy_quality_test = None
|
|
self.heavy_pending_pings = {}
|
|
self.heavy_quality_latest = None
|
|
self.telemetry_rates_hz = {
|
|
"SYS_STATUS": 1, "BATTERY_STATUS": 1, "SERVO_OUTPUT_RAW": 2,
|
|
"RC_CHANNELS": 2, "ESC_TELEMETRY_1_TO_4": 1,
|
|
}
|
|
self.ping_sequence = 0
|
|
self.next_ping_at = 0.0
|
|
self.pending_pings = {}
|
|
self.ping_results = deque(maxlen=300)
|
|
self.bad_frame_times = deque()
|
|
self.bad_frame_bytes = deque()
|
|
self.rx_sequence_by_source = {}
|
|
self.rx_delivery_results = deque(maxlen=1024)
|
|
self.next_quality_publish_at = 0.0
|
|
self.param_scan = deque()
|
|
self.param_waiting = None
|
|
self.param_deadline = 0.0
|
|
self.param_retries = 0
|
|
self.param_types = {}
|
|
self.pending_sets = {}
|
|
self.motor_test_lock = threading.Lock()
|
|
self.motor_test_request = None
|
|
self.motor_test_mode = False
|
|
self.motor_test_acknowledged = False
|
|
self.motor_test_last_motor = None
|
|
self.log_lock = threading.Lock()
|
|
self.log_file = None
|
|
self.log_path = None
|
|
self.console_lines = deque(maxlen=500)
|
|
self.console_rx_lines = deque(maxlen=300)
|
|
self.console_tx_lines = deque(maxlen=300)
|
|
|
|
def connect(self, port: str, baud: int):
|
|
print(f"[MavlinkWorker] connect() called: port={port!r}, baud={baud}")
|
|
if self.thread and self.thread.is_alive():
|
|
print(f"[MavlinkWorker] Thread already alive, ignoring connect()")
|
|
return
|
|
self.stop_event.clear()
|
|
log_dir = Path(__file__).resolve().parent / "logs"
|
|
log_dir.mkdir(exist_ok=True)
|
|
self.log_path = log_dir / f"ardusub_{datetime.now():%Y%m%d_%H%M%S}.log"
|
|
self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1)
|
|
normalized = normalize_endpoint(port)
|
|
print(f"[MavlinkWorker] normalized endpoint: {normalized!r}")
|
|
self.log("SESSION", f"start port={normalized} (input={port}) baud={baud} "
|
|
f"control_hz={self.rate_hz} rx=MAVLink2 tx=MAVLink2")
|
|
self.events.put(("log_path", str(self.log_path)))
|
|
self.last_heartbeat = 0.0
|
|
self.last_rx = 0.0
|
|
self.next_quality_publish_at = 0.0
|
|
self.thread = threading.Thread(
|
|
target=self._run, args=(normalized, baud), daemon=True
|
|
)
|
|
self.thread.start()
|
|
print(f"[MavlinkWorker] worker thread started")
|
|
|
|
def _run(self, port: str, baud: int):
|
|
print(f"[MavlinkWorker._run] ENTERED: port={port!r}, baud={baud}")
|
|
try:
|
|
print(f"[MavlinkWorker._run] Creating mavlink_connection...")
|
|
conn = mavutil.mavlink_connection(
|
|
port, baud=baud, source_system=255, source_component=190,
|
|
# Fail a missing TCP simulator immediately. pymavlink otherwise
|
|
# retries the refused socket several times and leaves the UI in
|
|
# "Opening" long after the actual failure.
|
|
autoreconnect=False, retries=0, force_connected=True,
|
|
)
|
|
print(f"[MavlinkWorker._run] mavlink_connection created: {type(conn).__name__}")
|
|
self.connection = conn
|
|
if port.startswith("tcp:"):
|
|
self.events.put(("status", f"Connected to {port}; waiting for vehicle heartbeat…"))
|
|
else:
|
|
self.events.put(("status", f"Port {port} open; waiting for heartbeat at {baud} baud…"))
|
|
|
|
# Send initial GCS heartbeat immediately
|
|
self.log("TX_HEARTBEAT", "GCS initial")
|
|
self._send_mav(conn, "heartbeat_send",
|
|
mavutil.mavlink.MAV_TYPE_GCS,
|
|
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
|
|
0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
|
|
)
|
|
print(f"[MavlinkWorker._run] Initial heartbeat sent, entering recv loop")
|
|
next_gcs_hb = time.monotonic() + 1.0
|
|
next_control = 0.0
|
|
while not self.stop_event.is_set():
|
|
now = time.monotonic()
|
|
if now >= next_gcs_hb:
|
|
self.log("TX_HEARTBEAT", "GCS")
|
|
self._send_mav(conn, "heartbeat_send",
|
|
mavutil.mavlink.MAV_TYPE_GCS,
|
|
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
|
|
0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
|
|
)
|
|
next_gcs_hb = now + 1.0
|
|
# ArduSub cannot use MANUAL_CONTROL while disarmed. Keeping a
|
|
# 20 Hz neutral stream alive here only competes with ARM and
|
|
# its acknowledgement on constrained links.
|
|
motor_test_mode = self._tick_motor_test(now)
|
|
if self.target_system and self.armed and now >= next_control:
|
|
if not motor_test_mode:
|
|
self._send_control()
|
|
# The first disarm packet is preceded by a neutral command.
|
|
# Until disarm is confirmed, keep sending neutral at a
|
|
# lower rate instead of consuming 20 Hz on the weak link.
|
|
control_hz = min(self.rate_hz, 5) if now < self.disarm_requested_until else self.rate_hz
|
|
next_control = now + (1.0 / max(1, control_hz))
|
|
elif not self.armed:
|
|
next_control = now
|
|
if self.target_system:
|
|
self._tick_arm_retry(now)
|
|
self._tick_link_probe(now)
|
|
self._tick_stream_config(now)
|
|
self._tick_param_scan(now)
|
|
if now >= self.next_quality_publish_at:
|
|
self._publish_link_quality()
|
|
self.next_quality_publish_at = now + 1.0
|
|
|
|
msg = conn.recv_match(blocking=False)
|
|
if msg:
|
|
self._handle_message(msg)
|
|
else:
|
|
time.sleep(0.01)
|
|
except Exception as exc:
|
|
print(f"[MavlinkWorker._run] EXCEPTION: {exc}")
|
|
self.events.put(("error", f"Connection error: {exc}"))
|
|
finally:
|
|
if self.heavy_quality_test is not None:
|
|
self.cancel_heavy_quality_test()
|
|
self._safe_stop()
|
|
if self.connection:
|
|
try:
|
|
self.connection.close()
|
|
except Exception:
|
|
pass
|
|
self.connection = None
|
|
self.target_system = 0
|
|
self.armed = False
|
|
self.config_paused = False
|
|
self.command_quiet_until = 0.0
|
|
self.disarm_requested_until = 0.0
|
|
self.pending_arm_state = None
|
|
self.next_arm_retry_at = 0.0
|
|
self.arm_retry_deadline = 0.0
|
|
self.arm_confirmation = 0
|
|
with self.motor_test_lock:
|
|
self.motor_test_request = None
|
|
self.motor_test_mode = False
|
|
self.motor_test_last_motor = None
|
|
self.stream_config.clear()
|
|
self.pending_pings.clear()
|
|
self.ping_results.clear()
|
|
self.bad_frame_times.clear()
|
|
self.bad_frame_bytes.clear()
|
|
self.rx_sequence_by_source.clear()
|
|
self.rx_delivery_results.clear()
|
|
self.log("SESSION", "disconnected")
|
|
with self.log_lock:
|
|
if self.log_file:
|
|
self.log_file.close()
|
|
self.log_file = None
|
|
self.events.put(("disconnected", "Disconnected"))
|
|
|
|
def log(self, category: str, message: str):
|
|
timestamp = datetime.now().astimezone().isoformat(timespec="milliseconds")
|
|
line = f"{timestamp} [{category}] {message}"
|
|
with self.log_lock:
|
|
self.console_lines.append(line[:2000])
|
|
if category.startswith("RX") or category == "ONBOARD_LOG_ERROR":
|
|
self.console_rx_lines.append(line[:2000])
|
|
elif category.startswith("TX") or category == "OPERATOR":
|
|
self.console_tx_lines.append(line[:2000])
|
|
if self.log_file:
|
|
self.log_file.write(line + "\n")
|
|
|
|
def read_console(self, limit=150):
|
|
with self.log_lock:
|
|
limit = max(1, min(300, int(limit)))
|
|
return {
|
|
"rx": list(self.console_rx_lines)[-limit:],
|
|
"tx": list(self.console_tx_lines)[-limit:],
|
|
}
|
|
|
|
def _send_mav(self, connection, method: str, *args):
|
|
"""Serialize MAVLink 2 writes and protect the start of each burst."""
|
|
with self.tx_lock:
|
|
# This full-duplex module pair loses the opening bytes of some
|
|
# PC-to-FC bursts after the line has been idle. Put a valid,
|
|
# expendable GCS heartbeat first so the following packet remains
|
|
# intact and the FC also sees a healthy GCS heartbeat stream.
|
|
if method != "heartbeat_send" and hasattr(connection.mav, "heartbeat_send"):
|
|
connection.mav.heartbeat_send(
|
|
mavutil.mavlink.MAV_TYPE_GCS,
|
|
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
|
|
0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
|
|
)
|
|
getattr(connection.mav, method)(*args)
|
|
|
|
def _handle_message(self, msg):
|
|
msg_type = msg.get_type()
|
|
if msg_type == "BAD_DATA":
|
|
now = time.monotonic()
|
|
self.bad_frame_times.append(now)
|
|
self.bad_frame_bytes.append((now, len(getattr(msg, "data", b""))))
|
|
self._trim_bad_frames(now)
|
|
self.log("RX_BAD_DATA", repr(getattr(msg, "data", b"")))
|
|
return
|
|
self.last_rx = time.monotonic()
|
|
self._track_rx_delivery(msg)
|
|
try:
|
|
payload = json.dumps(msg.to_dict(), default=str, separators=(",", ":"))
|
|
except Exception:
|
|
payload = str(msg)
|
|
self.log("RX", payload)
|
|
if msg_type == "TIMESYNC" and int(getattr(msg, "tc1", 0)) != 0:
|
|
token = int(getattr(msg, "ts1", -1))
|
|
heavy_probe = self.heavy_pending_pings.pop(token, None)
|
|
if heavy_probe is not None:
|
|
stage_index, sent_at = heavy_probe
|
|
self._record_heavy_probe(stage_index, (time.monotonic() - sent_at) * 1000.0)
|
|
self._publish_heavy_quality()
|
|
sent_at = None
|
|
else:
|
|
sent_at = self.pending_pings.pop(token, None)
|
|
if sent_at is not None:
|
|
rtt_ms = (time.monotonic() - sent_at) * 1000.0
|
|
self.ping_results.append((time.monotonic(), rtt_ms))
|
|
self.log("RX_LINK_PROBE", f"timesync={token} rtt_ms={rtt_ms:.1f}")
|
|
self._publish_link_quality()
|
|
elif msg_type == "HEARTBEAT" and getattr(msg, "type", 0) != mavutil.mavlink.MAV_TYPE_GCS:
|
|
if getattr(msg, "autopilot", mavutil.mavlink.MAV_AUTOPILOT_INVALID) == mavutil.mavlink.MAV_AUTOPILOT_INVALID:
|
|
return
|
|
if self.target_system and (msg.get_srcSystem(), msg.get_srcComponent()) != (self.target_system, self.target_component):
|
|
return
|
|
first = not self.target_system
|
|
self.target_system = msg.get_srcSystem()
|
|
self.target_component = msg.get_srcComponent()
|
|
self.last_heartbeat = time.monotonic()
|
|
armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
|
|
self.armed = armed
|
|
self.config_paused = armed
|
|
if not armed:
|
|
with self.motor_test_lock:
|
|
was_testing = self.motor_test_mode
|
|
self.motor_test_request = None
|
|
self.motor_test_mode = False
|
|
if was_testing:
|
|
self.events.put(("motor_test_stopped", "Vehicle disarmed"))
|
|
if self.pending_arm_state is not None and armed == self.pending_arm_state:
|
|
self.pending_arm_state = None
|
|
if not armed:
|
|
self.disarm_requested_until = 0.0
|
|
try:
|
|
mode = mavutil.mode_string_v10(msg)
|
|
except Exception:
|
|
mode = str(msg.custom_mode)
|
|
autopilot = mavutil.mavlink.enums["MAV_AUTOPILOT"].get(msg.autopilot)
|
|
ap_name = autopilot.name if autopilot else str(msg.autopilot)
|
|
self.events.put(("heartbeat", self.target_system, ap_name, mode, armed, self.last_heartbeat))
|
|
if first:
|
|
self.next_ping_at = time.monotonic()
|
|
self.events.put(("status", "Heartbeat received; controls are neutral."))
|
|
self._configure_low_bandwidth_streams()
|
|
self._start_servo_scan()
|
|
elif msg_type == "COMMAND_ACK":
|
|
if (msg.get_srcSystem(), msg.get_srcComponent()) != (self.target_system, self.target_component):
|
|
return
|
|
result = mavutil.mavlink.enums["MAV_RESULT"].get(msg.result)
|
|
result_name = result.name if result else str(msg.result)
|
|
if int(msg.command) == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL:
|
|
self.log("RX_STREAM_CONFIG_ACK", result_name)
|
|
return
|
|
if int(msg.command) == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST:
|
|
self.log("RX_MOTOR_TEST_ACK", result_name)
|
|
if result_name in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS"):
|
|
if not self.motor_test_acknowledged:
|
|
self.motor_test_acknowledged = True
|
|
self.events.put(("motor_test_ack", result_name))
|
|
else:
|
|
with self.motor_test_lock:
|
|
self.motor_test_request = None
|
|
self.motor_test_mode = False
|
|
self.events.put(("motor_test_rejected", result_name))
|
|
return
|
|
if (int(msg.command) == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM
|
|
and result_name not in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS")):
|
|
self.pending_arm_state = None
|
|
self.events.put(("ack", int(msg.command), result_name, self.last_rx))
|
|
elif msg_type == "STATUSTEXT":
|
|
text = msg.text
|
|
if isinstance(text, bytes):
|
|
text = text.decode(errors="replace")
|
|
text = text.rstrip("\x00")
|
|
if "/APM/LOGS" in text and "Failed" in text:
|
|
self.log("ONBOARD_LOG_ERROR", text)
|
|
self.events.put(("statustext", text))
|
|
elif msg_type == "SYS_STATUS":
|
|
voltage_mv = int(msg.voltage_battery)
|
|
current_ca = int(msg.current_battery)
|
|
remaining = int(msg.battery_remaining)
|
|
self.events.put(("battery", {
|
|
"voltage": voltage_mv / 1000.0 if 0 < voltage_mv < 65535 else None,
|
|
"current_a": current_ca / 100.0 if current_ca >= 0 else None,
|
|
"remaining_percent": remaining if 0 <= remaining <= 100 else None,
|
|
}))
|
|
elif msg_type == "BATTERY_STATUS":
|
|
voltages = [int(value) for value in getattr(msg, "voltages", [])]
|
|
valid_voltages = [value for value in voltages if 0 < value < 65535]
|
|
voltage = sum(valid_voltages) / 1000.0 if valid_voltages else None
|
|
current_ca = int(msg.current_battery)
|
|
consumed = int(msg.current_consumed)
|
|
remaining = int(msg.battery_remaining)
|
|
self.events.put(("battery", {
|
|
"voltage": voltage,
|
|
"current_a": current_ca / 100.0 if current_ca >= 0 else None,
|
|
"consumed_mah": consumed if consumed >= 0 else None,
|
|
"remaining_percent": remaining if 0 <= remaining <= 100 else None,
|
|
}))
|
|
elif msg_type == "SERVO_OUTPUT_RAW":
|
|
base = int(getattr(msg, "port", 0)) * 8 + 1
|
|
outputs = {}
|
|
for index in range(1, 9):
|
|
raw = int(getattr(msg, f"servo{index}_raw", 0))
|
|
outputs[str(base + index - 1)] = raw if 0 < raw < 65535 else None
|
|
self.events.put(("servo_output", outputs))
|
|
elif msg_type == "PARAM_VALUE":
|
|
param_id = msg.param_id
|
|
if isinstance(param_id, bytes):
|
|
param_id = param_id.decode(errors="replace")
|
|
param_id = param_id.rstrip("\x00")
|
|
self.param_types[param_id] = msg.param_type
|
|
if param_id == self.param_waiting:
|
|
self.param_waiting = None
|
|
self.param_deadline = time.monotonic() + 0.10
|
|
if param_id in self.pending_sets:
|
|
wanted = self.pending_sets[param_id]
|
|
if int(round(msg.param_value)) == wanted:
|
|
del self.pending_sets[param_id]
|
|
self.events.put(("param_saved", param_id, wanted))
|
|
if param_id.startswith("SERVO") and param_id.endswith("_FUNCTION"):
|
|
self.events.put(("servo_param", param_id, int(round(msg.param_value))))
|
|
elif param_id.startswith("MOT_") and param_id.endswith("_DIRECTION"):
|
|
parts = param_id.split("_")
|
|
if len(parts) == 3 and parts[1].isdigit():
|
|
self.events.put(("motor_direction", int(parts[1]),
|
|
int(round(msg.param_value))))
|
|
|
|
def _trim_bad_frames(self, now):
|
|
while self.bad_frame_times and now - self.bad_frame_times[0] > 20.0:
|
|
self.bad_frame_times.popleft()
|
|
|
|
def _track_rx_delivery(self, msg):
|
|
"""Estimate downlink loss from MAVLink's per-source packet sequence."""
|
|
try:
|
|
source = (int(msg.get_srcSystem()), int(msg.get_srcComponent()))
|
|
sequence = int(msg.get_seq())
|
|
except (AttributeError, TypeError, ValueError):
|
|
return
|
|
if self.target_system and source != (self.target_system, self.target_component):
|
|
return
|
|
previous = self.rx_sequence_by_source.get(source)
|
|
self.rx_sequence_by_source[source] = sequence
|
|
if previous is None:
|
|
return
|
|
missing = (sequence - previous - 1) & 0xFF
|
|
# A large backwards jump is a reset/reordered packet, not 200 losses.
|
|
if missing > 127:
|
|
return
|
|
timestamp = time.monotonic()
|
|
self.rx_delivery_results.extend((timestamp, False) for _ in range(missing))
|
|
self.rx_delivery_results.append((timestamp, True))
|
|
|
|
def _publish_link_quality(self):
|
|
now = time.monotonic()
|
|
self._trim_bad_frames(now)
|
|
while self.ping_results and now - self.ping_results[0][0] > 30.0:
|
|
self.ping_results.popleft()
|
|
while self.rx_delivery_results and now - self.rx_delivery_results[0][0] > 30.0:
|
|
self.rx_delivery_results.popleft()
|
|
while self.bad_frame_bytes and now - self.bad_frame_bytes[0][0] > 20.0:
|
|
self.bad_frame_bytes.popleft()
|
|
samples = [sample for _, sample in self.ping_results]
|
|
received = [sample for sample in samples if sample is not None]
|
|
total = len(samples)
|
|
loss_percent = 100.0 * (total - len(received)) / total if total else None
|
|
ordered_rtt = sorted(received)
|
|
latency_ms = (sum(ordered_rtt) / len(ordered_rtt)) if ordered_rtt else None
|
|
median_rtt_ms = ordered_rtt[len(ordered_rtt) // 2] if ordered_rtt else None
|
|
p95_rtt_ms = ordered_rtt[min(len(ordered_rtt) - 1, (len(ordered_rtt) * 95 + 99) // 100 - 1)] if ordered_rtt else None
|
|
rx_received = sum(1 for _, delivered in self.rx_delivery_results if delivered)
|
|
rx_missing = sum(1 for _, delivered in self.rx_delivery_results if not delivered)
|
|
rx_loss_percent = 100.0 * rx_missing / (rx_received + rx_missing) if rx_received else None
|
|
rx_corrupt_bytes = sum(byte_count for _, byte_count in self.bad_frame_bytes)
|
|
heartbeat_age = (now - self.last_heartbeat) if self.last_heartbeat else None
|
|
if total < 10 or rx_received < 10 or heartbeat_age is None:
|
|
grade = "testing"
|
|
elif (heartbeat_age > 3 or loss_percent > 15
|
|
or (rx_loss_percent is not None and rx_loss_percent > 5)
|
|
or len(self.bad_frame_bytes) >= 5):
|
|
grade = "poor"
|
|
elif (loss_percent > 3 or (rx_loss_percent is not None and rx_loss_percent > 1)
|
|
or (p95_rtt_ms is not None and p95_rtt_ms > 500) or self.bad_frame_bytes):
|
|
grade = "fair"
|
|
elif (loss_percent > 0.5 or (rx_loss_percent is not None and rx_loss_percent > 0.2)
|
|
or (p95_rtt_ms is not None and p95_rtt_ms > 200)):
|
|
grade = "good"
|
|
else:
|
|
grade = "excellent"
|
|
self.events.put(("link_quality", {
|
|
"grade": grade,
|
|
"latency_ms": round(latency_ms, 1) if latency_ms is not None else None,
|
|
"rtt_median_ms": round(median_rtt_ms, 1) if median_rtt_ms is not None else None,
|
|
"rtt_p95_ms": round(p95_rtt_ms, 1) if p95_rtt_ms is not None else None,
|
|
"loss_percent": round(loss_percent, 1) if loss_percent is not None else None,
|
|
"probe_loss_percent": round(loss_percent, 1) if loss_percent is not None else None,
|
|
"probe_samples": total,
|
|
"probe_replies": len(received),
|
|
"rx_packets_30s": rx_received,
|
|
"rx_sequence_loss_percent": round(rx_loss_percent, 1) if rx_loss_percent is not None else None,
|
|
"rx_corrupt_frames_20s": len(self.bad_frame_bytes),
|
|
"rx_corrupt_bytes_20s": rx_corrupt_bytes,
|
|
"samples": total,
|
|
"corrupt_frames_20s": len(self.bad_frame_times),
|
|
"heartbeat_age_s": round(heartbeat_age, 1) if heartbeat_age is not None else None,
|
|
"probe_supported": total > 0,
|
|
}))
|
|
|
|
def _send_motor_test(self, motor, throttle_percent):
|
|
if not self.connection or not self.target_system:
|
|
raise RuntimeError("No autopilot heartbeat")
|
|
self.log("TX_MOTOR_TEST", f"motor={motor} throttle={throttle_percent}%")
|
|
self._send_mav(self.connection, "command_int_send",
|
|
self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL,
|
|
mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, 0, 0,
|
|
float(motor), 0.0, float(throttle_percent), 1.0, 1, 0, 0,
|
|
)
|
|
|
|
def start_motor_test(self, motor, throttle_percent):
|
|
motor, throttle_percent = int(motor), int(throttle_percent)
|
|
if not 1 <= motor <= 12 or not 0 <= throttle_percent <= 100:
|
|
raise ValueError("Choose a motor and a test throttle from 0% to 100%")
|
|
if not self.connection or not self.target_system:
|
|
raise RuntimeError("Connect to the vehicle before testing a motor")
|
|
if not self.armed or time.monotonic() - self.last_heartbeat > 2.0:
|
|
raise RuntimeError("Arm the vehicle and confirm fresh telemetry before motor testing")
|
|
with self.motor_test_lock:
|
|
if self.motor_test_mode:
|
|
raise RuntimeError("Finish the current motor test before starting another")
|
|
self.motor_test_request = {"motor": motor, "throttle": throttle_percent, "next_at": 0.0}
|
|
self.motor_test_last_motor = motor
|
|
self.motor_test_mode = True
|
|
self.motor_test_acknowledged = False
|
|
self.set_axes(Axes())
|
|
self.log("OPERATOR", f"held motor test for motor {motor} at {throttle_percent}%")
|
|
return True
|
|
|
|
def stop_motor_test(self):
|
|
with self.motor_test_lock:
|
|
request = self.motor_test_request
|
|
if request is not None:
|
|
self.motor_test_last_motor = request["motor"]
|
|
motor = self.motor_test_last_motor
|
|
was_testing = self.motor_test_mode
|
|
self.motor_test_request = None
|
|
if was_testing and motor and self.connection and self.target_system:
|
|
self._send_motor_test(motor, 0)
|
|
return True
|
|
|
|
def _tick_motor_test(self, now):
|
|
motor = throttle = None
|
|
with self.motor_test_lock:
|
|
request = self.motor_test_request
|
|
active = self.motor_test_mode
|
|
if request is not None and not self.armed:
|
|
self.motor_test_request = None
|
|
self.motor_test_mode = False
|
|
active = False
|
|
self.events.put(("motor_test_stopped", "Vehicle disarmed"))
|
|
elif request is not None and now >= request["next_at"]:
|
|
request["next_at"] = now + 0.25
|
|
motor, throttle = request["motor"], request["throttle"]
|
|
else:
|
|
motor = throttle = None
|
|
if motor is not None:
|
|
self._send_motor_test(motor, throttle)
|
|
return active
|
|
|
|
def _tick_link_probe(self, now):
|
|
if self.heavy_quality_test is not None:
|
|
self._tick_heavy_quality(now)
|
|
return
|
|
expired = [sequence for sequence, sent_at in self.pending_pings.items()
|
|
if now - sent_at >= 2.5]
|
|
for sequence in expired:
|
|
self.pending_pings.pop(sequence, None)
|
|
self.ping_results.append((now, None))
|
|
self.log("LINK_PROBE_TIMEOUT", f"timesync={sequence}")
|
|
if expired:
|
|
self._publish_link_quality()
|
|
if (now < self.next_ping_at or now < self.command_quiet_until
|
|
or not self.connection or not self.target_system):
|
|
return
|
|
token = time.monotonic_ns()
|
|
self.pending_pings[token] = now
|
|
self.log("TX_LINK_PROBE", f"timesync={token}")
|
|
# ArduPilot implements TIMESYNC even on builds that omit PING. A
|
|
# request has tc1=0; the response mirrors our unique ts1 token.
|
|
self._send_mav(self.connection, "timesync_send", 0, token)
|
|
self.next_ping_at = now + (1.0 / max(1, self.ping_rate_hz))
|
|
|
|
def start_heavy_quality_test(self):
|
|
if not self.connection or not self.target_system:
|
|
raise RuntimeError("Connect to the flight controller before testing the link")
|
|
if self.armed:
|
|
raise RuntimeError("The heavy link test is available while the vehicle is disarmed")
|
|
if self.pending_arm_state is not None or time.monotonic() < self.command_quiet_until:
|
|
raise RuntimeError("Wait for the current vehicle command to finish")
|
|
if self.heavy_quality_test is not None:
|
|
raise RuntimeError("A heavy link test is already running")
|
|
self.pending_pings.clear()
|
|
self.heavy_pending_pings.clear()
|
|
self.heavy_quality_latest = {
|
|
"running": True, "stage": 1, "stages": 3, "rate_hz": 1,
|
|
"status": "Running", "results": [],
|
|
}
|
|
self.heavy_quality_test = {
|
|
"rates": (1, 5, 10), "duration_s": 10.0, "stage_index": 0,
|
|
"stage_started": time.monotonic(), "next_probe_at": time.monotonic(),
|
|
"stages": [{"rate_hz": rate, "sent": 0, "replies": 0,
|
|
"timeouts": 0, "rtts": []} for rate in (1, 5, 10)],
|
|
}
|
|
self.log("TX_HEAVY_TEST", "started rates=1,5,10Hz stage_seconds=10")
|
|
self._publish_heavy_quality()
|
|
return self.heavy_quality_latest
|
|
|
|
def cancel_heavy_quality_test(self):
|
|
if self.heavy_quality_test is None:
|
|
return self.heavy_quality_latest
|
|
self.heavy_pending_pings.clear()
|
|
test = self.heavy_quality_test
|
|
self.heavy_quality_test = None
|
|
self.heavy_quality_latest = {
|
|
"running": False, "stage": 0, "stages": len(test["rates"]),
|
|
"rate_hz": None, "status": "Cancelled", "results": self._heavy_results(test),
|
|
}
|
|
self.next_ping_at = time.monotonic()
|
|
self.log("TX_HEAVY_TEST", "cancelled")
|
|
self.events.put(("heavy_quality", self.heavy_quality_latest))
|
|
return self.heavy_quality_latest
|
|
|
|
@staticmethod
|
|
def _heavy_results(test):
|
|
results = []
|
|
for stage in test["stages"]:
|
|
rtts = sorted(stage["rtts"])
|
|
sent = stage["sent"]
|
|
replies = stage["replies"]
|
|
resolved = replies + stage["timeouts"]
|
|
results.append({
|
|
"rate_hz": stage["rate_hz"], "sent": sent, "replies": replies,
|
|
"loss_percent": round(100.0 * stage["timeouts"] / resolved, 1) if resolved else None,
|
|
"median_rtt_ms": round(rtts[len(rtts) // 2], 1) if rtts else None,
|
|
"p95_rtt_ms": round(rtts[min(len(rtts) - 1, (len(rtts) * 95 + 99) // 100 - 1)], 1) if rtts else None,
|
|
})
|
|
return results
|
|
|
|
def _record_heavy_probe(self, stage_index, rtt_ms=None):
|
|
test = self.heavy_quality_test
|
|
if test is None or not 0 <= stage_index < len(test["stages"]):
|
|
return
|
|
stage = test["stages"][stage_index]
|
|
if rtt_ms is None:
|
|
stage["timeouts"] += 1
|
|
else:
|
|
stage["replies"] += 1
|
|
stage["rtts"].append(rtt_ms)
|
|
|
|
def _publish_heavy_quality(self):
|
|
test = self.heavy_quality_test
|
|
if test is None:
|
|
return
|
|
index = test["stage_index"]
|
|
stage = test["stages"][index]
|
|
self.heavy_quality_latest = {
|
|
"running": True, "stage": index + 1, "stages": len(test["rates"]),
|
|
"rate_hz": stage["rate_hz"], "sent": stage["sent"],
|
|
"replies": stage["replies"], "timeouts": stage["timeouts"],
|
|
"status": f"Stage {index + 1} of {len(test['rates'])}: {stage['rate_hz']} Hz",
|
|
"results": self._heavy_results(test),
|
|
}
|
|
self.events.put(("heavy_quality", self.heavy_quality_latest))
|
|
|
|
def _tick_heavy_quality(self, now):
|
|
test = self.heavy_quality_test
|
|
if test is None:
|
|
return
|
|
for token, (stage_index, sent_at) in list(self.heavy_pending_pings.items()):
|
|
if now - sent_at >= 2.5:
|
|
self.heavy_pending_pings.pop(token, None)
|
|
self._record_heavy_probe(stage_index)
|
|
self.log("HEAVY_PROBE_TIMEOUT", f"rate={test['rates'][stage_index]}Hz timesync={token}")
|
|
index = test["stage_index"]
|
|
stage_pending = any(stage_index == index for stage_index, _ in self.heavy_pending_pings.values())
|
|
if now - test["stage_started"] >= test["duration_s"] and not stage_pending:
|
|
index += 1
|
|
if index >= len(test["rates"]):
|
|
results = self._heavy_results(test)
|
|
self.heavy_quality_test = None
|
|
self.heavy_quality_latest = {
|
|
"running": False, "stage": 3, "stages": 3, "rate_hz": None,
|
|
"status": "Complete", "results": results,
|
|
}
|
|
self.next_ping_at = now
|
|
self.log("TX_HEAVY_TEST", "complete")
|
|
self.events.put(("heavy_quality", self.heavy_quality_latest))
|
|
return
|
|
test["stage_index"] = index
|
|
test["stage_started"] = now
|
|
test["next_probe_at"] = now
|
|
self._publish_heavy_quality()
|
|
index = test["stage_index"]
|
|
stage = test["stages"][index]
|
|
if now >= test["next_probe_at"] and now - test["stage_started"] < test["duration_s"]:
|
|
token = time.monotonic_ns()
|
|
self.heavy_pending_pings[token] = (index, now)
|
|
stage["sent"] += 1
|
|
self.log("TX_HEAVY_PROBE", f"rate={stage['rate_hz']}Hz timesync={token}")
|
|
self._send_mav(self.connection, "timesync_send", 0, token)
|
|
test["next_probe_at"] = now + 1.0 / stage["rate_hz"]
|
|
if now >= self.next_quality_publish_at:
|
|
self._publish_heavy_quality()
|
|
self.next_quality_publish_at = now + 1.0
|
|
|
|
def set_link_rates(self, ping_hz, telemetry_rates):
|
|
self.ping_rate_hz = max(1, min(10, int(ping_hz)))
|
|
for name in self.telemetry_rates_hz:
|
|
if name in telemetry_rates:
|
|
self.telemetry_rates_hz[name] = max(0, min(10, int(telemetry_rates[name])))
|
|
if self.connection and self.target_system:
|
|
self._configure_low_bandwidth_streams()
|
|
def _start_servo_scan(self):
|
|
names = [f"SERVO{number}_FUNCTION" for number in range(1, 17)]
|
|
# ArduSub reverses mixer motors with MOT_n_DIRECTION (1 normal,
|
|
# -1 reversed). SERVOx_REVERSED does not reverse Motor functions.
|
|
names.extend(f"MOT_{number}_DIRECTION" for number in range(1, 13))
|
|
# Capture the exact PWM scaling that affects the three SimpleROV-3
|
|
# outputs, plus ArduSub's own joystick gain. These are read-only here
|
|
# and make direction asymmetry diagnosable from the session log.
|
|
names.extend(
|
|
f"SERVO{number}_{suffix}"
|
|
for number in range(1, 4)
|
|
for suffix in ("MIN", "TRIM", "MAX")
|
|
)
|
|
names.extend(("JS_GAIN_MIN", "JS_GAIN_DEFAULT", "JS_GAIN_MAX"))
|
|
self.param_scan = deque(names)
|
|
self.param_waiting = None
|
|
self.param_deadline = time.monotonic()
|
|
self.param_retries = 0
|
|
|
|
def _configure_low_bandwidth_streams(self):
|
|
"""Pace optional telemetry changes; never burst them at arm time."""
|
|
if not self.connection:
|
|
return
|
|
self.log("TX_STREAM_CONFIG", "stop MAV_DATA_STREAM_ALL")
|
|
self._send_mav(self.connection, "request_data_stream_send",
|
|
self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_DATA_STREAM_ALL, 0, 0,
|
|
)
|
|
# ATTITUDE and VFR_HUD interval commands were accepted by this FC.
|
|
# AHRS2 is also unneeded, but an acknowledgement has not been seen.
|
|
# NAMED_VALUE_FLOAT explicitly returned DENIED, so do not retry it.
|
|
# A single interval command is sometimes lost, so send two paced
|
|
# passes. This does not change persistent parameters or onboard logs.
|
|
unwanted = (
|
|
"ATTITUDE", "AHRS2", "VFR_HUD",
|
|
)
|
|
self.stream_config = deque(
|
|
(name, getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}"), -1)
|
|
for _ in range(2) for name in unwanted
|
|
)
|
|
for name, rate_hz in self.telemetry_rates_hz.items():
|
|
interval_us = -1 if rate_hz == 0 else round(1_000_000 / rate_hz)
|
|
self.stream_config.append((
|
|
name, getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}"), interval_us,
|
|
))
|
|
self.next_stream_config_at = time.monotonic()
|
|
self.events.put(("status", "Applying low-bandwidth telemetry rates…"))
|
|
|
|
def _tick_stream_config(self, now):
|
|
if (not self.connection or not self.target_system or self.armed
|
|
or now < self.command_quiet_until or now < self.next_stream_config_at
|
|
or not self.stream_config):
|
|
return
|
|
name, message_id, interval_us = self.stream_config.popleft()
|
|
self.log("TX_STREAM_CONFIG", f"{name} interval_us={interval_us}")
|
|
self._send_mav(self.connection, "command_long_send",
|
|
self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
|
|
0, message_id, interval_us, 0, 0, 0, 0, 0,
|
|
)
|
|
self.next_stream_config_at = now + 0.5
|
|
|
|
def _tick_param_scan(self, now):
|
|
if (not self.connection or self.config_paused
|
|
or self.stream_config or now < self.next_stream_config_at
|
|
or now < self.command_quiet_until or now < self.param_deadline):
|
|
return
|
|
if self.param_waiting is not None:
|
|
if self.param_retries < 2:
|
|
self.param_retries += 1
|
|
self._request_param(self.param_waiting)
|
|
self.param_deadline = now + 1.5
|
|
return
|
|
self.events.put(("param_missing", self.param_waiting))
|
|
self.param_waiting = None
|
|
if self.param_scan:
|
|
self.param_waiting = self.param_scan.popleft()
|
|
self.param_retries = 0
|
|
self._request_param(self.param_waiting)
|
|
self.param_deadline = now + 1.5
|
|
elif self.param_waiting is None:
|
|
self.events.put(("param_scan_complete",))
|
|
self.param_deadline = float("inf")
|
|
|
|
def _request_param(self, name: str):
|
|
self.log("TX_PARAM_REQUEST", name)
|
|
self._send_mav(self.connection, "param_request_read_send",
|
|
self.target_system, self.target_component, name.encode("ascii"), -1
|
|
)
|
|
|
|
def set_parameter(self, name: str, value: int):
|
|
if not self.connection or not self.target_system:
|
|
raise RuntimeError("No autopilot heartbeat")
|
|
if self.armed or self.config_paused:
|
|
raise RuntimeError("Disarm before changing motor configuration")
|
|
param_type = self.param_types.get(name)
|
|
if param_type is None:
|
|
raise RuntimeError(f"{name} has not been read from the autopilot")
|
|
self.pending_sets[name] = int(value)
|
|
self.log("TX_PARAM_SET", f"{name}={value} type={param_type}")
|
|
self._send_mav(self.connection, "param_set_send",
|
|
self.target_system, self.target_component, name.encode("ascii"),
|
|
float(value), param_type,
|
|
)
|
|
self.events.put(("status", f"Writing {name}={value}; waiting for confirmation…"))
|
|
|
|
def _send_control(self, force_neutral: bool = False):
|
|
conn = self.connection
|
|
if not conn or not self.target_system:
|
|
return
|
|
with self.lock:
|
|
now = time.monotonic()
|
|
expired = self.require_input_lease and (
|
|
not self.armed or now - self.last_heartbeat > 3.0
|
|
or now - self.last_input_at > self.input_timeout)
|
|
if expired or force_neutral:
|
|
# Never replay a stored command after telemetry/UI recovery.
|
|
self.axes = Axes()
|
|
self.output_axes = Axes()
|
|
active = not (force_neutral or expired)
|
|
target = self.axes if active else Axes()
|
|
gain = self.gain
|
|
independent_motor_scaling = self.independent_motor_scaling
|
|
motor_gains = dict(self.motor_gains)
|
|
if force_neutral:
|
|
self.output_axes = Axes()
|
|
else:
|
|
dt = (now - self.last_control_at) if self.last_control_at else (1.0 / max(1, self.rate_hz))
|
|
max_change = min(1.0, dt / max(0.1, self.ramp_seconds))
|
|
self.output_axes = Axes(
|
|
self._approach(self.output_axes.forward, target.forward, max_change),
|
|
self._approach(self.output_axes.lateral, target.lateral, max_change),
|
|
self._approach(self.output_axes.vertical, target.vertical, max_change),
|
|
self._approach(self.output_axes.yaw, target.yaw, max_change),
|
|
)
|
|
self.last_control_at = now
|
|
axes = self.output_axes
|
|
# SimpleROV-3 mixes horizontal motors from forward +/- yaw. Normalize
|
|
# the pair so combining the two axes cannot exceed the selected limit.
|
|
horizontal_mix_peak = max(
|
|
1.0,
|
|
abs(axes.forward + axes.yaw),
|
|
abs(axes.forward - axes.yaw),
|
|
)
|
|
limited_forward = axes.forward / horizontal_mix_peak
|
|
limited_yaw = axes.yaw / horizontal_mix_peak
|
|
if independent_motor_scaling:
|
|
# The desktop controller's SimpleROV-3 profile maps motors 1/2
|
|
# to surge/turn and motor 3 to vertical thrust. Convert the two
|
|
# independently scaled horizontal demands back into MANUAL_CONTROL
|
|
# axes so ArduSub's configured mixer remains in charge.
|
|
motor_1 = (limited_forward + limited_yaw) * motor_gains.get(1, 100) / 100.0
|
|
motor_2 = (limited_forward - limited_yaw) * motor_gains.get(2, 100) / 100.0
|
|
limited_forward = (motor_1 + motor_2) / 2.0
|
|
limited_yaw = (motor_1 - motor_2) / 2.0
|
|
vertical_gain = motor_gains.get(3, 100) / 100.0
|
|
x = round(limited_forward * 1000)
|
|
y = round(axes.lateral * 1000)
|
|
r = round(limited_yaw * 1000)
|
|
z = round(NEUTRAL_Z + axes.vertical * vertical_gain * 500)
|
|
else:
|
|
x = round(limited_forward * gain * 1000)
|
|
y = round(axes.lateral * gain * 1000)
|
|
r = round(limited_yaw * gain * 1000)
|
|
z = round(NEUTRAL_Z + axes.vertical * gain * 500)
|
|
self.log(
|
|
"TX_MANUAL_CONTROL",
|
|
f"x={x} y={y} z={z} r={r} active={active} "
|
|
f"target=({target.forward:.3f},{target.lateral:.3f},{target.vertical:.3f},{target.yaw:.3f})",
|
|
)
|
|
self._send_mav(conn, "manual_control_send", self.target_system, x, y, z, r, 0)
|
|
|
|
@staticmethod
|
|
def _approach(current: float, target: float, max_change: float) -> float:
|
|
difference = target - current
|
|
if abs(difference) <= max_change:
|
|
return target
|
|
return current + (max_change if difference > 0 else -max_change)
|
|
|
|
def command_arm(self, arm: bool):
|
|
conn = self.connection
|
|
if not conn or not self.target_system:
|
|
raise RuntimeError("No autopilot heartbeat")
|
|
if arm and self.heavy_quality_test is not None:
|
|
self.cancel_heavy_quality_test()
|
|
now = time.monotonic()
|
|
self.command_quiet_until = now + 6.0
|
|
if not arm:
|
|
self.disarm_requested_until = now + 6.0
|
|
self._send_control(force_neutral=True)
|
|
else:
|
|
# Give the ARM request priority over the low-rate parameter scan.
|
|
self.config_paused = True
|
|
self.log("TX_COMMAND", "ARM" if arm else "DISARM")
|
|
self.pending_arm_state = bool(arm)
|
|
self.next_arm_retry_at = now + 0.25
|
|
self.arm_retry_deadline = now + 5.0
|
|
self.arm_confirmation = 0
|
|
self._send_arm_packet(arm)
|
|
|
|
def _send_arm_packet(self, arm: bool):
|
|
conn = self.connection
|
|
if not conn or not self.target_system:
|
|
return
|
|
self._send_mav(conn, "command_long_send",
|
|
self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
|
self.arm_confirmation, 1.0 if arm else 0.0, 0, 0, 0, 0, 0, 0,
|
|
)
|
|
|
|
def _tick_arm_retry(self, now):
|
|
if self.pending_arm_state is None or now < self.next_arm_retry_at:
|
|
return
|
|
if now >= self.arm_retry_deadline:
|
|
self.log("TX_COMMAND_TIMEOUT", "ARM" if self.pending_arm_state else "DISARM")
|
|
self.pending_arm_state = None
|
|
return
|
|
self.log("TX_COMMAND_RETRY", "ARM" if self.pending_arm_state else "DISARM")
|
|
self.arm_confirmation = min(255, self.arm_confirmation + 1)
|
|
self._send_arm_packet(self.pending_arm_state)
|
|
self.next_arm_retry_at = now + 0.25
|
|
|
|
def set_manual_mode(self):
|
|
conn = self.connection
|
|
if not conn or not self.target_system:
|
|
raise RuntimeError("No autopilot heartbeat")
|
|
modes = conn.mode_mapping() or {}
|
|
manual = modes.get("MANUAL")
|
|
if manual is None:
|
|
raise RuntimeError("MANUAL mode is not advertised by this firmware")
|
|
self.log("TX_MODE", f"MANUAL custom_mode={manual}")
|
|
self._send_mav(conn, "set_mode_send",
|
|
self.target_system,
|
|
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
|
manual,
|
|
)
|
|
|
|
def set_motor_scaling(self, independent, gains):
|
|
clean = {motor: max(0, min(100, int(gains.get(motor, 100)))) for motor in range(1, 13)}
|
|
with self.lock:
|
|
self.independent_motor_scaling = bool(independent)
|
|
self.motor_gains = clean
|
|
|
|
def motor_output_scale(self, motor):
|
|
with self.lock:
|
|
return (self.motor_gains.get(int(motor), 100) / 100.0
|
|
if self.independent_motor_scaling else self.gain)
|
|
|
|
def set_axes(self, axes: Axes, sequence=None):
|
|
def conditioned(value):
|
|
# Ignore tiny pointer/key return noise, then quantize to stable 1% steps.
|
|
if abs(value) < 0.04:
|
|
return 0.0
|
|
return round(max(-1.0, min(1.0, value)), 2)
|
|
|
|
with self.lock:
|
|
if sequence is not None:
|
|
if sequence <= self.last_input_sequence:
|
|
return
|
|
self.last_input_sequence = sequence
|
|
self.last_input_at = time.monotonic()
|
|
self.axes = Axes(
|
|
conditioned(axes.forward), conditioned(axes.lateral),
|
|
conditioned(axes.vertical), conditioned(axes.yaw),
|
|
)
|
|
|
|
def disconnect(self):
|
|
self._send_control(force_neutral=True)
|
|
if self.connection and self.target_system:
|
|
try:
|
|
self.command_arm(False)
|
|
except Exception:
|
|
pass
|
|
self.stop_event.set()
|
|
|
|
def _safe_stop(self):
|
|
if self.connection and self.target_system:
|
|
try:
|
|
self._send_control(force_neutral=True)
|
|
self._send_mav(self.connection, "command_long_send",
|
|
self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
|
0, 0, 0, 0, 0, 0, 0, 0,
|
|
)
|
|
except Exception:
|
|
pass
|
|
|
|
|
|
class Joystick(tk.Canvas):
|
|
def __init__(self, master, callback, **kwargs):
|
|
self.size = 276
|
|
self.center_point = self.size / 2
|
|
self.radius = 108
|
|
self.knob_radius = 24
|
|
super().__init__(master, width=self.size, height=self.size, bg="#f5f5f2",
|
|
highlightthickness=0, bd=0, **kwargs)
|
|
self.callback = callback
|
|
self.value = (0.0, 0.0)
|
|
self.return_job = None
|
|
c, r, k = self.center_point, self.radius, self.knob_radius
|
|
self.create_oval(c-r, c-r, c+r, c+r, fill="#e9e9e5", outline="#d8d8d2", width=2)
|
|
self.create_oval(c-70, c-70, c+70, c+70, outline="#deded8", width=1)
|
|
self.create_line(c, c-r+12, c, c+r-12, fill="#d2d2cc", dash=(3, 6))
|
|
self.create_line(c-r+12, c, c+r-12, c, fill="#d2d2cc", dash=(3, 6))
|
|
self.create_text(c, 13, text="FORWARD", fill="#8a8a84", font=("Helvetica", 8, "bold"))
|
|
self.create_text(c, self.size-13, text="REVERSE", fill="#8a8a84", font=("Helvetica", 8, "bold"))
|
|
self.create_text(25, c, text="TURN\nLEFT", fill="#8a8a84", font=("Helvetica", 8, "bold"))
|
|
self.create_text(self.size-25, c, text="TURN\nRIGHT", fill="#8a8a84", font=("Helvetica", 8, "bold"))
|
|
self.knob = self.create_oval(c-k, c-k, c+k, c+k,
|
|
fill="#575c61", outline="#42474b", width=2)
|
|
self.bind("<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()
|