Skip to content

Ardupilot mavlink with pymavlink

SITL

1
2
# ver 4.6.1
./arducopter --model copter --defaults params/basic.param -I0
basic.param 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
FRAME_CLASS 1
FRAME_TYPE 1

SIM_GPS_DISABLE 0
AHRS_EKF_TYPE 2
INS_ACCOFFS_X 0.001
INS_ACCOFFS_Y 0.001
INS_ACCOFFS_Z 0.001
INS_ACCSCAL_X 1.001
INS_ACCSCAL_Y 1.001
INS_ACCSCAL_Z 1.001
INS_ACC2OFFS_X 0.001
INS_ACC2OFFS_Y 0.001
INS_ACC2OFFS_Z 0.001
INS_ACC2SCAL_X 1.001
INS_ACC2SCAL_Y 1.001
INS_ACC2SCAL_Z 1.001
INS_GYR_CAL 0

RNGFND1_TYPE 100        # 100 = simulated
RNGFND1_ORIENT 25     # 25 = Downward (typical for landing)
RNGFND1_MIN_CM 20     # 20 cm minimum range
RNGFND1_MAX_CM 400    # 400 cm max range
SIM_SONAR_SCALE 100   # Scaling for simulated range data
RNGFND1_SCALING 1     # Optional: default scaling

Hello

Set message interval

Send message to GCS

Send other location

Hello

Connect to SITL and print out every mavlink received typically only heartbeat and timesync received on no one request other messages

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
from pymavlink import mavutil
# cast helpers
from typing import cast
from pymavlink.dialects.v20.common import MAVLink_message
from pymavlink.mavutil import mavtcp


conn = mavutil.mavlink_connection('tcp:127.0.0.1:5760')
conn = cast(mavtcp, conn)
# Make sure the connection is valid
conn.wait_heartbeat()

# Get some information !
while True:
    try:
        msg = conn.recv_match(blocking=True)
        msg = cast(MAVLink_message, msg)
        if msg:
            print(msg.get_type(), msg.to_dict())
    except Exception as _:
        pass

Send command long to set message interval

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
from pymavlink import mavutil
# cast helpers
from typing import cast
from pymavlink.dialects.v20.common import MAVLink_message
from pymavlink.dialects.v20.ardupilotmega import MAVLink
from pymavlink.dialects.v20 import common
from pymavlink.mavutil import mavtcp

conn = mavutil.mavlink_connection('tcp:127.0.0.1:5760')
conn = cast(mavtcp, conn)

def request_message_interval(conn: mavtcp, message_id: int, frequency_hz: float):
    """
    Request MAVLink message in a desired frequency,
    documentation for SET_MESSAGE_INTERVAL:
        https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL

    Args:
        message_id (int): MAVLink message ID
        frequency_hz (float): Desired frequency in Hz
    """


    conn.mav.command_long_send(
        conn.target_system, conn.target_component,
        common.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
        message_id, # The MAVLink message ID
        1e6 / frequency_hz, # The interval between two messages in microseconds. Set to -1 to disable and 0 to request default rate.
        0, 0, 0, 0, # Unused parameters
        0, # Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast.
    )


# Make sure the connection is valid
conn.wait_heartbeat()

request_message_interval(conn, common.MAVLINK_MSG_ID_GPS_RAW_INT, 1)

# Get some information !
while True:
    try:
        msg = conn.recv_match(blocking=True)
        msg = cast(MAVLink_message, msg)
        if msg:
            print(msg.get_type(), msg.to_dict())
    except Exception as _:
        pass

Send message to gcs

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
from pymavlink import mavutil
import time
from pymavlink.dialects.v20 import common

# Create the connection to the top-side computer as companion computer/autopilot
master = mavutil.mavlink_connection(
    "udpout:localhost:14550", source_system=1, source_component=190
)

while True:

    master.mav.statustext_send(
        common.MAV_SEVERITY_INFO, "QGC will read this".encode()
    )
    time.sleep(1)

udpout

Send mavlink stream over udp

Send other location using ADSB

Send other location information using ADSB check mavlink message ADSB_VEHICLE (246) 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
from pymavlink import mavutil
import time
from pymavlink.dialects.v20 import common


conn = mavutil.mavlink_connection("udpout:127.0.0.1:14550", source_system=0)

ICAO_ADDR = 0xABCDEF  # must be unique per vehicle

while True:
    conn.mav.adsb_vehicle_send(
        ICAO_ADDR,                 # ICAO address (unique ID)
        int(-35.362160 *1e7),        # lat (deg * 1e7)
        int(149.164975 * 1e7),        # lon (deg * 1e7)
        common.ADSB_ALTITUDE_TYPE_PRESSURE_QNH,
        100 * 1000,                # altitude (mm)
        0,                          # heading (cdeg)
        0,                          # horizontal velocity (cm/s)
        0,                          # vertical velocity (cm/s)
        b"OTHER_VEHICLE",            # callsign
        mavutil.mavlink.ADSB_EMITTER_TYPE_UAV,                          # emitter_type
        0,                          # tslc
        mavutil.mavlink.ADSB_FLAGS_VALID_COORDS,                           # flags
        0
    )
    time.sleep(1)

alt text