Skip to content

Commit bf24712

Browse files
committed
Update messaging
1 parent 1567df9 commit bf24712

File tree

10 files changed

+434
-233
lines changed

10 files changed

+434
-233
lines changed

docs/source/conf.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
project = "MAVez"
1212
copyright = "2025, The Pennsylvania State University Unmanned Aerial Systems Club"
1313
author = "The Pennsylvania State University Unmanned Aerial Systems Club"
14-
version = "3.8.0"
14+
version = "3.9.0"
1515

1616
# -- General configuration ---------------------------------------------------
1717
# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration

docs/source/enums.rst

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@ Enums
33

44
MAV Result
55
-----------
6+
Note, not all MAV Results are supported by ardupilot. Read the `ardupilot documentation <https://ardupilot.org/copter/docs/ArduCopter_MAVLink_Messages.html>`_
67

78
.. automodule:: MAVez.enums.mav_result
89
:members:

pyproject.toml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
44

55
[project]
66
name='MAVez'
7-
version='3.8.3'
7+
version='3.9.0'
88
description='An intuitive MAVLink library for ArduPilot via pymavlink'
99
authors=[
1010
{name='The Pennsylvania State University Unmanned Aerial Systems Club', email='psuuasofficial@gmail.com'}

src/MAVez/controller.py

Lines changed: 283 additions & 213 deletions
Large diffs are not rendered by default.

src/MAVez/coordinate.py

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
# coordinate.py
2-
# version: 1.2.1
2+
# version: 1.3.0
33
# Authors: Theodore Tasman
44
# Creation Date: 2025-01-30
5-
# Last Modified: 2025-11-14
5+
# Last Modified: 2026-03-26
66
# Organization: PSU UAS
77

88
"""
@@ -66,13 +66,13 @@ def __str__(self):
6666

6767
__repr__ = __str__
6868

69-
def offset_coordinate(self, offset: float | int, heading: float | int) -> "Coordinate":
69+
def offset_coordinate(self, offset: float | int, bearing: float | int) -> "Coordinate":
7070
"""
7171
Offset the coordinate by a given distance and heading.
7272
7373
Args:
7474
offset (float): The distance to offset in meters.
75-
heading (float): The heading in degrees.
75+
bearing (float): The bearing in degrees.
7676
7777
Returns:
7878
Coordinate: A new Coordinate object with the offset applied.
@@ -81,11 +81,11 @@ def offset_coordinate(self, offset: float | int, heading: float | int) -> "Coord
8181
lat, lon = self.normalize()
8282

8383
# convert heading to radians
84-
heading = math.radians(heading)
84+
bearing = math.radians(bearing)
8585

8686
# calculate the offset in latitude and longitude
87-
lat_offset = offset * math.cos(heading)
88-
lon_offset = offset * math.sin(heading)
87+
lat_offset = offset * math.cos(bearing)
88+
lon_offset = offset * math.sin(bearing)
8989

9090
# convert the offset to degrees
9191
lat_offset = lat_offset / METERS_PER_DEGREE

src/MAVez/enums/mav_message.py

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,9 +127,57 @@ class MAVMessage(Enum):
127127
CONTROL_SYSTEM_STATE = 146
128128
BATTERY_STATUS = 147
129129
LANDING_TARGET = 149
130+
SENSOR_OFFSETS = 150
131+
SET_MAG_OFFSETS = 151
132+
MEMINFO = 152
133+
AP_ADC = 153
134+
DIGICAM_CONFIGURE = 154
135+
DIGICAM_CONTROL = 155
136+
MOUNT_CONFIGURE = 156
137+
MOUNT_CONTROL = 157
138+
MOUNT_STATUS = 158
139+
FENCE_POINT = 160
140+
FENCE_FETCH_POINT = 161
130141
FENCE_STATUS = 162
142+
AHRS = 163
143+
SIMSTATE = 164
144+
HWSTATUS = 165
145+
RADIO = 166
146+
LIMITS_STATUS = 167
147+
WIND = 168
148+
DATA16 = 169
149+
DATA32 = 170
150+
DATA64 = 171
151+
DATA96 = 172
152+
RANGEFINDER = 173
153+
AIRSPEED_AUTOCAL = 174
154+
RALLY_POINT = 175
155+
RALLY_FETCH_POINT = 176
156+
COMPASSMOT_STATUS = 177
157+
AHRS2 = 178
158+
CAMERA_STATUS = 179
159+
CAMERA_FEEDBACK = 180
160+
BATTERY2 = 181
161+
AHRS3 = 182
162+
AUTOPILOT_VERSION_REQUEST = 183
163+
REMOTE_LOG_DATA_BLOCK = 184
164+
REMOTE_LOG_BLOCK_STATUS = 185
165+
LED_CONTROL = 186
166+
MAG_CAL_PROGRESS = 191
131167
MAG_CAL_REPORT = 192
168+
EKF_STATUS_REPORT = 193
169+
PID_TUNING = 194
170+
DEEPSTALL = 195
171+
GIMBAL_REPORT = 200
172+
GIMBAL_CONTROL = 201
173+
GIMBAL_TORQUE_CMD_REPORT = 214
174+
GOPRO_HEARTBEAT = 215
175+
GOPRO_GET_REQUEST = 216
176+
GOPRO_GET_RESPONSE = 217
177+
GOPRO_SET_REQUEST = 218
178+
GOPRO_SET_RESPONSE = 219
132179
EFI_STATUS = 225
180+
RPM = 226
133181
ESTIMATOR_STATUS = 230
134182
WIND_COV = 231
135183
GPS_INPUT = 232
@@ -232,6 +280,39 @@ class MAVMessage(Enum):
232280
ILLUMINATOR_STATUS = 440
233281
WHEEL_DISTANCE = 9000
234282
WINCH_STATUS = 9005
283+
UAVIONIX_ADSB_OUT_CFG = 10001
284+
UAVIONIX_ADSB_OUT_DYNAMIC = 10002
285+
UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT = 10003
286+
UAVIONIX_ADSB_OUT_CFG_REGISTRATION = 10004
287+
UAVIONIX_ADSB_OUT_CFG_FLIGHTID = 10005
288+
UAVIONIX_ADSB_GET = 10006
289+
UAVIONIX_ADSB_OUT_CONTROL = 10007
290+
UAVIONIX_ADSB_OUT_STATUS = 10008
291+
DEVICE_OP_READ = 11000
292+
DEVICE_OP_READ_REPLY = 11001
293+
DEVICE_OP_WRITE = 11002
294+
DEVICE_OP_WRITE_REPLY = 11003
295+
SECURE_COMMAND = 11004
296+
SECURE_COMMAND_REPLY = 11005
297+
ADAP_TUNING = 11010
298+
VISION_POSITION_DELTA = 11011
299+
AOA_SSA = 11020
300+
ESC_TELEMETRY_1_TO_4 = 11030
301+
ESC_TELEMETRY_5_TO_8 = 11031
302+
ESC_TELEMETRY_9_TO_12 = 11032
303+
OSD_PARAM_CONFIG = 11033
304+
OSD_PARAM_CONFIG_REPLY = 11034
305+
OSD_PARAM_SHOW_CONFIG = 11035
306+
OSD_PARAM_SHOW_CONFIG_REPLY = 11036
307+
OBSTACLE_DISTANCE_3D = 11037
308+
WATER_DEPTH = 11038
309+
MCU_STATUS = 11039
310+
ESC_TELEMETRY_13_TO_16 = 11040
311+
ESC_TELEMETRY_17_TO_20 = 11041
312+
ESC_TELEMETRY_21_TO_24 = 11042
313+
ESC_TELEMETRY_25_TO_28 = 11043
314+
ESC_TELEMETRY_29_TO_32 = 11044
315+
NAMED_VALUE_STRING = 11060
235316
OPEN_DRONE_ID_BASIC_ID = 12900
236317
OPEN_DRONE_ID_LOCATION = 12901
237318
OPEN_DRONE_ID_AUTHENTICATION = 12902

src/MAVez/flight_controller.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
# flight_controller.py
2-
# version: 3.1.0
2+
# version: 3.1.1
33
# Original Author: Theodore Tasman
44
# Creation Date: 2025-01-30
5-
# Last Modified: 2025-11-14
5+
# Last Modified: 2026-03-26
66
# Organization: PSU UAS
77

88
"""
@@ -254,7 +254,7 @@ async def wait_for_landing(self, timeout=60) -> int:
254254

255255
# start receiving landing status
256256
response = await self.set_message_interval(
257-
message_type=MAVMessage.EXTENDED_SYS_STATE, interval=100000 # 1 second
257+
message_type=MAVMessage.EXTENDED_SYS_STATE, interval_us=100000 # 1 second
258258
)
259259
if response:
260260
self.logger.critical("[Flight] Failed waiting for landing.")
@@ -300,8 +300,8 @@ async def jump_to_next_mission_item(self) -> int:
300300
"""
301301

302302
self.logger.debug("[Flight] Waiting for current mission index")
303-
# wait for the current mission target to be received (should be broadcast by default)
304-
response = await self.receive_current_mission_index()
303+
# get the latest mission index
304+
response = await self.receive_current_mission_index(seq=self.__message_seq_by_type["MISSION_CURRENT"])
305305
if response == self.TIMEOUT_ERROR:
306306
return response
307307

src/MAVez/mission.py

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
# mission.py
2-
# version: 3.1.0
2+
# version: 3.1.1
33
# Author: Theodore Tasman
44
# Creation Date: 2025-01-30
55
# Last Modified: 2026-03-21
@@ -262,13 +262,16 @@ async def send_mission(self, reset: bool = True) -> int:
262262
"""
263263

264264
# send mission count
265+
next_mission_request_seq = self.controller.get_message_seq("MISSION_REQUEST") + 1
266+
next_mission_ack_seq = self.controller.get_message_seq("MISSION_ACK") + 1
265267
self.controller.send_mission_count(len(self.mission_items), self.type)
266268

267269
# continuous loop to await mission request, or timeout
268-
start_time = time.time()
270+
start_time = time.monotonic()
269271
while True:
270272
# await mission request
271-
seq = await self.controller.receive_mission_request()
273+
seq = await self.controller.receive_mission_request(next_mission_request_seq)
274+
next_mission_request_seq += 1
272275

273276
# verify seq is not an error
274277
if seq == self.controller.TIMEOUT_ERROR:
@@ -282,15 +285,15 @@ async def send_mission(self, reset: bool = True) -> int:
282285
break
283286

284287
# check for timeout
285-
if time.time() - start_time > self.MISSION_SEND_TIMEOUT:
288+
if time.monotonic() - start_time > self.MISSION_SEND_TIMEOUT:
286289
if self.controller.logger:
287290
self.controller.logger.error(
288291
f"[Mission] Mission send timeout after {self.MISSION_SEND_TIMEOUT} seconds"
289292
)
290293
return self.TIMEOUT_ERROR
291294

292295
# after sending all mission items, wait for mission acknowledgement
293-
response = await self.controller.receive_mission_ack() # returns 0 if successful
296+
response = await self.controller.receive_mission_ack(next_mission_ack_seq) # returns 0 if successful
294297
if response:
295298
return response # propagate error code
296299

@@ -310,10 +313,11 @@ async def clear_mission(self) -> int:
310313
int: 0 if the mission was cleared successfully, or an error code if there was an error.
311314
"""
312315
# send mission clear all
316+
next_mission_ack_seq = self.controller.get_message_seq("MISSION_ACK") + 1
313317
self.controller.send_clear_mission()
314318

315319
# await mission ack confirming mission was cleared
316-
response = await self.controller.receive_mission_ack()
320+
response = await self.controller.receive_mission_ack(next_mission_ack_seq)
317321
if response:
318322
if self.controller.logger:
319323
self.controller.logger.critical("[Mission] Could not clear mission.")

test_enum.py

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
from enum import Enum
2+
3+
4+
class MavMessageType(Enum):
5+
"""
6+
Enum for MAVLink message types.
7+
"""
8+
9+
UAVIONIX_ADSB_OUT_CFG = 10001
10+
UAVIONIX_ADSB_OUT_DYNAMIC = 10002
11+
UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT = 10003
12+
UAVIONIX_ADSB_OUT_CFG_REGISTRATION = 10004
13+
UAVIONIX_ADSB_OUT_CFG_FLIGHTID = 10005
14+
UAVIONIX_ADSB_GET = 10006
15+
UAVIONIX_ADSB_OUT_CONTROL = 10007
16+
UAVIONIX_ADSB_OUT_STATUS = 10008

testing/send_receive.py

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
from pathlib import Path
2+
3+
from MAVez import Controller, safe_logger, Mission, MAVMessage
4+
import logging
5+
import asyncio
6+
7+
async def main():
8+
logger = safe_logger.configure_logging(logging.INFO)
9+
con = Controller(
10+
logger=logger,
11+
timesync=True
12+
)
13+
await con.start()
14+
test_mission = Mission.from_file(
15+
controller=con,
16+
filepath=Path("./examples/sample_missions/sample1.txt")
17+
)
18+
await con.arm()
19+
await con.set_mode("AUTO")
20+
await test_mission.send_mission() if test_mission else print("oops")
21+
await asyncio.sleep(0.5)
22+
await con.set_message_interval(MAVMessage.WIND, int(0.5*1e6))
23+
while True:
24+
data = await con.receive_wind(normalize_direction=True)
25+
print(data)
26+
27+
28+
if __name__ == "__main__":
29+
asyncio.run(main())

0 commit comments

Comments
 (0)