mavctl 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
mavctl/__init__.py ADDED
@@ -0,0 +1,3 @@
1
+ """mavctl — headless MAVLink ground control station CLI."""
2
+
3
+ __version__ = "0.1.0"
@@ -0,0 +1,28 @@
1
+ """Adapter layer — the ONLY place pymavlink may be imported.
2
+
3
+ The rest of the codebase depends solely on the :class:`VehicleAdapter`
4
+ protocol defined here plus the :func:`create_adapter` factory, keeping the
5
+ MAVLink transport swappable and testable.
6
+ """
7
+
8
+ from mavctl.adapter.base import AdapterError, ConnectionLostError, VehicleAdapter
9
+
10
+
11
+ def create_adapter(connection_string: str, heartbeat_timeout_s: float = 3.0) -> VehicleAdapter:
12
+ """Build the default (pymavlink-backed) adapter.
13
+
14
+ Imported lazily so pymavlink stays confined to this layer and is only
15
+ loaded when an adapter is actually constructed.
16
+ """
17
+
18
+ from mavctl.adapter.pymavlink_adapter import PymavlinkAdapter
19
+
20
+ return PymavlinkAdapter(connection_string, heartbeat_timeout_s=heartbeat_timeout_s)
21
+
22
+
23
+ __all__ = [
24
+ "AdapterError",
25
+ "ConnectionLostError",
26
+ "VehicleAdapter",
27
+ "create_adapter",
28
+ ]
mavctl/adapter/base.py ADDED
@@ -0,0 +1,86 @@
1
+ """Abstract vehicle adapter interface.
2
+
3
+ No pymavlink import here — this module defines the contract the daemon
4
+ depends on. Concrete transports (e.g. :mod:`mavctl.adapter.pymavlink_adapter`)
5
+ implement it.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ from typing import Protocol, runtime_checkable
11
+
12
+ from mavctl.models import CommandOutcome, Telemetry, VehicleState
13
+
14
+
15
+ class AdapterError(Exception):
16
+ """Base class for adapter-level failures."""
17
+
18
+
19
+ class ConnectionLostError(AdapterError):
20
+ """Raised when the underlying link cannot be established or is lost."""
21
+
22
+
23
+ @runtime_checkable
24
+ class VehicleAdapter(Protocol):
25
+ """Transport-agnostic view of a single MAVLink vehicle.
26
+
27
+ Implementations maintain a continuously-updated snapshot of vehicle
28
+ status and telemetry; :meth:`get_state` and :meth:`get_telemetry` are
29
+ cheap, non-blocking reads of that snapshot. The command verbs
30
+ (:meth:`arm` … :meth:`rtl`) send a COMMAND_LONG and block only until the
31
+ vehicle's COMMAND_ACK (with retries), never until the manoeuvre finishes;
32
+ completion is observed by polling the snapshot.
33
+ """
34
+
35
+ def connect(self) -> None:
36
+ """Open the link and begin ingesting messages.
37
+
38
+ Raises:
39
+ ConnectionLostError: if the link cannot be opened.
40
+ """
41
+ ...
42
+
43
+ def disconnect(self) -> None:
44
+ """Close the link and stop ingesting messages. Idempotent."""
45
+ ...
46
+
47
+ def get_state(self) -> VehicleState:
48
+ """Return the latest cached vehicle status snapshot."""
49
+ ...
50
+
51
+ def get_telemetry(self) -> Telemetry:
52
+ """Return the latest cached telemetry snapshot."""
53
+ ...
54
+
55
+ def mode_names(self) -> list[str]:
56
+ """Return the flight-mode names supported by the vehicle."""
57
+ ...
58
+
59
+ def arm(self) -> CommandOutcome:
60
+ """Send an arm command and await its ACK.
61
+
62
+ Force-arm (MAV_CMD_COMPONENT_ARM_DISARM param2=21196) is deliberately
63
+ not part of this interface: pre-arm checks are never bypassed.
64
+ """
65
+ ...
66
+
67
+ def disarm(self, force: bool = False) -> CommandOutcome:
68
+ """Send a disarm command and await its ACK."""
69
+ ...
70
+
71
+ def set_mode(self, mode: str) -> CommandOutcome:
72
+ """Switch flight mode by name (resolved via the vehicle mode map)."""
73
+ ...
74
+
75
+ def takeoff(self, altitude_m: float) -> CommandOutcome:
76
+ """Command a takeoff to ``altitude_m`` metres relative altitude."""
77
+ ...
78
+
79
+ def land(self) -> CommandOutcome:
80
+ """Command a land at the current position."""
81
+ ...
82
+
83
+ def rtl(self) -> CommandOutcome:
84
+ """Command a return-to-launch."""
85
+ ...
86
+
@@ -0,0 +1,584 @@
1
+ """pymavlink-backed vehicle adapter.
2
+
3
+ This is the single module in the codebase permitted to import pymavlink.
4
+ A background thread performs the blocking MAVLink reads and updates a
5
+ lock-protected snapshot; :meth:`get_state` / :meth:`get_telemetry` are cheap
6
+ reads of that snapshot. Command verbs send a COMMAND_LONG and block only
7
+ until the matching COMMAND_ACK (captured by the reader thread), with retries.
8
+ """
9
+
10
+ from __future__ import annotations
11
+
12
+ import contextlib
13
+ import math
14
+ import threading
15
+ import time
16
+ from typing import Any
17
+
18
+ from pymavlink import mavutil
19
+
20
+ from mavctl.adapter.base import ConnectionLostError
21
+ from mavctl.models import (
22
+ Attitude,
23
+ Battery,
24
+ CommandOutcome,
25
+ GpsInfo,
26
+ HomePosition,
27
+ Position,
28
+ Telemetry,
29
+ VehicleState,
30
+ Velocity,
31
+ )
32
+
33
+ # Message types we subscribe to.
34
+ _SUBSCRIBED = (
35
+ "HEARTBEAT",
36
+ "SYS_STATUS",
37
+ "GLOBAL_POSITION_INT",
38
+ "ATTITUDE",
39
+ "GPS_RAW_INT",
40
+ "COMMAND_ACK",
41
+ "EXTENDED_SYS_STATE",
42
+ "HOME_POSITION",
43
+ )
44
+
45
+ _GPS_FIX_LABELS = {
46
+ 0: "no_gps",
47
+ 1: "no_fix",
48
+ 2: "2d_fix",
49
+ 3: "3d_fix",
50
+ 4: "dgps",
51
+ 5: "rtk_float",
52
+ 6: "rtk_fixed",
53
+ 7: "static",
54
+ 8: "ppp",
55
+ }
56
+
57
+ _MAV_STATE_LABELS = {
58
+ 0: "uninit",
59
+ 1: "boot",
60
+ 2: "calibrating",
61
+ 3: "standby",
62
+ 4: "active",
63
+ 5: "critical",
64
+ 6: "emergency",
65
+ 7: "poweroff",
66
+ 8: "flight_termination",
67
+ }
68
+
69
+ _LANDED_STATE_LABELS = {
70
+ 0: "undefined",
71
+ 1: "on_ground",
72
+ 2: "in_air",
73
+ 3: "takeoff",
74
+ 4: "landing",
75
+ }
76
+
77
+ # Sentinels used by MAVLink to mean "field not populated".
78
+ _UINT16_MAX = 65535
79
+
80
+ # Magic param2 value for a forced DISARM (emergency motor stop). Arm never
81
+ # sends it: there is no force-arm path in mavctl by design.
82
+ _FORCE_ARM_MAGIC = 21196.0
83
+
84
+
85
+ class PymavlinkAdapter:
86
+ """Concrete :class:`~mavctl.adapter.base.VehicleAdapter` over pymavlink.
87
+
88
+ Args:
89
+ connection_string: mavutil-style connection, e.g. ``udp:127.0.0.1:14550``.
90
+ heartbeat_timeout_s: link is considered lost if no HEARTBEAT arrives
91
+ within this many seconds.
92
+ source_system: MAVLink source system id for this GCS.
93
+ command_ack_timeout_s: seconds to wait for a COMMAND_ACK per attempt.
94
+ command_retries: how many times to (re)send a command awaiting its ACK.
95
+ """
96
+
97
+ def __init__(
98
+ self,
99
+ connection_string: str,
100
+ heartbeat_timeout_s: float = 3.0,
101
+ source_system: int = 255,
102
+ command_ack_timeout_s: float = 5.0,
103
+ command_retries: int = 3,
104
+ command_ack_settle_s: float = 1.0,
105
+ ) -> None:
106
+ self._connection_string = connection_string
107
+ self._heartbeat_timeout_s = heartbeat_timeout_s
108
+ self._source_system = source_system
109
+ self._command_ack_timeout_s = command_ack_timeout_s
110
+ self._command_retries = command_retries
111
+ # Post-timeout quiet window for a command id (see _send_command).
112
+ self._command_ack_settle_s = command_ack_settle_s
113
+
114
+ self._master: Any | None = None
115
+ self._reader: threading.Thread | None = None
116
+ self._stop = threading.Event()
117
+ self._lock = threading.Lock()
118
+ self._send_lock = threading.Lock()
119
+ # Serializes a whole command transaction (send -> await ACK -> retry)
120
+ # so two commands can never interleave or steal each other's ACK.
121
+ # Held only by executor threads running command verbs; the reader
122
+ # thread and snapshot reads (get_state/get_telemetry) never take it.
123
+ self._command_lock = threading.Lock()
124
+
125
+ # COMMAND_ACK rendezvous: command id -> (result, recv_monotonic).
126
+ #
127
+ # MAVLink protocol limit: COMMAND_ACK carries the command id (and
128
+ # result) but not confirmation/param1, so arm and disarm — which share
129
+ # MAV_CMD_COMPONENT_ARM_DISARM — cannot be perfectly correlated to a
130
+ # specific send. Transaction serialization + stale-ACK clear reduce
131
+ # concurrent races; they cannot prove a late ACK after a timeout
132
+ # belongs to the next same-id send. See quarantine below.
133
+ self._ack_cond = threading.Condition()
134
+ self._acks: dict[int, tuple[int, float]] = {}
135
+ # command id -> monotonic deadline. After a full timeout we drop ACKs
136
+ # for that id until the settle window ends, and the next same-id send
137
+ # waits out the window first. Risk reduction only — not perfect
138
+ # correlation.
139
+ self._ack_quarantine_until: dict[int, float] = {}
140
+
141
+ # Target ids, learned from (and then locked to) the first autopilot
142
+ # heartbeat. Only this system+component may update the snapshot or
143
+ # answer COMMAND_ACKs. Defaults are placeholders until lock; ACKs and
144
+ # snapshot telemetry are rejected while unlocked.
145
+ self._target_system = 1
146
+ self._target_component = 1
147
+ self._target_locked = False
148
+ self._streams_requested = False
149
+
150
+ # Snapshot fields, all guarded by ``_lock``.
151
+ self._system_id: int | None = None
152
+ self._component_id: int | None = None
153
+ self._last_hb_monotonic: float | None = None
154
+ self._last_hb_epoch: float | None = None
155
+ self._flight_mode: str | None = None
156
+ self._armed: bool | None = None
157
+ self._system_status: str | None = None
158
+ self._landed_state: str | None = None
159
+ self._relative_alt_m: float | None = None
160
+ self._battery = Battery()
161
+ self._gps = GpsInfo()
162
+ self._home: HomePosition | None = None
163
+ self._position = Position()
164
+ self._attitude = Attitude()
165
+ self._velocity = Velocity()
166
+ self._telemetry_ts: float | None = None
167
+
168
+ # -- lifecycle ---------------------------------------------------------
169
+
170
+ def connect(self) -> None:
171
+ """Open the link and start the background reader thread.
172
+
173
+ Does not block waiting for a heartbeat; the vehicle may connect
174
+ later. Raises :class:`ConnectionLostError` if the link cannot open.
175
+ """
176
+
177
+ if self._reader is not None and self._reader.is_alive():
178
+ return
179
+ try:
180
+ self._master = mavutil.mavlink_connection(
181
+ self._connection_string,
182
+ source_system=self._source_system,
183
+ )
184
+ except Exception as exc:
185
+ raise ConnectionLostError(
186
+ f"could not open link {self._connection_string!r}: {exc}"
187
+ ) from exc
188
+
189
+ self._stop.clear()
190
+ self._reader = threading.Thread(
191
+ target=self._read_loop,
192
+ name="mavctl-reader",
193
+ daemon=True,
194
+ )
195
+ self._reader.start()
196
+
197
+ def disconnect(self) -> None:
198
+ """Stop the reader thread and close the link. Idempotent."""
199
+
200
+ self._stop.set()
201
+ reader = self._reader
202
+ if reader is not None and reader.is_alive() and reader is not threading.current_thread():
203
+ reader.join(timeout=3.0)
204
+ self._reader = None
205
+ if self._master is not None:
206
+ with contextlib.suppress(Exception):
207
+ self._master.close()
208
+ self._master = None
209
+
210
+ # -- snapshot reads ----------------------------------------------------
211
+
212
+ def get_state(self) -> VehicleState:
213
+ with self._lock:
214
+ age = self._heartbeat_age_locked()
215
+ connected = age is not None and age <= self._heartbeat_timeout_s
216
+ return VehicleState(
217
+ connected=connected,
218
+ connection_string=self._connection_string,
219
+ system_id=self._system_id,
220
+ component_id=self._component_id,
221
+ last_heartbeat_ts=self._last_hb_epoch,
222
+ heartbeat_age_s=round(age, 3) if age is not None else None,
223
+ flight_mode=self._flight_mode if connected else None,
224
+ armed=self._armed if connected else None,
225
+ system_status=self._system_status if connected else None,
226
+ landed_state=self._landed_state if connected else None,
227
+ relative_alt_m=self._relative_alt_m,
228
+ battery=self._battery.model_copy(),
229
+ gps=self._gps.model_copy(),
230
+ home_position=self._home.model_copy() if self._home is not None else None,
231
+ )
232
+
233
+ def get_telemetry(self) -> Telemetry:
234
+ with self._lock:
235
+ return Telemetry(
236
+ timestamp=self._telemetry_ts,
237
+ position=self._position.model_copy(),
238
+ attitude=self._attitude.model_copy(),
239
+ velocity=self._velocity.model_copy(),
240
+ )
241
+
242
+ # -- commands ----------------------------------------------------------
243
+
244
+ def mode_names(self) -> list[str]:
245
+ return sorted(self._mode_mapping().keys())
246
+
247
+ def arm(self) -> CommandOutcome:
248
+ # param2 stays 0.0 unconditionally: mavctl never sends the 21196 magic
249
+ # that would bypass the autopilot's pre-arm checks.
250
+ return self._send_command(
251
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
252
+ [1.0, 0.0],
253
+ )
254
+
255
+ def disarm(self, force: bool = False) -> CommandOutcome:
256
+ return self._send_command(
257
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
258
+ [0.0, _FORCE_ARM_MAGIC if force else 0.0],
259
+ )
260
+
261
+ def set_mode(self, mode: str) -> CommandOutcome:
262
+ mapping = self._mode_mapping()
263
+ number = mapping.get(mode.upper())
264
+ if number is None:
265
+ raise ValueError(f"unknown flight mode {mode!r}; available: {sorted(mapping)}")
266
+ return self._send_command(
267
+ mavutil.mavlink.MAV_CMD_DO_SET_MODE,
268
+ [float(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), float(number)],
269
+ )
270
+
271
+ def takeoff(self, altitude_m: float) -> CommandOutcome:
272
+ return self._send_command(
273
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
274
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, float(altitude_m)],
275
+ )
276
+
277
+ def land(self) -> CommandOutcome:
278
+ return self._send_command(mavutil.mavlink.MAV_CMD_NAV_LAND, [])
279
+
280
+ def rtl(self) -> CommandOutcome:
281
+ return self._send_command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, [])
282
+
283
+ def _send_command(
284
+ self,
285
+ command: int,
286
+ params: list[float],
287
+ ack_timeout: float | None = None,
288
+ retries: int | None = None,
289
+ ) -> CommandOutcome:
290
+ master = self._master
291
+ if master is None:
292
+ raise ConnectionLostError("link is not open")
293
+ timeout = ack_timeout if ack_timeout is not None else self._command_ack_timeout_s
294
+ attempts = retries if retries is not None else self._command_retries
295
+ padded = (params + [0.0] * 7)[:7]
296
+
297
+ # One command transaction at a time: send + await-ACK + retries are
298
+ # atomic so a concurrent command can neither interleave a send nor
299
+ # consume this command's ACK.
300
+ #
301
+ # Protocol limit (not fully solvable here): COMMAND_ACK only names the
302
+ # MAV_CMD id. Shared ids (arm/disarm) and late ACKs after a timeout
303
+ # cannot be correlated to a specific send with certainty. Mitigations:
304
+ # (1) this lock, (2) clear stale map entries at transaction start,
305
+ # (3) after a full timeout, quarantine that command id for
306
+ # ``command_ack_settle_s`` — drop ACKs and delay the next same-id
307
+ # send. These lower risk; they do not provide perfect correlation.
308
+ with self._command_lock:
309
+ self._wait_ack_quarantine(command)
310
+ # Drop any stale ACK for this command id left by a prior
311
+ # transaction (arm/disarm share MAV_CMD_COMPONENT_ARM_DISARM).
312
+ with self._ack_cond:
313
+ self._acks.pop(command, None)
314
+ for attempt in range(1, attempts + 1):
315
+ send_ts = time.monotonic()
316
+ with self._send_lock:
317
+ master.mav.command_long_send(
318
+ self._target_system,
319
+ self._target_component,
320
+ command,
321
+ attempt - 1, # confirmation counter
322
+ *padded,
323
+ )
324
+ result = self._await_ack(command, send_ts, timeout)
325
+ if result is not None:
326
+ with self._ack_cond:
327
+ self._ack_quarantine_until.pop(command, None)
328
+ return CommandOutcome.from_ack(result, attempt)
329
+ # Full timeout: open a settle window so a late ACK for this
330
+ # failed transaction is less likely to satisfy the next same-id send.
331
+ with self._ack_cond:
332
+ self._acks.pop(command, None)
333
+ if self._command_ack_settle_s > 0:
334
+ self._ack_quarantine_until[command] = (
335
+ time.monotonic() + self._command_ack_settle_s
336
+ )
337
+ return CommandOutcome.timeout(attempts)
338
+
339
+ def _wait_ack_quarantine(self, command: int) -> None:
340
+ """Block until any post-timeout settle window for ``command`` ends."""
341
+
342
+ while True:
343
+ with self._ack_cond:
344
+ until = self._ack_quarantine_until.get(command)
345
+ if until is None:
346
+ return
347
+ remaining = until - time.monotonic()
348
+ if remaining <= 0:
349
+ self._ack_quarantine_until.pop(command, None)
350
+ return
351
+ time.sleep(min(remaining, 0.05))
352
+
353
+ def _await_ack(self, command: int, send_ts: float, timeout: float) -> int | None:
354
+ deadline = send_ts + timeout
355
+ with self._ack_cond:
356
+ while True:
357
+ entry = self._acks.get(command)
358
+ if entry is not None and entry[1] >= send_ts:
359
+ return entry[0]
360
+ remaining = deadline - time.monotonic()
361
+ if remaining <= 0:
362
+ return None
363
+ self._ack_cond.wait(timeout=remaining)
364
+
365
+ def _mode_mapping(self) -> dict[str, int]:
366
+ master = self._master
367
+ if master is None:
368
+ return {}
369
+ with contextlib.suppress(Exception):
370
+ mapping = master.mode_mapping()
371
+ if mapping:
372
+ return {str(name): int(num) for name, num in mapping.items()}
373
+ return {}
374
+
375
+ # -- internals ---------------------------------------------------------
376
+
377
+ def _heartbeat_age_locked(self) -> float | None:
378
+ if self._last_hb_monotonic is None:
379
+ return None
380
+ return time.monotonic() - self._last_hb_monotonic
381
+
382
+ def _read_loop(self) -> None:
383
+ master = self._master
384
+ if master is None:
385
+ return
386
+ while not self._stop.is_set():
387
+ try:
388
+ msg = master.recv_match(type=list(_SUBSCRIBED), blocking=True, timeout=1.0)
389
+ except Exception:
390
+ time.sleep(0.1)
391
+ continue
392
+ if msg is None:
393
+ continue
394
+ self._handle_message(msg)
395
+
396
+ def _handle_message(self, msg: Any) -> None:
397
+ msg_type = msg.get_type()
398
+ if msg_type == "BAD_DATA":
399
+ return
400
+ handler = _HANDLERS.get(msg_type)
401
+ if handler is not None:
402
+ handler(self, msg)
403
+
404
+ def _is_locked_target(self, msg: Any) -> bool:
405
+ """True only when the autopilot target is locked and ``msg`` is from it."""
406
+
407
+ if not self._target_locked:
408
+ return False
409
+ return bool(
410
+ msg.get_srcSystem() == self._target_system
411
+ and msg.get_srcComponent() == self._target_component
412
+ )
413
+
414
+ def _on_heartbeat(self, msg: Any) -> None:
415
+ src_system = msg.get_srcSystem()
416
+ src_component = msg.get_srcComponent()
417
+ # Lock onto the autopilot component on first discovery; afterwards only
418
+ # that exact system+component may update the vehicle snapshot. Heartbeats
419
+ # from other components (gimbal, companion, GCS) must never overwrite
420
+ # flight_mode / armed / system_status.
421
+ if not self._target_locked:
422
+ if src_component != mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1:
423
+ return # no autopilot seen yet; ignore other components
424
+ self._target_system = src_system
425
+ self._target_component = src_component
426
+ self._target_locked = True
427
+ elif not self._is_locked_target(msg):
428
+ return
429
+
430
+ now_mono = time.monotonic()
431
+ now_epoch = time.time()
432
+ flight_mode = self._flightmode_string(msg)
433
+ armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
434
+ system_status = _MAV_STATE_LABELS.get(msg.system_status, f"state_{msg.system_status}")
435
+ self._request_streams_once()
436
+ with self._lock:
437
+ self._system_id = src_system
438
+ self._component_id = src_component
439
+ self._last_hb_monotonic = now_mono
440
+ self._last_hb_epoch = now_epoch
441
+ self._flight_mode = flight_mode
442
+ self._armed = armed
443
+ self._system_status = system_status
444
+
445
+ def _request_streams_once(self) -> None:
446
+ """Best-effort: ask for the extended-status stream so EXTENDED_SYS_STATE
447
+ (landed_state) and HOME_POSITION populate. Runs on the reader thread."""
448
+
449
+ if self._streams_requested or self._master is None:
450
+ return
451
+ self._streams_requested = True
452
+ with contextlib.suppress(Exception), self._send_lock:
453
+ self._master.mav.request_data_stream_send(
454
+ self._target_system,
455
+ self._target_component,
456
+ mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS,
457
+ 2, # Hz
458
+ 1, # start
459
+ )
460
+
461
+ def _flightmode_string(self, msg: Any) -> str | None:
462
+ master = self._master
463
+ if master is not None:
464
+ with contextlib.suppress(Exception):
465
+ mode = master.flightmode
466
+ if isinstance(mode, str):
467
+ return mode
468
+ return f"mode({msg.custom_mode})"
469
+
470
+ def _on_sys_status(self, msg: Any) -> None:
471
+ if not self._is_locked_target(msg):
472
+ return
473
+ voltage = msg.voltage_battery
474
+ current = msg.current_battery
475
+ remaining = msg.battery_remaining
476
+ battery = Battery(
477
+ voltage_v=(voltage / 1000.0) if voltage not in (0, _UINT16_MAX) else None,
478
+ current_a=(current / 100.0) if current >= 0 else None,
479
+ remaining_pct=remaining if remaining >= 0 else None,
480
+ )
481
+ with self._lock:
482
+ self._battery = battery
483
+
484
+ def _on_global_position(self, msg: Any) -> None:
485
+ if not self._is_locked_target(msg):
486
+ return
487
+ vx = msg.vx / 100.0
488
+ vy = msg.vy / 100.0
489
+ vz = msg.vz / 100.0
490
+ heading = msg.hdg / 100.0 if msg.hdg != _UINT16_MAX else None
491
+ relative_alt = msg.relative_alt / 1000.0
492
+ position = Position(
493
+ lat_deg=msg.lat / 1e7,
494
+ lon_deg=msg.lon / 1e7,
495
+ alt_msl_m=msg.alt / 1000.0,
496
+ relative_alt_m=relative_alt,
497
+ )
498
+ velocity = Velocity(
499
+ vx_ms=vx,
500
+ vy_ms=vy,
501
+ vz_ms=vz,
502
+ groundspeed_ms=math.hypot(vx, vy),
503
+ heading_deg=heading,
504
+ )
505
+ with self._lock:
506
+ self._position = position
507
+ self._velocity = velocity
508
+ self._relative_alt_m = relative_alt
509
+ self._telemetry_ts = time.time()
510
+
511
+ def _on_attitude(self, msg: Any) -> None:
512
+ if not self._is_locked_target(msg):
513
+ return
514
+ attitude = Attitude(
515
+ roll_deg=math.degrees(msg.roll),
516
+ pitch_deg=math.degrees(msg.pitch),
517
+ yaw_deg=math.degrees(msg.yaw),
518
+ )
519
+ with self._lock:
520
+ self._attitude = attitude
521
+ self._telemetry_ts = time.time()
522
+
523
+ def _on_gps_raw(self, msg: Any) -> None:
524
+ if not self._is_locked_target(msg):
525
+ return
526
+ gps = GpsInfo(
527
+ fix_type=msg.fix_type,
528
+ fix_label=_GPS_FIX_LABELS.get(msg.fix_type, "unknown"),
529
+ satellites_visible=(
530
+ msg.satellites_visible if msg.satellites_visible != 255 else None
531
+ ),
532
+ )
533
+ with self._lock:
534
+ self._gps = gps
535
+
536
+ def _on_command_ack(self, msg: Any) -> None:
537
+ # Refuse all ACKs until the autopilot target is locked. Default
538
+ # target ids (1/1) are placeholders and must not accept traffic.
539
+ if not self._target_locked:
540
+ return
541
+ # Only accept ACKs from the locked autopilot; ignore ACKs emitted by
542
+ # other systems/components so one vehicle's ACK can never satisfy a
543
+ # command addressed to ours.
544
+ if not self._is_locked_target(msg):
545
+ return
546
+ command = int(msg.command)
547
+ with self._ack_cond:
548
+ until = self._ack_quarantine_until.get(command)
549
+ if until is not None and time.monotonic() < until:
550
+ # Late ACK during post-timeout settle window — drop.
551
+ return
552
+ self._acks[command] = (int(msg.result), time.monotonic())
553
+ self._ack_cond.notify_all()
554
+
555
+ def _on_extended_sys_state(self, msg: Any) -> None:
556
+ if not self._is_locked_target(msg):
557
+ return
558
+ label = _LANDED_STATE_LABELS.get(msg.landed_state)
559
+ with self._lock:
560
+ self._landed_state = label
561
+
562
+ def _on_home_position(self, msg: Any) -> None:
563
+ if not self._is_locked_target(msg):
564
+ return
565
+ home = HomePosition(
566
+ lat_deg=msg.latitude / 1e7,
567
+ lon_deg=msg.longitude / 1e7,
568
+ alt_msl_m=msg.altitude / 1000.0,
569
+ )
570
+ with self._lock:
571
+ self._home = home
572
+
573
+
574
+ # Dispatch table mapping MAVLink message type -> bound-method.
575
+ _HANDLERS: dict[str, Any] = {
576
+ "HEARTBEAT": PymavlinkAdapter._on_heartbeat,
577
+ "SYS_STATUS": PymavlinkAdapter._on_sys_status,
578
+ "GLOBAL_POSITION_INT": PymavlinkAdapter._on_global_position,
579
+ "ATTITUDE": PymavlinkAdapter._on_attitude,
580
+ "GPS_RAW_INT": PymavlinkAdapter._on_gps_raw,
581
+ "COMMAND_ACK": PymavlinkAdapter._on_command_ack,
582
+ "EXTENDED_SYS_STATE": PymavlinkAdapter._on_extended_sys_state,
583
+ "HOME_POSITION": PymavlinkAdapter._on_home_position,
584
+ }