urml-ardupilot-runtime 0.4.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.
- urml_ardupilot_runtime/__init__.py +43 -0
- urml_ardupilot_runtime/_version.py +3 -0
- urml_ardupilot_runtime/adapter.py +796 -0
- urml_ardupilot_runtime/config.py +159 -0
- urml_ardupilot_runtime/probe.py +45 -0
- urml_ardupilot_runtime-0.4.0.dist-info/METADATA +162 -0
- urml_ardupilot_runtime-0.4.0.dist-info/RECORD +8 -0
- urml_ardupilot_runtime-0.4.0.dist-info/WHEEL +4 -0
|
@@ -0,0 +1,43 @@
|
|
|
1
|
+
"""urml_ardupilot_runtime — ArduPilot / MAVLink reference runtime for URML.
|
|
2
|
+
|
|
3
|
+
Public API:
|
|
4
|
+
|
|
5
|
+
ArduCopterAdapter(config)
|
|
6
|
+
MAVLink adapter for ArduCopter (Pixhawk-class boards running ArduPilot,
|
|
7
|
+
or ArduCopter SITL). Subclasses the PX4 reference adapter and adds the
|
|
8
|
+
ArduPilot-specific preamble that PX4 does not need: GUIDED mode entry,
|
|
9
|
+
explicit arming, ack filtering by command id, arrival waits, global
|
|
10
|
+
(WGS84) setpoints, camera trigger, and output lines (gripper, winch,
|
|
11
|
+
servo). pymavlink is imported lazily so this module loads everywhere.
|
|
12
|
+
|
|
13
|
+
ArduPilotAdapterConfig
|
|
14
|
+
Connection + identity + location bindings. Loaded from YAML via
|
|
15
|
+
load_ardupilot_config(path).
|
|
16
|
+
|
|
17
|
+
probe(config)
|
|
18
|
+
Read-only identity snapshot of the connected autopilot. Also exposed as
|
|
19
|
+
``python -m urml_ardupilot_runtime.probe COM5``.
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
from __future__ import annotations
|
|
23
|
+
|
|
24
|
+
from urml_ardupilot_runtime._version import __version__
|
|
25
|
+
from urml_ardupilot_runtime.adapter import ArduCopterAdapter, probe
|
|
26
|
+
from urml_ardupilot_runtime.config import (
|
|
27
|
+
ArduPilotAdapterConfig,
|
|
28
|
+
CameraTrigger,
|
|
29
|
+
GlobalPosition,
|
|
30
|
+
OutputLineBinding,
|
|
31
|
+
load_ardupilot_config,
|
|
32
|
+
)
|
|
33
|
+
|
|
34
|
+
__all__ = [
|
|
35
|
+
"ArduCopterAdapter",
|
|
36
|
+
"ArduPilotAdapterConfig",
|
|
37
|
+
"CameraTrigger",
|
|
38
|
+
"GlobalPosition",
|
|
39
|
+
"OutputLineBinding",
|
|
40
|
+
"__version__",
|
|
41
|
+
"load_ardupilot_config",
|
|
42
|
+
"probe",
|
|
43
|
+
]
|
|
@@ -0,0 +1,796 @@
|
|
|
1
|
+
"""ArduCopterAdapter — ArduPilot (ArduCopter) MAVLink substrate adapter.
|
|
2
|
+
|
|
3
|
+
Subclasses ``PX4Adapter`` from the PX4 reference runtime. The two autopilots
|
|
4
|
+
share the MAVLink command set, so the wire format is the same; what differs
|
|
5
|
+
is *firmware behaviour* around those commands, and that is what this class
|
|
6
|
+
owns:
|
|
7
|
+
|
|
8
|
+
- **Mode entry.** ArduCopter accepts ``MAV_CMD_NAV_TAKEOFF`` and
|
|
9
|
+
position setpoints only in GUIDED. PX4 auto-enters its equivalent.
|
|
10
|
+
- **Arming.** ArduCopter never arms itself for a GCS-commanded take-off.
|
|
11
|
+
This adapter sends ``MAV_CMD_COMPONENT_ARM_DISARM`` and waits for the
|
|
12
|
+
armed flag on a heartbeat. A refused arm surfaces the autopilot's own
|
|
13
|
+
``PreArm:`` STATUSTEXT as the reason. Nothing here disables pre-arm
|
|
14
|
+
checks.
|
|
15
|
+
- **Ack filtering.** ArduPilot links are chatty; ``COMMAND_ACK`` is
|
|
16
|
+
matched on its ``command`` field rather than taken first-come.
|
|
17
|
+
- **Arrival.** A GUIDED setpoint is one-shot, so ``move_to`` waits until
|
|
18
|
+
the aircraft reports itself within ``arrival_radius_m`` (or times out).
|
|
19
|
+
That is what makes a ``sequence`` of waypoints mean what it says.
|
|
20
|
+
- **Global setpoints.** A location bound to WGS84 in the config flies as
|
|
21
|
+
``SET_POSITION_TARGET_GLOBAL_INT`` (relative-altitude frame).
|
|
22
|
+
- **Camera and output lines.** ``capture`` triggers the on-board camera;
|
|
23
|
+
``set_output`` drives an ArduPilot gripper, winch, or servo.
|
|
24
|
+
|
|
25
|
+
Failures are returned, never raised, matching every other URML adapter.
|
|
26
|
+
Only a broken connection that cannot be opened raises.
|
|
27
|
+
|
|
28
|
+
## Bench versus field
|
|
29
|
+
|
|
30
|
+
On a bench with no GPS fix, GUIDED entry or arming is refused by the
|
|
31
|
+
autopilot. This adapter reports that refusal verbatim and stops. That is
|
|
32
|
+
the intended bench proof: the link works, the vehicle said no, nothing
|
|
33
|
+
moved.
|
|
34
|
+
"""
|
|
35
|
+
|
|
36
|
+
from __future__ import annotations
|
|
37
|
+
|
|
38
|
+
import math
|
|
39
|
+
import time
|
|
40
|
+
from collections import deque
|
|
41
|
+
from contextlib import suppress
|
|
42
|
+
from typing import Any, Literal
|
|
43
|
+
|
|
44
|
+
from urml_px4_runtime.adapter import PX4Adapter
|
|
45
|
+
from urml_ros2_runtime.substrate.base import (
|
|
46
|
+
CaptureResult,
|
|
47
|
+
NavigationResult,
|
|
48
|
+
SubstrateResult,
|
|
49
|
+
)
|
|
50
|
+
|
|
51
|
+
from urml_ardupilot_runtime.config import (
|
|
52
|
+
ArduPilotAdapterConfig,
|
|
53
|
+
GlobalPosition,
|
|
54
|
+
OutputLineBinding,
|
|
55
|
+
load_ardupilot_config,
|
|
56
|
+
)
|
|
57
|
+
|
|
58
|
+
__all__ = ["ArduCopterAdapter", "probe"]
|
|
59
|
+
|
|
60
|
+
# MAVLink constants (common.xml / ardupilotmega.xml). Spelled out so the
|
|
61
|
+
# module needs no pymavlink import to load, and so tests can assert on ids.
|
|
62
|
+
MAV_AUTOPILOT_ARDUPILOTMEGA = 3
|
|
63
|
+
MAV_TYPE_COPTER = frozenset({2, 13, 14, 15, 29}) # quad, hexa, octo, tri, dodeca
|
|
64
|
+
|
|
65
|
+
MAV_CMD_NAV_TAKEOFF = 22
|
|
66
|
+
MAV_CMD_DO_SET_MODE = 176
|
|
67
|
+
MAV_CMD_DO_SET_SERVO = 183
|
|
68
|
+
MAV_CMD_DO_SET_ROI_LOCATION = 195
|
|
69
|
+
MAV_CMD_DO_SET_ROI_NONE = 197
|
|
70
|
+
MAV_CMD_DO_DIGICAM_CONTROL = 203
|
|
71
|
+
MAV_CMD_DO_GRIPPER = 211
|
|
72
|
+
MAV_CMD_COMPONENT_ARM_DISARM = 400
|
|
73
|
+
MAV_CMD_SET_MESSAGE_INTERVAL = 511
|
|
74
|
+
MAV_CMD_REQUEST_MESSAGE = 512
|
|
75
|
+
MAV_CMD_DO_WINCH = 42600
|
|
76
|
+
|
|
77
|
+
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
|
|
78
|
+
MAV_MODE_FLAG_SAFETY_ARMED = 128
|
|
79
|
+
MAV_FRAME_LOCAL_NED = 1
|
|
80
|
+
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6
|
|
81
|
+
|
|
82
|
+
GRIPPER_ACTION_RELEASE = 0
|
|
83
|
+
GRIPPER_ACTION_GRAB = 1
|
|
84
|
+
WINCH_RELATIVE_LENGTH_CONTROL = 1 # ArduCopter implements 0/1/2 only; DELIVER(4)/RETRACT(6) return FAILED
|
|
85
|
+
|
|
86
|
+
MSG_ID_HEARTBEAT = 0
|
|
87
|
+
MSG_ID_GPS_RAW_INT = 24
|
|
88
|
+
MSG_ID_LOCAL_POSITION_NED = 32
|
|
89
|
+
MSG_ID_GLOBAL_POSITION_INT = 33
|
|
90
|
+
MSG_ID_BATTERY_STATUS = 147
|
|
91
|
+
MSG_ID_AUTOPILOT_VERSION = 148
|
|
92
|
+
|
|
93
|
+
# ArduCopter flight-mode numbers. pymavlink's ``mode_mapping()`` is
|
|
94
|
+
# preferred at runtime; this table is the fallback so the adapter also
|
|
95
|
+
# works against a minimal connection object.
|
|
96
|
+
COPTER_MODES: dict[str, int] = {
|
|
97
|
+
"STABILIZE": 0,
|
|
98
|
+
"ALT_HOLD": 2,
|
|
99
|
+
"AUTO": 3,
|
|
100
|
+
"GUIDED": 4,
|
|
101
|
+
"LOITER": 5,
|
|
102
|
+
"RTL": 6,
|
|
103
|
+
"LAND": 9,
|
|
104
|
+
"POSHOLD": 16,
|
|
105
|
+
"BRAKE": 17,
|
|
106
|
+
}
|
|
107
|
+
_MODE_NAMES = {v: k for k, v in COPTER_MODES.items()}
|
|
108
|
+
|
|
109
|
+
_MAV_RESULT_NAMES = {
|
|
110
|
+
0: "accepted",
|
|
111
|
+
1: "temporarily_rejected",
|
|
112
|
+
2: "denied",
|
|
113
|
+
3: "unsupported",
|
|
114
|
+
4: "failed",
|
|
115
|
+
5: "in_progress",
|
|
116
|
+
6: "cancelled",
|
|
117
|
+
}
|
|
118
|
+
|
|
119
|
+
# Position-only type_mask: ignore velocity, acceleration, yaw, yaw rate.
|
|
120
|
+
_POSITION_ONLY_MASK = 0b0000_1111_1111_1000
|
|
121
|
+
|
|
122
|
+
_EARTH_RADIUS_M = 6_371_000.0
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
def _haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
|
126
|
+
"""Great-circle distance in metres between two WGS84 points."""
|
|
127
|
+
p1, p2 = math.radians(lat1), math.radians(lat2)
|
|
128
|
+
dphi = p2 - p1
|
|
129
|
+
dlmb = math.radians(lon2 - lon1)
|
|
130
|
+
a = math.sin(dphi / 2) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(dlmb / 2) ** 2
|
|
131
|
+
return 2 * _EARTH_RADIUS_M * math.asin(math.sqrt(a))
|
|
132
|
+
|
|
133
|
+
|
|
134
|
+
class ArduCopterAdapter(PX4Adapter):
|
|
135
|
+
"""MAVLink adapter for ArduCopter over pymavlink."""
|
|
136
|
+
|
|
137
|
+
def __init__(self, config: ArduPilotAdapterConfig | None = None) -> None:
|
|
138
|
+
cfg = config or ArduPilotAdapterConfig()
|
|
139
|
+
super().__init__(cfg)
|
|
140
|
+
self._ap_config: ArduPilotAdapterConfig = cfg
|
|
141
|
+
self._statustext: deque[str] = deque(maxlen=20)
|
|
142
|
+
self._last_heartbeat: Any = None
|
|
143
|
+
self._last_global: Any = None
|
|
144
|
+
self._last_local: Any = None
|
|
145
|
+
self._capture_count = 0
|
|
146
|
+
self._roi_active = False
|
|
147
|
+
|
|
148
|
+
# ------------------------------------------------------------------
|
|
149
|
+
# Lifecycle
|
|
150
|
+
# ------------------------------------------------------------------
|
|
151
|
+
|
|
152
|
+
def __enter__(self) -> ArduCopterAdapter:
|
|
153
|
+
return self
|
|
154
|
+
|
|
155
|
+
def _connect(self) -> Any:
|
|
156
|
+
"""Open the link, verify it is an ArduCopter, request telemetry."""
|
|
157
|
+
if self._connection is not None:
|
|
158
|
+
return self._connection
|
|
159
|
+
url = self._ap_config.effective_connection_url()
|
|
160
|
+
conn = self._mavutil.mavlink_connection(
|
|
161
|
+
url,
|
|
162
|
+
source_system=self._ap_config.system_id,
|
|
163
|
+
source_component=self._ap_config.component_id,
|
|
164
|
+
)
|
|
165
|
+
hb = conn.wait_heartbeat(timeout=self._ap_config.heartbeat_timeout_seconds)
|
|
166
|
+
if hb is None:
|
|
167
|
+
with suppress(Exception):
|
|
168
|
+
conn.close()
|
|
169
|
+
raise RuntimeError(
|
|
170
|
+
f"heartbeat_timeout: no MAVLink heartbeat on {url!r} within "
|
|
171
|
+
f"{self._ap_config.heartbeat_timeout_seconds:.0f}s"
|
|
172
|
+
)
|
|
173
|
+
autopilot = getattr(hb, "autopilot", None)
|
|
174
|
+
vehicle_type = getattr(hb, "type", None)
|
|
175
|
+
if autopilot != MAV_AUTOPILOT_ARDUPILOTMEGA:
|
|
176
|
+
with suppress(Exception):
|
|
177
|
+
conn.close()
|
|
178
|
+
raise RuntimeError(
|
|
179
|
+
f"not_an_ardupilot_autopilot: heartbeat autopilot={autopilot!r} "
|
|
180
|
+
f"(expected {MAV_AUTOPILOT_ARDUPILOTMEGA} = MAV_AUTOPILOT_ARDUPILOTMEGA)"
|
|
181
|
+
)
|
|
182
|
+
if vehicle_type not in MAV_TYPE_COPTER:
|
|
183
|
+
with suppress(Exception):
|
|
184
|
+
conn.close()
|
|
185
|
+
raise RuntimeError(
|
|
186
|
+
f"not_a_copter: heartbeat MAV_TYPE={vehicle_type!r}; ArduCopterAdapter v0.1 "
|
|
187
|
+
"supports multirotor types only (ArduPlane / ArduRover are RFC-0041 follow-ups)"
|
|
188
|
+
)
|
|
189
|
+
self._last_heartbeat = hb
|
|
190
|
+
self._connection = conn
|
|
191
|
+
self._request_streams(conn)
|
|
192
|
+
return conn
|
|
193
|
+
|
|
194
|
+
def _request_streams(self, conn: Any) -> None:
|
|
195
|
+
"""Ask for position and battery at ``stream_rate_hz`` (fire and forget).
|
|
196
|
+
|
|
197
|
+
ArduPilot honours ``SET_MESSAGE_INTERVAL`` regardless of the
|
|
198
|
+
``SR*_`` stream parameters, so the measurement and arrival paths do
|
|
199
|
+
not depend on how the board happens to be configured.
|
|
200
|
+
"""
|
|
201
|
+
interval_us = int(1_000_000 / self._ap_config.stream_rate_hz)
|
|
202
|
+
for msg_id in (
|
|
203
|
+
MSG_ID_GLOBAL_POSITION_INT,
|
|
204
|
+
MSG_ID_LOCAL_POSITION_NED,
|
|
205
|
+
MSG_ID_BATTERY_STATUS,
|
|
206
|
+
MSG_ID_GPS_RAW_INT,
|
|
207
|
+
):
|
|
208
|
+
conn.mav.command_long_send(
|
|
209
|
+
conn.target_system,
|
|
210
|
+
conn.target_component,
|
|
211
|
+
MAV_CMD_SET_MESSAGE_INTERVAL,
|
|
212
|
+
0,
|
|
213
|
+
float(msg_id),
|
|
214
|
+
float(interval_us),
|
|
215
|
+
0.0,
|
|
216
|
+
0.0,
|
|
217
|
+
0.0,
|
|
218
|
+
0.0,
|
|
219
|
+
0.0,
|
|
220
|
+
)
|
|
221
|
+
|
|
222
|
+
# ------------------------------------------------------------------
|
|
223
|
+
# Low-level helpers
|
|
224
|
+
# ------------------------------------------------------------------
|
|
225
|
+
|
|
226
|
+
def _note(self, msg: Any) -> None:
|
|
227
|
+
"""Cache the messages the adapter reads state from."""
|
|
228
|
+
kind = msg.get_type() if hasattr(msg, "get_type") else None
|
|
229
|
+
if kind == "HEARTBEAT":
|
|
230
|
+
self._last_heartbeat = msg
|
|
231
|
+
elif kind == "GLOBAL_POSITION_INT":
|
|
232
|
+
self._last_global = msg
|
|
233
|
+
elif kind == "LOCAL_POSITION_NED":
|
|
234
|
+
self._last_local = msg
|
|
235
|
+
elif kind == "STATUSTEXT":
|
|
236
|
+
text = getattr(msg, "text", "")
|
|
237
|
+
if isinstance(text, bytes):
|
|
238
|
+
text = text.decode("utf-8", "replace")
|
|
239
|
+
text = str(text).rstrip("\x00").strip()
|
|
240
|
+
if text:
|
|
241
|
+
self._statustext.append(text)
|
|
242
|
+
|
|
243
|
+
def _recent_prearm_text(self, *, listen_seconds: float = 1.5) -> str:
|
|
244
|
+
"""The autopilot's own explanation for a refusal, if it gave one.
|
|
245
|
+
|
|
246
|
+
ArduPilot sends the ``PreArm:`` / ``Arm:`` STATUSTEXT lines shortly
|
|
247
|
+
*after* the failed COMMAND_ACK, so listen briefly before composing
|
|
248
|
+
the reason.
|
|
249
|
+
"""
|
|
250
|
+
conn = self._connection
|
|
251
|
+
if conn is not None and listen_seconds > 0:
|
|
252
|
+
deadline = time.monotonic() + listen_seconds
|
|
253
|
+
while True:
|
|
254
|
+
remaining = deadline - time.monotonic()
|
|
255
|
+
if remaining <= 0:
|
|
256
|
+
break
|
|
257
|
+
msg = conn.recv_match(type=["STATUSTEXT"], blocking=True, timeout=remaining)
|
|
258
|
+
if msg is None:
|
|
259
|
+
break
|
|
260
|
+
self._note(msg)
|
|
261
|
+
hits = [t for t in self._statustext if t.startswith(("PreArm", "Arm:", "Mode change", "Flight mode"))]
|
|
262
|
+
if not hits:
|
|
263
|
+
hits = list(self._statustext)[-2:]
|
|
264
|
+
return "; ".join(hits)
|
|
265
|
+
|
|
266
|
+
def _send_command_long(self, command: int, *params: float) -> tuple[bool, str | None]:
|
|
267
|
+
"""Send a MAV_CMD via COMMAND_LONG and wait for *its* COMMAND_ACK.
|
|
268
|
+
|
|
269
|
+
Unlike the PX4 base class, the ack is matched on ``ack.command`` so a
|
|
270
|
+
stray ack for an earlier command (stream requests, a GCS on the same
|
|
271
|
+
link) cannot be mistaken for ours. STATUSTEXT seen while waiting is
|
|
272
|
+
kept so a rejection can carry the autopilot's reason.
|
|
273
|
+
"""
|
|
274
|
+
try:
|
|
275
|
+
conn = self._connect()
|
|
276
|
+
except Exception as exc:
|
|
277
|
+
return False, f"connection_failed: {exc}"
|
|
278
|
+
padded = list(params) + [0.0] * (7 - len(params))
|
|
279
|
+
conn.mav.command_long_send(
|
|
280
|
+
conn.target_system,
|
|
281
|
+
conn.target_component,
|
|
282
|
+
command,
|
|
283
|
+
0,
|
|
284
|
+
*padded[:7],
|
|
285
|
+
)
|
|
286
|
+
deadline = time.monotonic() + self._ap_config.ack_timeout_seconds
|
|
287
|
+
while True:
|
|
288
|
+
remaining = deadline - time.monotonic()
|
|
289
|
+
if remaining <= 0:
|
|
290
|
+
return False, "ack_timeout"
|
|
291
|
+
msg = conn.recv_match(type=["COMMAND_ACK", "STATUSTEXT"], blocking=True, timeout=remaining)
|
|
292
|
+
if msg is None:
|
|
293
|
+
return False, "ack_timeout"
|
|
294
|
+
self._note(msg)
|
|
295
|
+
if msg.get_type() != "COMMAND_ACK":
|
|
296
|
+
continue
|
|
297
|
+
if int(getattr(msg, "command", -1)) != command:
|
|
298
|
+
continue
|
|
299
|
+
result = int(getattr(msg, "result", -1))
|
|
300
|
+
if result == 0:
|
|
301
|
+
return True, None
|
|
302
|
+
return False, f"mav_result_{_MAV_RESULT_NAMES.get(result, result)}"
|
|
303
|
+
|
|
304
|
+
def _wait_for(self, msg_type: str, predicate: Any, timeout_seconds: float) -> Any | None:
|
|
305
|
+
"""Read ``msg_type`` messages until ``predicate`` holds or time runs out."""
|
|
306
|
+
try:
|
|
307
|
+
conn = self._connect()
|
|
308
|
+
except Exception:
|
|
309
|
+
return None
|
|
310
|
+
deadline = time.monotonic() + timeout_seconds
|
|
311
|
+
while True:
|
|
312
|
+
remaining = deadline - time.monotonic()
|
|
313
|
+
if remaining <= 0:
|
|
314
|
+
return None
|
|
315
|
+
msg = conn.recv_match(type=[msg_type, "STATUSTEXT"], blocking=True, timeout=remaining)
|
|
316
|
+
if msg is None:
|
|
317
|
+
return None
|
|
318
|
+
self._note(msg)
|
|
319
|
+
if msg.get_type() == msg_type and predicate(msg):
|
|
320
|
+
return msg
|
|
321
|
+
|
|
322
|
+
def _mode_id(self, name: str) -> int | None:
|
|
323
|
+
try:
|
|
324
|
+
conn = self._connect()
|
|
325
|
+
except Exception:
|
|
326
|
+
return None
|
|
327
|
+
mapping = None
|
|
328
|
+
getter = getattr(conn, "mode_mapping", None)
|
|
329
|
+
if callable(getter):
|
|
330
|
+
with suppress(Exception):
|
|
331
|
+
mapping = getter()
|
|
332
|
+
if not mapping or name not in mapping:
|
|
333
|
+
mapping = COPTER_MODES
|
|
334
|
+
value = mapping.get(name)
|
|
335
|
+
return int(value) if value is not None else None
|
|
336
|
+
|
|
337
|
+
def _current_mode(self) -> int | None:
|
|
338
|
+
hb = self._last_heartbeat
|
|
339
|
+
if hb is None:
|
|
340
|
+
return None
|
|
341
|
+
return int(getattr(hb, "custom_mode", -1))
|
|
342
|
+
|
|
343
|
+
def _mode_name(self) -> str:
|
|
344
|
+
mode = self._current_mode()
|
|
345
|
+
if mode is None:
|
|
346
|
+
return "unknown"
|
|
347
|
+
return _MODE_NAMES.get(mode, str(mode))
|
|
348
|
+
|
|
349
|
+
def _is_armed(self) -> bool:
|
|
350
|
+
hb = self._last_heartbeat
|
|
351
|
+
if hb is None:
|
|
352
|
+
return False
|
|
353
|
+
return bool(int(getattr(hb, "base_mode", 0)) & MAV_MODE_FLAG_SAFETY_ARMED)
|
|
354
|
+
|
|
355
|
+
def _set_mode(self, name: str) -> tuple[bool, str | None]:
|
|
356
|
+
"""Enter a flight mode and confirm it on a heartbeat."""
|
|
357
|
+
try:
|
|
358
|
+
self._connect()
|
|
359
|
+
except Exception as exc:
|
|
360
|
+
return False, f"connection_failed: {exc}"
|
|
361
|
+
mode_id = self._mode_id(name)
|
|
362
|
+
if mode_id is None:
|
|
363
|
+
return False, f"mode_unknown: {name!r}"
|
|
364
|
+
if self._current_mode() == mode_id:
|
|
365
|
+
return True, None
|
|
366
|
+
ok, reason = self._send_command_long(
|
|
367
|
+
MAV_CMD_DO_SET_MODE, float(MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), float(mode_id)
|
|
368
|
+
)
|
|
369
|
+
if not ok:
|
|
370
|
+
detail = self._recent_prearm_text()
|
|
371
|
+
return False, f"mode_rejected: {name} {reason}" + (f"; {detail}" if detail else "")
|
|
372
|
+
hb = self._wait_for(
|
|
373
|
+
"HEARTBEAT",
|
|
374
|
+
lambda m: int(getattr(m, "custom_mode", -1)) == mode_id,
|
|
375
|
+
self._ap_config.mode_timeout_seconds,
|
|
376
|
+
)
|
|
377
|
+
if hb is None:
|
|
378
|
+
detail = self._recent_prearm_text()
|
|
379
|
+
return False, f"mode_rejected: {name} not confirmed on heartbeat" + (f"; {detail}" if detail else "")
|
|
380
|
+
return True, None
|
|
381
|
+
|
|
382
|
+
def _arm(self) -> tuple[bool, str | None]:
|
|
383
|
+
"""Arm and confirm the armed flag on a heartbeat. Never bypasses pre-arm."""
|
|
384
|
+
if self._is_armed():
|
|
385
|
+
return True, None
|
|
386
|
+
ok, reason = self._send_command_long(MAV_CMD_COMPONENT_ARM_DISARM, 1.0)
|
|
387
|
+
if not ok:
|
|
388
|
+
detail = self._recent_prearm_text()
|
|
389
|
+
return False, f"arm_rejected: {reason}" + (f"; {detail}" if detail else "")
|
|
390
|
+
hb = self._wait_for(
|
|
391
|
+
"HEARTBEAT",
|
|
392
|
+
lambda m: bool(int(getattr(m, "base_mode", 0)) & MAV_MODE_FLAG_SAFETY_ARMED),
|
|
393
|
+
self._ap_config.arm_timeout_seconds,
|
|
394
|
+
)
|
|
395
|
+
if hb is None:
|
|
396
|
+
detail = self._recent_prearm_text()
|
|
397
|
+
return False, "arm_rejected: armed flag not seen on heartbeat" + (f"; {detail}" if detail else "")
|
|
398
|
+
return True, None
|
|
399
|
+
|
|
400
|
+
def _relative_alt_m(self, msg: Any) -> float:
|
|
401
|
+
return float(getattr(msg, "relative_alt", 0)) / 1000.0
|
|
402
|
+
|
|
403
|
+
# ------------------------------------------------------------------
|
|
404
|
+
# Drone-profile dispatch
|
|
405
|
+
# ------------------------------------------------------------------
|
|
406
|
+
|
|
407
|
+
def send_takeoff_goal(
|
|
408
|
+
self,
|
|
409
|
+
*,
|
|
410
|
+
altitude: float,
|
|
411
|
+
climb_rate: float | None = None,
|
|
412
|
+
) -> NavigationResult:
|
|
413
|
+
target = float(altitude)
|
|
414
|
+
ok, reason = self._set_mode("GUIDED")
|
|
415
|
+
if not ok:
|
|
416
|
+
return NavigationResult(success=False, reason=reason)
|
|
417
|
+
ok, reason = self._arm()
|
|
418
|
+
if not ok:
|
|
419
|
+
return NavigationResult(success=False, reason=reason)
|
|
420
|
+
ok, reason = self._send_command_long(MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, target)
|
|
421
|
+
if not ok:
|
|
422
|
+
detail = self._recent_prearm_text()
|
|
423
|
+
return NavigationResult(
|
|
424
|
+
success=False,
|
|
425
|
+
reason=f"takeoff_rejected: {reason}" + (f"; {detail}" if detail else ""),
|
|
426
|
+
)
|
|
427
|
+
reached = self._wait_for(
|
|
428
|
+
"GLOBAL_POSITION_INT",
|
|
429
|
+
lambda m: self._relative_alt_m(m) >= 0.95 * target,
|
|
430
|
+
self._ap_config.takeoff_timeout_seconds,
|
|
431
|
+
)
|
|
432
|
+
if reached is None:
|
|
433
|
+
return NavigationResult(success=False, reason="takeoff_timeout: target altitude not reached")
|
|
434
|
+
return NavigationResult(
|
|
435
|
+
success=True,
|
|
436
|
+
final_pose={"x": 0.0, "y": 0.0, "z": self._relative_alt_m(reached)},
|
|
437
|
+
frame="agl",
|
|
438
|
+
)
|
|
439
|
+
|
|
440
|
+
def send_land_goal(
|
|
441
|
+
self,
|
|
442
|
+
*,
|
|
443
|
+
at: str | None = None,
|
|
444
|
+
precision: Literal["standard", "precise"] = "standard",
|
|
445
|
+
) -> NavigationResult:
|
|
446
|
+
self._clear_roi()
|
|
447
|
+
ok, reason = self._set_mode("LAND")
|
|
448
|
+
if not ok:
|
|
449
|
+
return NavigationResult(success=False, reason=reason)
|
|
450
|
+
# Landing ends with an automatic disarm. Wait for it, bounded; a
|
|
451
|
+
# confirmed mode change is still a success if the wait runs out.
|
|
452
|
+
self._wait_for(
|
|
453
|
+
"HEARTBEAT",
|
|
454
|
+
lambda m: not (int(getattr(m, "base_mode", 0)) & MAV_MODE_FLAG_SAFETY_ARMED),
|
|
455
|
+
self._ap_config.arrival_timeout_seconds,
|
|
456
|
+
)
|
|
457
|
+
return NavigationResult(success=True)
|
|
458
|
+
|
|
459
|
+
def send_return_to_home_goal(
|
|
460
|
+
self,
|
|
461
|
+
*,
|
|
462
|
+
speed: float | None = None,
|
|
463
|
+
altitude: float | None = None,
|
|
464
|
+
) -> NavigationResult:
|
|
465
|
+
self._clear_roi()
|
|
466
|
+
ok, reason = self._set_mode("RTL")
|
|
467
|
+
if not ok:
|
|
468
|
+
return NavigationResult(success=False, reason=reason)
|
|
469
|
+
return NavigationResult(success=True)
|
|
470
|
+
|
|
471
|
+
# ------------------------------------------------------------------
|
|
472
|
+
# Navigation
|
|
473
|
+
# ------------------------------------------------------------------
|
|
474
|
+
|
|
475
|
+
def send_navigation_goal(
|
|
476
|
+
self,
|
|
477
|
+
*,
|
|
478
|
+
location: str | None = None,
|
|
479
|
+
pose: dict[str, float] | None = None,
|
|
480
|
+
frame: str | None = None,
|
|
481
|
+
carrying: dict[str, Any] | None = None,
|
|
482
|
+
speed: float | None = None,
|
|
483
|
+
) -> NavigationResult:
|
|
484
|
+
# hover with no `over` reaches here as speed == 0 and no target:
|
|
485
|
+
# in GUIDED an ArduCopter holds position on its own, so this is a
|
|
486
|
+
# confirmed no-op rather than a failure.
|
|
487
|
+
if location is None and pose is None:
|
|
488
|
+
if speed == 0.0:
|
|
489
|
+
ok, reason = self._set_mode("GUIDED")
|
|
490
|
+
return NavigationResult(success=ok, reason=reason)
|
|
491
|
+
return NavigationResult(
|
|
492
|
+
success=False,
|
|
493
|
+
reason="send_navigation_goal called without location or pose",
|
|
494
|
+
)
|
|
495
|
+
|
|
496
|
+
if location is not None:
|
|
497
|
+
gp = self._ap_config.resolve_global(location)
|
|
498
|
+
if gp is not None:
|
|
499
|
+
return self._fly_global(gp)
|
|
500
|
+
ned = self._ap_config.resolve_location(location)
|
|
501
|
+
if ned is None:
|
|
502
|
+
return NavigationResult(
|
|
503
|
+
success=False,
|
|
504
|
+
reason=f"location_not_configured: {location!r} is declared in the manifest "
|
|
505
|
+
"but not bound in the adapter config (location_to_global or location_to_pose).",
|
|
506
|
+
)
|
|
507
|
+
north, east, alt = ned.north, ned.east, ned.alt
|
|
508
|
+
else:
|
|
509
|
+
assert pose is not None
|
|
510
|
+
north = float(pose.get("x", 0.0))
|
|
511
|
+
east = float(pose.get("y", 0.0))
|
|
512
|
+
alt = float(pose.get("z", 0.0))
|
|
513
|
+
return self._fly_local(north, east, alt, frame)
|
|
514
|
+
|
|
515
|
+
def _fly_local(self, north: float, east: float, alt: float, frame: str | None) -> NavigationResult:
|
|
516
|
+
ok, reason = self._set_mode("GUIDED")
|
|
517
|
+
if not ok:
|
|
518
|
+
return NavigationResult(success=False, reason=reason)
|
|
519
|
+
conn = self._connection
|
|
520
|
+
conn.mav.set_position_target_local_ned_send(
|
|
521
|
+
0,
|
|
522
|
+
conn.target_system,
|
|
523
|
+
conn.target_component,
|
|
524
|
+
MAV_FRAME_LOCAL_NED,
|
|
525
|
+
_POSITION_ONLY_MASK,
|
|
526
|
+
north,
|
|
527
|
+
east,
|
|
528
|
+
-alt,
|
|
529
|
+
0.0,
|
|
530
|
+
0.0,
|
|
531
|
+
0.0,
|
|
532
|
+
0.0,
|
|
533
|
+
0.0,
|
|
534
|
+
0.0,
|
|
535
|
+
0.0,
|
|
536
|
+
0.0,
|
|
537
|
+
)
|
|
538
|
+
radius = self._ap_config.arrival_radius_m
|
|
539
|
+
alt_tol = self._ap_config.arrival_alt_tolerance_m
|
|
540
|
+
|
|
541
|
+
def _arrived(m: Any) -> bool:
|
|
542
|
+
dx = float(getattr(m, "x", 0.0)) - north
|
|
543
|
+
dy = float(getattr(m, "y", 0.0)) - east
|
|
544
|
+
dz = -float(getattr(m, "z", 0.0)) - alt
|
|
545
|
+
return math.hypot(dx, dy) <= radius and abs(dz) <= alt_tol
|
|
546
|
+
|
|
547
|
+
got = self._wait_for("LOCAL_POSITION_NED", _arrived, self._ap_config.arrival_timeout_seconds)
|
|
548
|
+
if got is None:
|
|
549
|
+
return NavigationResult(success=False, reason="arrival_timeout: setpoint not reached")
|
|
550
|
+
return NavigationResult(
|
|
551
|
+
success=True,
|
|
552
|
+
final_pose={"x": north, "y": east, "z": alt},
|
|
553
|
+
frame=frame or "ned",
|
|
554
|
+
)
|
|
555
|
+
|
|
556
|
+
def _fly_global(self, gp: GlobalPosition) -> NavigationResult:
|
|
557
|
+
ok, reason = self._set_mode("GUIDED")
|
|
558
|
+
if not ok:
|
|
559
|
+
return NavigationResult(success=False, reason=reason)
|
|
560
|
+
conn = self._connection
|
|
561
|
+
conn.mav.set_position_target_global_int_send(
|
|
562
|
+
0,
|
|
563
|
+
conn.target_system,
|
|
564
|
+
conn.target_component,
|
|
565
|
+
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
566
|
+
_POSITION_ONLY_MASK,
|
|
567
|
+
round(gp.lat * 1e7),
|
|
568
|
+
round(gp.lon * 1e7),
|
|
569
|
+
float(gp.alt_agl),
|
|
570
|
+
0.0,
|
|
571
|
+
0.0,
|
|
572
|
+
0.0,
|
|
573
|
+
0.0,
|
|
574
|
+
0.0,
|
|
575
|
+
0.0,
|
|
576
|
+
0.0,
|
|
577
|
+
0.0,
|
|
578
|
+
)
|
|
579
|
+
radius = self._ap_config.arrival_radius_m
|
|
580
|
+
alt_tol = self._ap_config.arrival_alt_tolerance_m
|
|
581
|
+
|
|
582
|
+
def _arrived(m: Any) -> bool:
|
|
583
|
+
lat = float(getattr(m, "lat", 0)) / 1e7
|
|
584
|
+
lon = float(getattr(m, "lon", 0)) / 1e7
|
|
585
|
+
d = _haversine_m(lat, lon, gp.lat, gp.lon)
|
|
586
|
+
return d <= radius and abs(self._relative_alt_m(m) - gp.alt_agl) <= alt_tol
|
|
587
|
+
|
|
588
|
+
got = self._wait_for("GLOBAL_POSITION_INT", _arrived, self._ap_config.arrival_timeout_seconds)
|
|
589
|
+
if got is None:
|
|
590
|
+
return NavigationResult(success=False, reason="arrival_timeout: global setpoint not reached")
|
|
591
|
+
if gp.look_at is not None:
|
|
592
|
+
ok, reason = self._send_command_long(
|
|
593
|
+
MAV_CMD_DO_SET_ROI_LOCATION, 0, 0, 0, 0, gp.look_at.lat, gp.look_at.lon, 0.0
|
|
594
|
+
)
|
|
595
|
+
if not ok:
|
|
596
|
+
return NavigationResult(success=False, reason=f"roi_rejected: {reason}")
|
|
597
|
+
self._roi_active = True
|
|
598
|
+
return NavigationResult(
|
|
599
|
+
success=True,
|
|
600
|
+
final_pose={"x": gp.lon, "y": gp.lat, "z": gp.alt_agl},
|
|
601
|
+
frame="wgs84",
|
|
602
|
+
)
|
|
603
|
+
|
|
604
|
+
def _clear_roi(self) -> None:
|
|
605
|
+
if not self._roi_active:
|
|
606
|
+
return
|
|
607
|
+
self._roi_active = False
|
|
608
|
+
with suppress(Exception):
|
|
609
|
+
self._send_command_long(MAV_CMD_DO_SET_ROI_NONE)
|
|
610
|
+
|
|
611
|
+
# ------------------------------------------------------------------
|
|
612
|
+
# Camera
|
|
613
|
+
# ------------------------------------------------------------------
|
|
614
|
+
|
|
615
|
+
def capture_media(
|
|
616
|
+
self,
|
|
617
|
+
*,
|
|
618
|
+
media: Literal["photo", "video"],
|
|
619
|
+
target: str | None,
|
|
620
|
+
duration_seconds: float | None,
|
|
621
|
+
attributes: dict[str, Any] | None,
|
|
622
|
+
) -> CaptureResult:
|
|
623
|
+
cam = self._ap_config.camera
|
|
624
|
+
if cam is None:
|
|
625
|
+
return CaptureResult(
|
|
626
|
+
success=False,
|
|
627
|
+
reason="camera_not_configured: set `camera:` in the adapter config "
|
|
628
|
+
"(kind: digicam | servo) to trigger an autopilot-wired camera.",
|
|
629
|
+
)
|
|
630
|
+
if media != "photo":
|
|
631
|
+
return CaptureResult(
|
|
632
|
+
success=False, reason="video_not_supported: ArduCopterAdapter v0.1 triggers stills only"
|
|
633
|
+
)
|
|
634
|
+
if cam.kind == "servo":
|
|
635
|
+
if cam.channel is None:
|
|
636
|
+
return CaptureResult(success=False, reason="camera_not_configured: servo trigger needs `channel`")
|
|
637
|
+
ok, reason = self._send_command_long(MAV_CMD_DO_SET_SERVO, float(cam.channel), float(cam.pwm_on))
|
|
638
|
+
if ok:
|
|
639
|
+
time.sleep(cam.pulse_ms / 1000.0)
|
|
640
|
+
ok, reason = self._send_command_long(MAV_CMD_DO_SET_SERVO, float(cam.channel), float(cam.pwm_off))
|
|
641
|
+
else:
|
|
642
|
+
# DO_DIGICAM_CONTROL: p5 = shoot command (1 = trigger).
|
|
643
|
+
ok, reason = self._send_command_long(MAV_CMD_DO_DIGICAM_CONTROL, 0, 0, 0, 0, 1, 0, 0)
|
|
644
|
+
if not ok:
|
|
645
|
+
return CaptureResult(success=False, reason=f"capture_rejected: {reason}")
|
|
646
|
+
if cam.settle_seconds > 0:
|
|
647
|
+
time.sleep(cam.settle_seconds)
|
|
648
|
+
self._capture_count += 1
|
|
649
|
+
g = self._last_global
|
|
650
|
+
pose = (
|
|
651
|
+
{
|
|
652
|
+
"lat": float(getattr(g, "lat", 0)) / 1e7,
|
|
653
|
+
"lon": float(getattr(g, "lon", 0)) / 1e7,
|
|
654
|
+
"alt_agl": self._relative_alt_m(g),
|
|
655
|
+
}
|
|
656
|
+
if g is not None
|
|
657
|
+
else None
|
|
658
|
+
)
|
|
659
|
+
return CaptureResult(
|
|
660
|
+
success=True,
|
|
661
|
+
payload={
|
|
662
|
+
"type": "photo",
|
|
663
|
+
"format": "on-camera",
|
|
664
|
+
"uri": f"camera://shot/{self._capture_count}",
|
|
665
|
+
"pose": pose,
|
|
666
|
+
"frame": "wgs84" if pose else None,
|
|
667
|
+
"timestamp": time.time(),
|
|
668
|
+
"_note": "Image is stored on the camera, not transferred over MAVLink. "
|
|
669
|
+
"The pose is the autopilot position at trigger time.",
|
|
670
|
+
},
|
|
671
|
+
)
|
|
672
|
+
|
|
673
|
+
# ------------------------------------------------------------------
|
|
674
|
+
# Output lines (RFC-0017): gripper, winch, servo
|
|
675
|
+
# ------------------------------------------------------------------
|
|
676
|
+
|
|
677
|
+
def set_output_line(
|
|
678
|
+
self,
|
|
679
|
+
*,
|
|
680
|
+
output: str,
|
|
681
|
+
value: bool | float,
|
|
682
|
+
pulse_ms: float | None = None,
|
|
683
|
+
) -> SubstrateResult:
|
|
684
|
+
binding = self._ap_config.output_lines.get(output)
|
|
685
|
+
if binding is None:
|
|
686
|
+
return SubstrateResult(
|
|
687
|
+
success=False,
|
|
688
|
+
reason=f"output_line_not_configured: {output!r} has no entry in the adapter config `output_lines`.",
|
|
689
|
+
)
|
|
690
|
+
on = bool(value)
|
|
691
|
+
ok, reason = self._drive_line(binding, on)
|
|
692
|
+
if not ok:
|
|
693
|
+
return SubstrateResult(success=False, reason=f"output_rejected: {output} {reason}")
|
|
694
|
+
if pulse_ms is not None:
|
|
695
|
+
time.sleep(float(pulse_ms) / 1000.0)
|
|
696
|
+
ok, reason = self._drive_line(binding, not on)
|
|
697
|
+
if not ok:
|
|
698
|
+
return SubstrateResult(success=False, reason=f"output_rejected: {output} revert {reason}")
|
|
699
|
+
return SubstrateResult(success=True)
|
|
700
|
+
|
|
701
|
+
def _drive_line(self, binding: OutputLineBinding, on: bool) -> tuple[bool, str | None]:
|
|
702
|
+
if binding.kind == "gripper":
|
|
703
|
+
action = GRIPPER_ACTION_RELEASE if on else GRIPPER_ACTION_GRAB
|
|
704
|
+
return self._send_command_long(MAV_CMD_DO_GRIPPER, float(binding.instance), float(action))
|
|
705
|
+
if binding.kind == "winch":
|
|
706
|
+
# ArduCopter's DO_WINCH handler (4.6.x) accepts RELAXED, RELATIVE_LENGTH_CONTROL,
|
|
707
|
+
# and RATE_CONTROL; the enum's DELIVER / RETRACT values are rejected. Deliver is
|
|
708
|
+
# a positive relative length, retract the same length negative.
|
|
709
|
+
length = binding.deliver_length_m if on else -binding.deliver_length_m
|
|
710
|
+
return self._send_command_long(
|
|
711
|
+
MAV_CMD_DO_WINCH,
|
|
712
|
+
float(binding.instance),
|
|
713
|
+
float(WINCH_RELATIVE_LENGTH_CONTROL),
|
|
714
|
+
length,
|
|
715
|
+
binding.rate_m_s,
|
|
716
|
+
)
|
|
717
|
+
if binding.channel is None:
|
|
718
|
+
return False, "servo binding needs `channel`"
|
|
719
|
+
pwm = binding.on_pwm if on else binding.off_pwm
|
|
720
|
+
return self._send_command_long(MAV_CMD_DO_SET_SERVO, float(binding.channel), float(pwm))
|
|
721
|
+
|
|
722
|
+
# ------------------------------------------------------------------
|
|
723
|
+
# Read-only identity probe
|
|
724
|
+
# ------------------------------------------------------------------
|
|
725
|
+
|
|
726
|
+
def probe(self, *, listen_seconds: float = 3.0) -> dict[str, Any]:
|
|
727
|
+
"""Identity snapshot. Sends only a version request; changes no state."""
|
|
728
|
+
conn = self._connect()
|
|
729
|
+
hb = self._last_heartbeat
|
|
730
|
+
out: dict[str, Any] = {
|
|
731
|
+
"connection_url": self._ap_config.effective_connection_url(),
|
|
732
|
+
"autopilot": "ArduPilot" if getattr(hb, "autopilot", None) == MAV_AUTOPILOT_ARDUPILOTMEGA else "unknown",
|
|
733
|
+
"mav_type": int(getattr(hb, "type", -1)),
|
|
734
|
+
"system_id": int(hb.get_srcSystem()) if hasattr(hb, "get_srcSystem") else int(conn.target_system),
|
|
735
|
+
"component_id": (
|
|
736
|
+
int(hb.get_srcComponent()) if hasattr(hb, "get_srcComponent") else int(conn.target_component)
|
|
737
|
+
),
|
|
738
|
+
"armed": self._is_armed(),
|
|
739
|
+
"mode": self._mode_name(),
|
|
740
|
+
"firmware": None,
|
|
741
|
+
"battery_v": None,
|
|
742
|
+
"gps_fix": None,
|
|
743
|
+
"satellites": None,
|
|
744
|
+
}
|
|
745
|
+
conn.mav.command_long_send(
|
|
746
|
+
conn.target_system,
|
|
747
|
+
conn.target_component,
|
|
748
|
+
MAV_CMD_REQUEST_MESSAGE,
|
|
749
|
+
0,
|
|
750
|
+
float(MSG_ID_AUTOPILOT_VERSION),
|
|
751
|
+
0.0,
|
|
752
|
+
0.0,
|
|
753
|
+
0.0,
|
|
754
|
+
0.0,
|
|
755
|
+
0.0,
|
|
756
|
+
0.0,
|
|
757
|
+
)
|
|
758
|
+
deadline = time.monotonic() + listen_seconds
|
|
759
|
+
while True:
|
|
760
|
+
remaining = deadline - time.monotonic()
|
|
761
|
+
if remaining <= 0:
|
|
762
|
+
break
|
|
763
|
+
msg = conn.recv_match(blocking=True, timeout=remaining)
|
|
764
|
+
if msg is None:
|
|
765
|
+
break
|
|
766
|
+
self._note(msg)
|
|
767
|
+
kind = msg.get_type()
|
|
768
|
+
if kind == "AUTOPILOT_VERSION":
|
|
769
|
+
fv = int(getattr(msg, "flight_sw_version", 0))
|
|
770
|
+
out["firmware"] = f"{(fv >> 24) & 0xFF}.{(fv >> 16) & 0xFF}.{(fv >> 8) & 0xFF}"
|
|
771
|
+
elif kind == "BATTERY_STATUS":
|
|
772
|
+
volts = getattr(msg, "voltages", None) or [0]
|
|
773
|
+
if volts and int(volts[0]) != 0xFFFF:
|
|
774
|
+
out["battery_v"] = round(float(volts[0]) / 1000.0, 2)
|
|
775
|
+
elif kind == "SYS_STATUS" and out["battery_v"] is None:
|
|
776
|
+
mv = int(getattr(msg, "voltage_battery", 0))
|
|
777
|
+
if mv and mv != 0xFFFF:
|
|
778
|
+
out["battery_v"] = round(mv / 1000.0, 2)
|
|
779
|
+
elif kind == "GPS_RAW_INT":
|
|
780
|
+
out["gps_fix"] = int(getattr(msg, "fix_type", 0))
|
|
781
|
+
out["satellites"] = int(getattr(msg, "satellites_visible", 0))
|
|
782
|
+
elif kind == "HEARTBEAT":
|
|
783
|
+
out["armed"] = self._is_armed()
|
|
784
|
+
out["mode"] = self._mode_name()
|
|
785
|
+
out["statustext"] = list(self._statustext)
|
|
786
|
+
return out
|
|
787
|
+
|
|
788
|
+
|
|
789
|
+
def probe(config: ArduPilotAdapterConfig | str | None = None, *, listen_seconds: float = 3.0) -> dict[str, Any]:
|
|
790
|
+
"""Convenience: open, probe, close. ``config`` may be a path or a config."""
|
|
791
|
+
if isinstance(config, str):
|
|
792
|
+
cfg = load_ardupilot_config(config)
|
|
793
|
+
else:
|
|
794
|
+
cfg = config or ArduPilotAdapterConfig()
|
|
795
|
+
with ArduCopterAdapter(cfg) as adapter:
|
|
796
|
+
return adapter.probe(listen_seconds=listen_seconds)
|
|
@@ -0,0 +1,159 @@
|
|
|
1
|
+
"""Deployment-side configuration for ``ArduCopterAdapter``.
|
|
2
|
+
|
|
3
|
+
Extends ``PX4AdapterConfig`` (the connection URL, MAVLink identity, default
|
|
4
|
+
timeouts, and the name -> local-NED binding) with what an ArduPilot
|
|
5
|
+
deployment additionally needs:
|
|
6
|
+
|
|
7
|
+
- A serial ``baud`` (a Pixhawk on USB ignores it; a SiK telemetry radio
|
|
8
|
+
does not).
|
|
9
|
+
- ArduPilot-specific timeouts: mode entry, arming, take-off climb,
|
|
10
|
+
arrival at a setpoint.
|
|
11
|
+
- A name -> WGS84 binding (``location_to_global``) so a manifest location
|
|
12
|
+
can be flown as a global setpoint. Produced offline by
|
|
13
|
+
``tools/scripts/geocode_locations.py``; the runtime never geocodes.
|
|
14
|
+
- A camera trigger description and named output-line bindings (gripper,
|
|
15
|
+
winch, servo) for ``capture`` and ``set_output``.
|
|
16
|
+
|
|
17
|
+
All of this is deployment- and aircraft-specific, so it lives in an
|
|
18
|
+
``ardupilot_adapter.yaml`` next to the URML artifacts, never in the
|
|
19
|
+
program, manifest, or envelope.
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
from __future__ import annotations
|
|
23
|
+
|
|
24
|
+
import re
|
|
25
|
+
from pathlib import Path
|
|
26
|
+
from typing import Literal
|
|
27
|
+
|
|
28
|
+
import yaml
|
|
29
|
+
from pydantic import BaseModel, ConfigDict, Field
|
|
30
|
+
from urml_px4_runtime.config import NEDPosition, PX4AdapterConfig
|
|
31
|
+
|
|
32
|
+
__all__ = [
|
|
33
|
+
"ArduPilotAdapterConfig",
|
|
34
|
+
"CameraTrigger",
|
|
35
|
+
"GlobalPosition",
|
|
36
|
+
"LookAt",
|
|
37
|
+
"NEDPosition",
|
|
38
|
+
"OutputLineBinding",
|
|
39
|
+
"load_ardupilot_config",
|
|
40
|
+
]
|
|
41
|
+
|
|
42
|
+
_SERIAL_URL = re.compile(r"^(COM\d+|/dev/tty[A-Za-z0-9.]+)$", re.IGNORECASE)
|
|
43
|
+
|
|
44
|
+
|
|
45
|
+
class LookAt(BaseModel):
|
|
46
|
+
"""A WGS84 point of interest the aircraft yaws toward after arrival."""
|
|
47
|
+
|
|
48
|
+
model_config = ConfigDict(extra="forbid")
|
|
49
|
+
|
|
50
|
+
lat: float = Field(..., ge=-90.0, le=90.0)
|
|
51
|
+
lon: float = Field(..., ge=-180.0, le=180.0)
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
class GlobalPosition(BaseModel):
|
|
55
|
+
"""A WGS84 position with altitude above the launch point.
|
|
56
|
+
|
|
57
|
+
``alt_agl`` is metres above home, which is what ArduCopter's
|
|
58
|
+
``MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`` means. Terrain-following is a
|
|
59
|
+
substrate parameter, not something this config models.
|
|
60
|
+
"""
|
|
61
|
+
|
|
62
|
+
model_config = ConfigDict(extra="forbid")
|
|
63
|
+
|
|
64
|
+
lat: float = Field(..., ge=-90.0, le=90.0)
|
|
65
|
+
lon: float = Field(..., ge=-180.0, le=180.0)
|
|
66
|
+
alt_agl: float = Field(..., ge=0.0)
|
|
67
|
+
look_at: LookAt | None = None
|
|
68
|
+
|
|
69
|
+
|
|
70
|
+
class CameraTrigger(BaseModel):
|
|
71
|
+
"""How ``capture(media: photo)`` fires the on-board camera.
|
|
72
|
+
|
|
73
|
+
``digicam`` sends ``MAV_CMD_DO_DIGICAM_CONTROL`` and lets the
|
|
74
|
+
autopilot's ``CAM_TRIGG_TYPE`` decide the physical output. ``servo``
|
|
75
|
+
pulses a servo channel directly (``MAV_CMD_DO_SET_SERVO``) for shutter
|
|
76
|
+
cables wired to an AUX output.
|
|
77
|
+
"""
|
|
78
|
+
|
|
79
|
+
model_config = ConfigDict(extra="forbid")
|
|
80
|
+
|
|
81
|
+
kind: Literal["digicam", "servo"] = "digicam"
|
|
82
|
+
channel: int | None = Field(None, ge=1, le=16, description="Servo output channel (servo kind only).")
|
|
83
|
+
pwm_on: int = Field(1900, ge=800, le=2200)
|
|
84
|
+
pwm_off: int = Field(1100, ge=800, le=2200)
|
|
85
|
+
pulse_ms: float = Field(300.0, gt=0)
|
|
86
|
+
settle_seconds: float = Field(0.5, ge=0, description="Pause after the trigger before the result is returned.")
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
class OutputLineBinding(BaseModel):
|
|
90
|
+
"""Binds a manifest-declared output line to an ArduPilot mechanism."""
|
|
91
|
+
|
|
92
|
+
model_config = ConfigDict(extra="forbid")
|
|
93
|
+
|
|
94
|
+
kind: Literal["gripper", "winch", "servo"]
|
|
95
|
+
instance: int = Field(1, ge=1, description="Gripper / winch instance number (gripper, winch kinds).")
|
|
96
|
+
channel: int | None = Field(None, ge=1, le=16, description="Servo output channel (servo kind).")
|
|
97
|
+
on_pwm: int = Field(1900, ge=800, le=2200)
|
|
98
|
+
off_pwm: int = Field(1100, ge=800, le=2200)
|
|
99
|
+
deliver_length_m: float = Field(10.0, gt=0, description="Winch: line length paid out on `true`.")
|
|
100
|
+
rate_m_s: float = Field(0.5, gt=0, description="Winch: line rate for deliver / retract.")
|
|
101
|
+
|
|
102
|
+
|
|
103
|
+
class ArduPilotAdapterConfig(PX4AdapterConfig):
|
|
104
|
+
"""ArduPilot / MAVLink connection and binding config."""
|
|
105
|
+
|
|
106
|
+
model_config = ConfigDict(extra="forbid")
|
|
107
|
+
|
|
108
|
+
connection_url: str = Field(
|
|
109
|
+
default="udp:127.0.0.1:14550",
|
|
110
|
+
description=(
|
|
111
|
+
"pymavlink connection string. ArduCopter SITL GCS port: 'udp:127.0.0.1:14550'. "
|
|
112
|
+
"USB: 'COM5' or '/dev/ttyACM0'. Telemetry radio: '/dev/ttyUSB0' with `baud`."
|
|
113
|
+
),
|
|
114
|
+
)
|
|
115
|
+
baud: int = Field(115200, description="Serial baud rate; appended to serial-shaped URLs without one.")
|
|
116
|
+
component_id: int = Field(
|
|
117
|
+
default=190,
|
|
118
|
+
description="MAV_COMP_ID_MISSIONPLANNER. ArduPilot expects a GCS-class component id from an operator link.",
|
|
119
|
+
)
|
|
120
|
+
vehicle: Literal["copter"] = "copter"
|
|
121
|
+
|
|
122
|
+
mode_timeout_seconds: float = 5.0
|
|
123
|
+
arm_timeout_seconds: float = 10.0
|
|
124
|
+
takeoff_timeout_seconds: float = 60.0
|
|
125
|
+
arrival_radius_m: float = Field(1.5, gt=0)
|
|
126
|
+
arrival_alt_tolerance_m: float = Field(1.0, gt=0)
|
|
127
|
+
arrival_timeout_seconds: float = 120.0
|
|
128
|
+
stream_rate_hz: float = Field(2.0, gt=0, description="Requested rate for position and battery telemetry.")
|
|
129
|
+
|
|
130
|
+
location_to_global: dict[str, GlobalPosition] = Field(
|
|
131
|
+
default_factory=dict,
|
|
132
|
+
description="Map manifest-declared location names to WGS84 positions. Wins over location_to_pose.",
|
|
133
|
+
)
|
|
134
|
+
camera: CameraTrigger | None = None
|
|
135
|
+
output_lines: dict[str, OutputLineBinding] = Field(
|
|
136
|
+
default_factory=dict,
|
|
137
|
+
description="Map manifest `outputs.lines` names to gripper / winch / servo mechanisms.",
|
|
138
|
+
)
|
|
139
|
+
|
|
140
|
+
def effective_connection_url(self) -> str:
|
|
141
|
+
"""The URL handed to pymavlink: serial ports get `,<baud>` appended."""
|
|
142
|
+
url = self.connection_url
|
|
143
|
+
if _SERIAL_URL.match(url):
|
|
144
|
+
return f"{url},{self.baud}"
|
|
145
|
+
return url
|
|
146
|
+
|
|
147
|
+
def resolve_global(self, name: str) -> GlobalPosition | None:
|
|
148
|
+
"""Return the WGS84 binding for a named location, or None."""
|
|
149
|
+
return self.location_to_global.get(name)
|
|
150
|
+
|
|
151
|
+
|
|
152
|
+
def load_ardupilot_config(path: str | Path) -> ArduPilotAdapterConfig:
|
|
153
|
+
"""Parse an ``ardupilot_adapter.yaml`` file into an ``ArduPilotAdapterConfig``."""
|
|
154
|
+
p = Path(path)
|
|
155
|
+
with p.open(encoding="utf-8") as fh:
|
|
156
|
+
data = yaml.safe_load(fh) or {}
|
|
157
|
+
if not isinstance(data, dict):
|
|
158
|
+
raise ValueError(f"ardupilot-config file {p} did not contain a YAML mapping at the top level.")
|
|
159
|
+
return ArduPilotAdapterConfig.model_validate(data)
|
|
@@ -0,0 +1,45 @@
|
|
|
1
|
+
"""``python -m urml_ardupilot_runtime.probe COM5`` — read-only bring-up check.
|
|
2
|
+
|
|
3
|
+
Opens the link, waits for a heartbeat, prints what the autopilot says it
|
|
4
|
+
is, and closes. Sends no mode, arm, or motion command. Exit 0 on a
|
|
5
|
+
heartbeat from an ArduCopter, 1 otherwise.
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from __future__ import annotations
|
|
9
|
+
|
|
10
|
+
import argparse
|
|
11
|
+
import sys
|
|
12
|
+
|
|
13
|
+
from urml_ardupilot_runtime.adapter import probe
|
|
14
|
+
from urml_ardupilot_runtime.config import ArduPilotAdapterConfig
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
def main(argv: list[str] | None = None) -> int:
|
|
18
|
+
parser = argparse.ArgumentParser(
|
|
19
|
+
prog="python -m urml_ardupilot_runtime.probe",
|
|
20
|
+
description="Read-only identity probe of a connected ArduPilot autopilot.",
|
|
21
|
+
)
|
|
22
|
+
parser.add_argument("connection", help="pymavlink URL: COM5, /dev/ttyACM0, udp:127.0.0.1:14550 ...")
|
|
23
|
+
parser.add_argument("--baud", type=int, default=115200)
|
|
24
|
+
parser.add_argument("--listen", type=float, default=3.0, help="Seconds to listen for telemetry.")
|
|
25
|
+
args = parser.parse_args(argv)
|
|
26
|
+
|
|
27
|
+
cfg = ArduPilotAdapterConfig(connection_url=args.connection, baud=args.baud)
|
|
28
|
+
try:
|
|
29
|
+
info = probe(cfg, listen_seconds=args.listen)
|
|
30
|
+
except RuntimeError as exc:
|
|
31
|
+
print(f"probe failed: {exc}", file=sys.stderr)
|
|
32
|
+
return 1
|
|
33
|
+
for key, value in info.items():
|
|
34
|
+
if key == "statustext":
|
|
35
|
+
continue
|
|
36
|
+
print(f"{key}: {value}")
|
|
37
|
+
if info.get("statustext"):
|
|
38
|
+
print("statustext:")
|
|
39
|
+
for line in info["statustext"]:
|
|
40
|
+
print(f" - {line}")
|
|
41
|
+
return 0
|
|
42
|
+
|
|
43
|
+
|
|
44
|
+
if __name__ == "__main__":
|
|
45
|
+
raise SystemExit(main())
|
|
@@ -0,0 +1,162 @@
|
|
|
1
|
+
Metadata-Version: 2.5
|
|
2
|
+
Name: urml-ardupilot-runtime
|
|
3
|
+
Version: 0.4.0
|
|
4
|
+
Summary: ArduPilot / MAVLink reference runtime for URML — Universal Robot Language.
|
|
5
|
+
Project-URL: Homepage, https://github.com/URML-MARS/URML
|
|
6
|
+
Project-URL: Repository, https://github.com/URML-MARS/URML
|
|
7
|
+
Project-URL: Issues, https://github.com/URML-MARS/URML/issues
|
|
8
|
+
Author: URML Maintainers
|
|
9
|
+
License: Apache-2.0
|
|
10
|
+
Keywords: arducopter,ardupilot,drone,mavlink,pixhawk,robotics,runtime,urml
|
|
11
|
+
Classifier: Development Status :: 3 - Alpha
|
|
12
|
+
Classifier: Intended Audience :: Developers
|
|
13
|
+
Classifier: License :: OSI Approved :: Apache Software License
|
|
14
|
+
Classifier: Operating System :: OS Independent
|
|
15
|
+
Classifier: Programming Language :: Python :: 3 :: Only
|
|
16
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
17
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
18
|
+
Classifier: Topic :: Scientific/Engineering
|
|
19
|
+
Requires-Python: >=3.11
|
|
20
|
+
Requires-Dist: pydantic<3,>=2.6
|
|
21
|
+
Requires-Dist: pyyaml<7,>=6.0
|
|
22
|
+
Requires-Dist: urml-px4-runtime>=0.4.0
|
|
23
|
+
Requires-Dist: urml-ros2-runtime>=0.4.0
|
|
24
|
+
Requires-Dist: urml-validator>=0.4.0
|
|
25
|
+
Provides-Extra: ardupilot
|
|
26
|
+
Requires-Dist: pymavlink<3,>=2.4; extra == 'ardupilot'
|
|
27
|
+
Requires-Dist: pyserial<4,>=3.5; extra == 'ardupilot'
|
|
28
|
+
Provides-Extra: dev
|
|
29
|
+
Requires-Dist: mypy>=1.10; extra == 'dev'
|
|
30
|
+
Requires-Dist: pytest-cov>=5; extra == 'dev'
|
|
31
|
+
Requires-Dist: pytest>=8; extra == 'dev'
|
|
32
|
+
Requires-Dist: ruff>=0.5; extra == 'dev'
|
|
33
|
+
Description-Content-Type: text/markdown
|
|
34
|
+
|
|
35
|
+
<p align="center">
|
|
36
|
+
<a href="https://urml.dev"><img src="https://urml.dev/favicon.svg" alt="URML" width="72" height="72"></a>
|
|
37
|
+
</p>
|
|
38
|
+
|
|
39
|
+
<p align="center">
|
|
40
|
+
A small, opinionated, human-readable language for describing robot intent.
|
|
41
|
+
</p>
|
|
42
|
+
|
|
43
|
+
<p align="center">
|
|
44
|
+
<a href="https://urml.dev"><b>urml.dev</b></a>
|
|
45
|
+
</p>
|
|
46
|
+
|
|
47
|
+
---
|
|
48
|
+
|
|
49
|
+
# urml-ardupilot-runtime
|
|
50
|
+
|
|
51
|
+
**ArduPilot / MAVLink reference runtime for URML.** Flies ArduCopter (a Pixhawk-class board running ArduPilot, or ArduCopter SITL) from a validated URML program over [pymavlink](https://github.com/ArduPilot/pymavlink). No ROS 2 dependency. This is the package [RFC-0041](../../docs/rfcs/0041-ardupilot-integration.md) proposed; Copter ships first, Plane and Rover are follow-ups.
|
|
52
|
+
|
|
53
|
+
`ArduCopterAdapter` subclasses the PX4 reference adapter ([urml-px4-runtime](../px4-runtime/)). The MAVLink command set is shared; what this package adds is the firmware behaviour ArduPilot needs around those commands and PX4 does not.
|
|
54
|
+
|
|
55
|
+
## What ArduPilot needs that PX4 does not
|
|
56
|
+
|
|
57
|
+
| Concern | PX4Adapter | ArduCopterAdapter |
|
|
58
|
+
|---|---|---|
|
|
59
|
+
| Flight mode | implicit | enters `GUIDED` before take-off and setpoints, confirms on heartbeat |
|
|
60
|
+
| Arming | implicit | `MAV_CMD_COMPONENT_ARM_DISARM`, waits for the armed flag; a refusal carries the autopilot's `PreArm:` text |
|
|
61
|
+
| `COMMAND_ACK` | first ack wins | matched on `ack.command` |
|
|
62
|
+
| `move_to` | send setpoint, return | send setpoint, wait for arrival within `arrival_radius_m` |
|
|
63
|
+
| `land` / `return_to_home` | `NAV_LAND` / `NAV_RTL` commands | `LAND` / `RTL` mode entry |
|
|
64
|
+
| Global positions | none | `SET_POSITION_TARGET_GLOBAL_INT` for locations bound to WGS84 in the config |
|
|
65
|
+
| `capture` | not supported | `DO_DIGICAM_CONTROL` or a servo pulse; result carries the autopilot position at trigger time |
|
|
66
|
+
| `set_output` | not supported | ArduPilot gripper (`DO_GRIPPER`), winch (`DO_WINCH`), or servo (`DO_SET_SERVO`) |
|
|
67
|
+
| Serial | URL only | `baud` appended to `COMn` / `/dev/tty*` URLs; `pyserial` in the extra |
|
|
68
|
+
|
|
69
|
+
Nothing in this package disables `ARMING_CHECK` or any pre-arm gate. On a bench with no GPS fix the autopilot refuses GUIDED or arming, and the adapter reports that refusal as the step's `reason` and stops. That refusal is the intended bench proof.
|
|
70
|
+
|
|
71
|
+
## Method coverage
|
|
72
|
+
|
|
73
|
+
| URML primitive | MAVLink | ArduCopter behaviour |
|
|
74
|
+
|---|---|---|
|
|
75
|
+
| `take_off` | `DO_SET_MODE(GUIDED)`, `COMPONENT_ARM_DISARM`, `NAV_TAKEOFF` | climbs; success when `GLOBAL_POSITION_INT.relative_alt` reaches 95 % of target |
|
|
76
|
+
| `move_to` (named, WGS84-bound) | `SET_POSITION_TARGET_GLOBAL_INT` | flies to lat/lon at relative altitude; optional `DO_SET_ROI_LOCATION` after arrival |
|
|
77
|
+
| `move_to` (named, NED-bound, or `pose`) | `SET_POSITION_TARGET_LOCAL_NED` | flies to local-NED offset from home |
|
|
78
|
+
| `hover` | mode confirm only | GUIDED holds position; a `hover` without `over` is a confirmed no-op |
|
|
79
|
+
| `land` | `DO_SET_MODE(LAND)` | waits for auto-disarm (bounded) |
|
|
80
|
+
| `return_to_home` | `DO_SET_MODE(RTL)` | clears any ROI first |
|
|
81
|
+
| `capture` (photo) | `DO_DIGICAM_CONTROL` or `DO_SET_SERVO` pulse | image stays on the camera; payload has `camera://shot/N` and the trigger-time position |
|
|
82
|
+
| `set_output` | `DO_GRIPPER` / `DO_WINCH` / `DO_SET_SERVO` | per `output_lines` binding in the config; winch uses relative-length control (+length deliver, -length retract) because ArduCopter 4.6 rejects the `WINCH_DELIVER` / `WINCH_RETRACT` actions |
|
|
83
|
+
| `measure` (distance, voltage), `wait_for`, `report`, `wait` | inherited from PX4Adapter | |
|
|
84
|
+
| `dock`, `grasp`, `release`, `detect`, `speak`, `listen`, video capture | not supported | documented `not_supported` result, never raised |
|
|
85
|
+
|
|
86
|
+
## Install
|
|
87
|
+
|
|
88
|
+
```bash
|
|
89
|
+
pip install -e reference/ardupilot-runtime[ardupilot]
|
|
90
|
+
```
|
|
91
|
+
|
|
92
|
+
The extra installs `pymavlink` and `pyserial`. Without it the module imports but constructing the adapter raises a clear install hint.
|
|
93
|
+
|
|
94
|
+
## Bring-up
|
|
95
|
+
|
|
96
|
+
Read-only probe. Sends no mode, arm, or motion command.
|
|
97
|
+
|
|
98
|
+
```bash
|
|
99
|
+
python -m urml_ardupilot_runtime.probe COM5
|
|
100
|
+
```
|
|
101
|
+
|
|
102
|
+
Prints the autopilot type, vehicle type, MAVLink system id, armed state, mode, firmware version, battery voltage, GPS fix, and any recent STATUSTEXT. A Pixhawk on USB is usually `COM<n>` on Windows and `/dev/ttyACM0` on Linux; ArduCopter SITL is `udp:127.0.0.1:14550`.
|
|
103
|
+
|
|
104
|
+
## Use
|
|
105
|
+
|
|
106
|
+
```bash
|
|
107
|
+
urml execute program.urml.yaml -m manifest.yaml --profile drone --no-policy \
|
|
108
|
+
--adapter ardupilot --adapter-config ardupilot_adapter.yaml
|
|
109
|
+
```
|
|
110
|
+
|
|
111
|
+
A minimal `ardupilot_adapter.yaml`:
|
|
112
|
+
|
|
113
|
+
```yaml
|
|
114
|
+
connection_url: "COM5"
|
|
115
|
+
baud: 115200
|
|
116
|
+
|
|
117
|
+
location_to_pose: # local metres from the launch point
|
|
118
|
+
bench_north: { north: 5.0, east: 0.0, alt: 3.0 }
|
|
119
|
+
home: { north: 0.0, east: 0.0, alt: 0.0 }
|
|
120
|
+
|
|
121
|
+
location_to_global: # WGS84, wins over location_to_pose for the same name
|
|
122
|
+
site_p1: { lat: 32.0853, lon: 34.7818, alt_agl: 100.0, look_at: { lat: 32.0850, lon: 34.7815 } }
|
|
123
|
+
|
|
124
|
+
camera:
|
|
125
|
+
kind: digicam # or `servo` with `channel`
|
|
126
|
+
|
|
127
|
+
output_lines:
|
|
128
|
+
payload_latch: { kind: gripper, instance: 1 }
|
|
129
|
+
winch: { kind: winch, instance: 1, deliver_length_m: 15.0, rate_m_s: 0.5 }
|
|
130
|
+
```
|
|
131
|
+
|
|
132
|
+
`location_to_global` is written by [`tools/scripts/geocode_locations.py`](../../tools/scripts/geocode_locations.py) at configuration time. The runtime never geocodes and never touches the network.
|
|
133
|
+
|
|
134
|
+
From Python:
|
|
135
|
+
|
|
136
|
+
```python
|
|
137
|
+
from urml_ardupilot_runtime import ArduCopterAdapter, load_ardupilot_config
|
|
138
|
+
from urml_ros2_runtime import URMLRuntime
|
|
139
|
+
|
|
140
|
+
with ArduCopterAdapter(load_ardupilot_config("ardupilot_adapter.yaml")) as adapter:
|
|
141
|
+
runtime = URMLRuntime(adapter)
|
|
142
|
+
result = runtime.execute(program, manifest, envelope, profiles=("drone",))
|
|
143
|
+
```
|
|
144
|
+
|
|
145
|
+
`CompositeAdapter` from the PX4 runtime accepts an `ArduCopterAdapter` as its `flight` backend unchanged.
|
|
146
|
+
|
|
147
|
+
## Tests
|
|
148
|
+
|
|
149
|
+
Three tiers, mirroring the PX4 runtime:
|
|
150
|
+
|
|
151
|
+
- `tests/test_arducopter_adapter.py`: hermetic, a fake `pymavlink` is injected; runs everywhere.
|
|
152
|
+
- `tests/integration/test_arducopter_live.py`: gated on `URML_ARDUPILOT_INTEGRATION=1`; real pymavlink, no autopilot contacted.
|
|
153
|
+
- `tests/integration/test_arducopter_bench.py`: gated on `URML_ARDUPILOT_BENCH=<port>`; a real board, props off. Asserts the identity probe, a successful battery read through the runtime, and that a take-off is refused cleanly by the autopilot's own pre-arm checks.
|
|
154
|
+
- `tests/integration/test_arducopter_sitl_e2e.py`: gated on `URML_ARDUPILOT_SITL=1`; flies the `drone/flight_only_positive` conformance fixture against ArduCopter SITL.
|
|
155
|
+
|
|
156
|
+
## Status
|
|
157
|
+
|
|
158
|
+
Bench link verified on hardware (Pixhawk, ArduCopter 4.6.3, USB). The SITL e2e (flight-only fixture plus both flight-test examples) is green against ArduCopter SITL built from `Copter-4.6.3`, run locally on 2026-08-29. No physical flight is claimed anywhere in this repository; see [`docs/demos/sentence-to-pixhawk.md`](../../docs/demos/sentence-to-pixhawk.md) for the bench runbook and the two flight-test runbooks that gate a field run on a green SITL pass.
|
|
159
|
+
|
|
160
|
+
## License
|
|
161
|
+
|
|
162
|
+
Apache 2.0. The adapter talks to ArduPilot firmware (GPLv3) over MAVLink; it links `pymavlink` (LGPLv3) as a normal Python import and never links the firmware.
|
|
@@ -0,0 +1,8 @@
|
|
|
1
|
+
urml_ardupilot_runtime/__init__.py,sha256=HTpMd5e57cHkIyJ3Hmwkd-_YSM_s1uqgxqOzeJkdZE8,1365
|
|
2
|
+
urml_ardupilot_runtime/_version.py,sha256=r-XED1Lc4KMmJaU0ulYpVP8MzyjFyL7JKbg6-2IlTk0,113
|
|
3
|
+
urml_ardupilot_runtime/adapter.py,sha256=sUAZdjtpS9tojjO3UE9j12_zzFSKBMB7_9SmYUaQcrY,31337
|
|
4
|
+
urml_ardupilot_runtime/config.py,sha256=iKblQyeQtQq5MPjBFRbeYVw6-bJS_X0CUQeZ4jg0Yno,6157
|
|
5
|
+
urml_ardupilot_runtime/probe.py,sha256=9fIBCTsF-rbWrOdMzZIbVY7v8S-9Dq8juC7T06esuks,1558
|
|
6
|
+
urml_ardupilot_runtime-0.4.0.dist-info/METADATA,sha256=DcFrextRLcZrLn-DzfmVjySin6bavQWsEtqmsVZaRt8,8476
|
|
7
|
+
urml_ardupilot_runtime-0.4.0.dist-info/WHEEL,sha256=zOwg4jB6zX2kU910N-cMawjivD6tO8NEWvE12je1bVk,87
|
|
8
|
+
urml_ardupilot_runtime-0.4.0.dist-info/RECORD,,
|