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 +3 -0
- mavctl/adapter/__init__.py +28 -0
- mavctl/adapter/base.py +86 -0
- mavctl/adapter/pymavlink_adapter.py +584 -0
- mavctl/cli/__init__.py +10 -0
- mavctl/cli/app.py +406 -0
- mavctl/cli/render.py +56 -0
- mavctl/daemon/__init__.py +19 -0
- mavctl/daemon/__main__.py +41 -0
- mavctl/daemon/client.py +80 -0
- mavctl/daemon/guards.py +447 -0
- mavctl/daemon/process.py +166 -0
- mavctl/daemon/server.py +496 -0
- mavctl/daemon/wire.py +30 -0
- mavctl/models/__init__.py +33 -0
- mavctl/models/commands.py +69 -0
- mavctl/models/protocol.py +62 -0
- mavctl/models/state.py +53 -0
- mavctl/models/telemetry.py +41 -0
- mavctl/paths.py +39 -0
- mavctl-0.2.0.dist-info/METADATA +225 -0
- mavctl-0.2.0.dist-info/RECORD +25 -0
- mavctl-0.2.0.dist-info/WHEEL +4 -0
- mavctl-0.2.0.dist-info/entry_points.txt +2 -0
- mavctl-0.2.0.dist-info/licenses/LICENSE +21 -0
mavctl/__init__.py
ADDED
|
@@ -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
|
+
}
|