Skip to content

Commit aef759f

Browse files
committed
examples: add two MavlinkDirect examples
This is to demonstrate the new MavlinkDirect functionality.
1 parent 30e2023 commit aef759f

File tree

2 files changed

+88
-0
lines changed

2 files changed

+88
-0
lines changed

examples/mavlink_direct_receive.py

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#!/usr/bin/env python3
2+
3+
import asyncio
4+
from mavsdk import System
5+
import json
6+
7+
8+
async def print_messages():
9+
drone = System()
10+
print("Waiting for drone to connect...")
11+
await drone.connect(system_address="udpin://0.0.0.0:14540")
12+
13+
async for state in drone.core.connection_state():
14+
if state.is_connected:
15+
print(f"-- Connected to drone!")
16+
break
17+
18+
async for message in drone.mavlink_direct.message("ATTITUDE"):
19+
fields = json.loads(message.fields_json)
20+
21+
# Format as JSON again for the reader but this time with indenting.
22+
fields_formatted = json.dumps(fields, indent=4)
23+
24+
print(f"{message.message_name} from {message.system_id}/{message.component_id}:")
25+
print("----")
26+
print(f"{fields_formatted}")
27+
print("----")
28+
29+
30+
if __name__ == "__main__":
31+
asyncio.run(print_messages())

examples/mavlink_direct_send.py

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
#!/usr/bin/env python3
2+
3+
import asyncio
4+
from mavsdk import System, mavlink_direct
5+
import json
6+
import random
7+
8+
9+
async def send_airspeed():
10+
drone = System()
11+
print("Waiting for drone to connect...")
12+
await drone.connect(system_address="udpin://0.0.0.0:14540")
13+
14+
async for state in drone.core.connection_state():
15+
if state.is_connected:
16+
print(f"-- Connected to drone!")
17+
break
18+
19+
await drone.mavlink_direct.load_custom_xml("""
20+
<mavlink>
21+
<messages>
22+
<message id="295" name="AIRSPEED">
23+
<description>Airspeed information from a sensor.</description>
24+
<field type="uint8_t" name="id" instance="true">Sensor ID.</field>
25+
<field type="float" name="airspeed" units="m/s">Calibrated airspeed (CAS).</field>
26+
<field type="int16_t" name="temperature" units="cdegC">Temperature. INT16_MAX for value unknown/not supplied.</field>
27+
<field type="float" name="raw_press" units="hPa">Raw differential pressure. NaN for value unknown/not supplied.</field>
28+
<field type="uint8_t" name="flags" enum="AIRSPEED_SENSOR_FLAGS">Airspeed sensor flags.</field>
29+
</message>
30+
</messages>
31+
</mavlink>
32+
""")
33+
34+
for i in range(20):
35+
print(f"Sending airspeed {i}...")
36+
fields = {
37+
"id": 0,
38+
"airspeed": random.random() * 10,
39+
"temperature": 20 * 100,
40+
"raw_press": 1010,
41+
"flags": 0,
42+
}
43+
44+
message = mavlink_direct.MavlinkMessage(
45+
message_name="AIRSPEED",
46+
system_id=245,
47+
component_id=25,
48+
target_system_id=0,
49+
target_component_id=0,
50+
fields_json=json.dumps(fields),
51+
)
52+
await drone.mavlink_direct.send_message(message)
53+
await asyncio.sleep(0.1)
54+
55+
56+
if __name__ == "__main__":
57+
asyncio.run(send_airspeed())

0 commit comments

Comments
 (0)