PitresAli
8ccd7689f7
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.
1124 lines
53 KiB
Python
1124 lines
53 KiB
Python
from __future__ import annotations
|
|
|
|
import queue
|
|
import json
|
|
import os
|
|
import threading
|
|
import time
|
|
import math
|
|
from collections import deque
|
|
from dataclasses import dataclass
|
|
from datetime import datetime
|
|
from pathlib import Path
|
|
from rov_v2 import FRAMES, decode_telemetry
|
|
|
|
# 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
|
|
pitch: float = 0.0
|
|
roll: float = 0.0
|
|
|
|
|
|
def normalize_endpoint(port: str) -> str:
|
|
p = str(port).strip()
|
|
if not p:
|
|
return p
|
|
lower = p.lower()
|
|
if lower.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:")):
|
|
return p
|
|
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.network_link = False
|
|
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.frame_type = None
|
|
self.firmware_version = None
|
|
self.pending_accessory = None
|
|
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.frame_type = None
|
|
self.firmware_version = None
|
|
self.pending_accessory = None
|
|
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,
|
|
"ATTITUDE": 2, "GLOBAL_POSITION_INT": 2, "SCALED_IMU": 1, "SCALED_PRESSURE2": 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)
|
|
self.network_link = normalized.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:"))
|
|
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 not self.network_link and 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 self.target_system and msg_type != "HEARTBEAT" and callable(getattr(msg, "get_srcSystem", None)):
|
|
if msg.get_srcSystem() != self.target_system:
|
|
return
|
|
if msg_type in ("AUTOPILOT_VERSION", "ATTITUDE", "GLOBAL_POSITION_INT", "SCALED_IMU", "SCALED_PRESSURE2", "PARAM_VALUE", "COMMAND_ACK") and msg.get_srcComponent() != self.target_component:
|
|
return
|
|
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()
|
|
self._send_mav(self.connection, "command_long_send", self.target_system,
|
|
self.target_component, mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
|
|
0, mavutil.mavlink.MAVLINK_MSG_ID_AUTOPILOT_VERSION, 0, 0, 0, 0, 0, 0)
|
|
elif msg_type == "AUTOPILOT_VERSION":
|
|
value = int(msg.flight_sw_version)
|
|
self.firmware_version = (value >> 24, (value >> 16) & 255, (value >> 8) & 255)
|
|
self.events.put(("capabilities", {"firmware": list(self.firmware_version)}))
|
|
elif msg_type in ("ATTITUDE", "GLOBAL_POSITION_INT", "SCALED_IMU", "SCALED_IMU2", "SCALED_IMU3", "SCALED_PRESSURE2"):
|
|
decoded = decode_telemetry(msg)
|
|
if decoded:
|
|
self.events.put(("telemetry", decoded[0], decoded[1], time.monotonic()))
|
|
elif msg_type == "COMMAND_ACK":
|
|
if int(msg.command) == mavutil.mavlink.MAV_CMD_DO_SET_RELAY and self.pending_accessory:
|
|
name, value, _ = self.pending_accessory
|
|
self.pending_accessory = None
|
|
self.events.put(("accessory_ack", name, value, int(msg.result)))
|
|
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":
|
|
if int(getattr(msg, "id", 0)) != 0:
|
|
return
|
|
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 == "FRAME_TYPE":
|
|
self.frame_type = int(round(msg.param_value))
|
|
self.events.put(("capabilities", {"frame": self.frame_type}))
|
|
elif 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 = ["FRAME_TYPE"] + [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 if name not in self.telemetry_rates_hz
|
|
)
|
|
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._approach(self.output_axes.pitch, target.pitch, max_change),
|
|
self._approach(self.output_axes.roll, target.roll, 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 self.frame_type not in (None, 4):
|
|
# Shared envelope for arbitrary standard mixers. The autopilot still
|
|
# owns allocation/stabilization; never infer a mixer from motor count.
|
|
peak = max(1.0, sum(abs(value) for value in vars(axes).values()))
|
|
limited_forward, limited_yaw = axes.forward / peak, axes.yaw / peak
|
|
else:
|
|
peak = 1.0
|
|
if independent_motor_scaling and self.frame_type == 4:
|
|
# 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 / peak * gain * 1000)
|
|
r = round(limited_yaw * gain * 1000)
|
|
z = round(NEUTRAL_Z + axes.vertical / peak * 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})",
|
|
)
|
|
supported = FRAMES.get(self.frame_type, FRAMES[4])[2]
|
|
if "lateral" not in supported:
|
|
y = 0
|
|
if self.firmware_version and ((4, 5, 0) <= self.firmware_version < (4, 6, 0) or self.firmware_version >= (4, 8, 0)):
|
|
# ArduSub 4.5 accepts S=pitch and T=roll as PWM offsets (±400).
|
|
# The verified 4.8 development source uses normalized S/T values.
|
|
attitude_scale = 1000 if self.firmware_version >= (4, 8, 0) else 400
|
|
pitch = round(axes.pitch / peak * gain * attitude_scale) if "pitch" in supported else 0
|
|
roll = round(axes.roll / peak * gain * attitude_scale) if "roll" in supported else 0
|
|
self._send_mav(conn, "manual_control_send", self.target_system, x, y, z, r, 0, 0, 3, pitch, roll)
|
|
else:
|
|
self._send_mav(conn, "manual_control_send", self.target_system, x, y, z, r, 0)
|
|
|
|
def accessory(self, name, relay, enabled):
|
|
if not self.connection or not self.target_system or time.monotonic() - self.last_heartbeat > 3:
|
|
raise RuntimeError("Vehicle offline")
|
|
if self.pending_accessory and time.monotonic() - self.pending_accessory[2] < 3:
|
|
raise RuntimeError("Accessory pending")
|
|
self.pending_accessory = (name, enabled, time.monotonic())
|
|
self._send_mav(self.connection, "command_long_send", self.target_system, self.target_component,
|
|
mavutil.mavlink.MAV_CMD_DO_SET_RELAY, 0, relay, int(enabled), 0, 0, 0, 0, 0)
|
|
return True
|
|
|
|
@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):
|
|
if not math.isfinite(value):
|
|
return 0.0
|
|
# 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),
|
|
conditioned(axes.pitch), conditioned(axes.roll),
|
|
)
|
|
|
|
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
|
|
|
|
|
|
def __getattr__(name):
|
|
if name in ("App", "Joystick", "SpringScale"):
|
|
import legacy_ui
|
|
return getattr(legacy_ui, name)
|
|
raise AttributeError(name)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# The production interface is CSS/HTML rendered by the local WebView.
|
|
# The optional Tk frontend is loaded lazily from legacy_ui.
|
|
from web_ui import main as run_web_interface
|
|
|
|
run_web_interface()
|