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()