stsL/argo
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects 1 Releases Wiki Activity

Clean up UI windows and strengthen settings workflows

Browse Source
This commit is contained in:
PitresAli
2026-09-30 15:22:09 +02:00
parent 7c85397af0
commit 0093f3f4aa
18 changed files with 2533 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app.py
+448 -32
View File
@@ -13,6 +13,9 @@ 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
@@ -67,6 +70,10 @@ class MavlinkWorker:
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()
@@ -87,6 +94,34 @@ class MavlinkWorker:
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, "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
@@ -96,6 +131,9 @@ class MavlinkWorker:
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}")
@@ -109,8 +147,12 @@ class MavlinkWorker:
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} control_hz={self.rate_hz}")
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
)
@@ -137,7 +179,7 @@ class MavlinkWorker:
# Send initial GCS heartbeat immediately
self.log("TX_HEARTBEAT", "GCS initial")
conn.mav.heartbeat_send(
self._send_mav(conn, "heartbeat_send",
mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
@@ -149,17 +191,32 @@ class MavlinkWorker:
now = time.monotonic()
if now >= next_gcs_hb:
self.log("TX_HEARTBEAT", "GCS")
conn.mav.heartbeat_send(
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
if self.target_system and now >= next_control:
# 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.
if self.target_system and self.armed and now >= next_control:
self._send_control()
next_control = now + (1.0 / max(1, self.rate_hz))
# 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:
@@ -170,6 +227,8 @@ class MavlinkWorker:
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:
@@ -180,6 +239,19 @@ class MavlinkWorker:
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
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:
@@ -189,22 +261,71 @@ class MavlinkWorker:
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(f"{timestamp} [{category}] {message}\n")
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 == "HEARTBEAT" and getattr(msg, "type", 0) != mavutil.mavlink.MAV_TYPE_GCS:
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):
@@ -216,6 +337,10 @@ class MavlinkWorker:
armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
self.armed = armed
self.config_paused = armed
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:
@@ -224,6 +349,7 @@ class MavlinkWorker:
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()
@@ -232,12 +358,21 @@ class MavlinkWorker:
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_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")
self.events.put(("statustext", text.rstrip("\x00")))
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)
if 0 < voltage_mv < 65535:
@@ -264,6 +399,239 @@ class MavlinkWorker:
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 _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,
@@ -284,31 +652,52 @@ class MavlinkWorker:
self.param_retries = 0
def _configure_low_bandwidth_streams(self):
"""Stop inherited high-rate streams and request only diagnostic essentials."""
"""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.connection.mav.request_data_stream_send(
self._send_mav(self.connection, "request_data_stream_send",
self.target_system, self.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL, 0, 0,
)
# Heartbeat and STATUSTEXT do not need to be requested. Keep only
# output/input evidence needed to diagnose controls, at 2 Hz each.
for message_id, interval_us, name in (
(mavutil.mavlink.MAVLINK_MSG_ID_SYS_STATUS, 1_000_000, "SYS_STATUS"),
(mavutil.mavlink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500_000, "SERVO_OUTPUT_RAW"),
(mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS, 500_000, "RC_CHANNELS"),
):
self.log("TX_STREAM_CONFIG", f"{name} interval_us={interval_us}")
self.connection.mav.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.events.put(("status", "Low-bandwidth telemetry configured; reading servo setup…"))
# 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 now < self.param_deadline:
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:
@@ -329,7 +718,7 @@ class MavlinkWorker:
def _request_param(self, name: str):
self.log("TX_PARAM_REQUEST", name)
self.connection.mav.param_request_read_send(
self._send_mav(self.connection, "param_request_read_send",
self.target_system, self.target_component, name.encode("ascii"), -1
)
@@ -343,7 +732,7 @@ class MavlinkWorker:
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.connection.mav.param_set_send(
self._send_mav(self.connection, "param_set_send",
self.target_system, self.target_component, name.encode("ascii"),
float(value), param_type,
)
@@ -396,7 +785,7 @@ class MavlinkWorker:
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})",
)
conn.mav.manual_control_send(self.target_system, x, y, z, r, 0)
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:
@@ -409,18 +798,45 @@ class MavlinkWorker:
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")
conn.mav.command_long_send(
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,
0, 1.0 if arm else 0.0, 0, 0, 0, 0, 0, 0,
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:
@@ -430,7 +846,7 @@ class MavlinkWorker:
if manual is None:
raise RuntimeError("MANUAL mode is not advertised by this firmware")
self.log("TX_MODE", f"MANUAL custom_mode={manual}")
conn.mav.set_mode_send(
self._send_mav(conn, "set_mode_send",
self.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
manual,
@@ -467,7 +883,7 @@ class MavlinkWorker:
if self.connection and self.target_system:
try:
self._send_control(force_neutral=True)
self.connection.mav.command_long_send(
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,