receiving and sending mavlink messages using pymavlink library

Question

I have created a proxy between QGC(Ground Control Station) and vehicle in Python. Here is the code:

gcs_conn = mavutil.mavlink_connection('tcpin:localhost:15795')
gcs_conn.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %(gcs_conn.target_system, gcs_conn.target_system))
vehicle = mavutil.mavlink_connection('tcp:localhost:5760')
vehicle.wait_heartbeat() # recieving heartbeat from the vehicle
print("Heartbeat from system (system %u component %u)" %(vehicle.target_system, vehicle.target_system))
while True:
     gcs_msg = gcs_conn.recv_match()
     if gcs_msg == None:
         pass
     else:
         vehicle.mav.send(gcs_msg)
         print(gcs_msg)

     vcl_msg = vehicle.recv_match()
     if vcl_msg == None:
         pass
     else:
         gcs_conn.mav.send(vcl_msg)
         print(vcl_msg)

I need to receive the messages from the QGC and then forward them to the vehicle and also receive the messages from the vehicle and forward them to the QGC. When I run the code I get this error.

is there any one who can help me?

Answer 1

For forwarding MAVLink successfully a few things need to happen. I'm assuming you need a usable connection to a GCS, like QGroundControl or MissionPlanner. I use QGC, and my design has basic testing with it.

Note that this is written with Python3. This snippet is not tested, but I have a (much more complex) version tested and working.

from pymavlink import mavutil
import time


# PyMAVLink has an issue that received messages which contain strings
# cannot be resent, because they become Python strings (not bytestrings)
# This converts those messages so your code doesn't crash when
# you try to send the message again.
def fixMAVLinkMessageForForward(msg):
    msg_type = msg.get_type()
    if msg_type in ('PARAM_VALUE', 'PARAM_REQUEST_READ', 'PARAM_SET'):
        if type(msg.param_id) == str:
            msg.param_id = msg.param_id.encode()
    elif msg_type == 'STATUSTEXT':
        if type(msg.text) == str:
            msg.text = msg.text.encode()
    return msg


# Modified from the snippet in your question
# UDP will work just as well or better
gcs_conn = mavutil.mavlink_connection('tcp:localhost:15795', input=False)
gcs_conn.wait_heartbeat()
print(f'Heartbeat from system (system {gcs_conn.target_system} component {gcs_conn.target_system})')

vehicle = mavutil.mavlink_connection('tcp:localhost:5760')
vehicle.wait_heartbeat()
print(f'Heartbeat from system (system {vehicle.target_system} component {vehicle.target_system})')

while True:
    # Don't block for a GCS message - we have messages
    # from the vehicle to get too
    gcs_msg = gcs_conn.recv_match(blocking=False)
    if gcs_msg is None:
        pass
    elif gcs_msg.get_type() != 'BAD_DATA':
        # We now have a message we want to forward. Now we need to
        # make it safe to send
        gcs_msg = fixMAVLinkMessageForForward(gcs_msg)

        # Finally, in order to forward this, we actually need to
        # hack PyMAVLink so the message has the right source
        # information attached.
        vehicle.mav.srcSystem = gcs_msg.get_srcSystem()
        vehicle.mav.srcComponent = gcs_msg.get_srcComponent()

        # Only now is it safe to send the message
        vehicle.mav.send(gcs_msg)
        print(gcs_msg)

    vcl_msg = vehicle.recv_match(blocking=False)
    if vcl_msg is None:
        pass
    elif vcl_msg.get_type() != 'BAD_DATA':
        # We now have a message we want to forward. Now we need to
        # make it safe to send
        vcl_msg = fixMAVLinkMessageForForward(vcl_msg)

        # Finally, in order to forward this, we actually need to
        # hack PyMAVLink so the message has the right source
        # information attached.
        gcs_conn.mav.srcSystem = vcl_msg.get_srcSystem()
        gcs_conn.mav.srcComponent = vcl_msg.get_srcComponent()

        gcs_conn.mav.send(vcl_msg)
        print(vcl_msg)

    # Don't abuse the CPU by running the loop at maximum speed
    time.sleep(0.001)

Notes

Make sure your loop isn't being blocked

The loop must quickly check if a message is available from one connection or the other, instead of waiting for a message to be available from a single connection. Otherwise a message on the other connection will not go through until the blocking connection has a message.

Check message validity

Check that you actually got a valid message, as opposed to a BAD_DATA message. Attempting to send BAD_DATA will crash

Make sure the recipient gets the correct information about the sender

By default PyMAVLink, when sending a message, will encode YOUR system and component IDs (usually left at zero), instead of the IDs from the message. A GCS receiving this may be confused (ie, QGC) and not properly connect to the vehicle (despite showing the messages in MAVLink inspector).

This is fixed by hacking PyMAVLink such that your system and component IDs match the forwarded message. This can be revered after the message is sent if necessary. See the example to see how I did it.

Loop update rate

It's important that the update rate is fast enough to handle high traffic conditions (especially, say, for downloading params), but it shouldn't peg out the CPU either. I find that a 1000hz update rate works well enough.

Answer 2

If you print your message before sending you'll notice it always fails when you try to send a BAD_DATA message type.

So this should fix it (same for vcl_msg):

if gcs_msg and gcs_msg.get_type() != 'BAD_DATA':
    vehicle.mav.send(gcs_msg)

PD: I noticed that you don't specify tcp as input or output, it defaults to input. Than means both connections are inputs. I recommend setting up the GCS connection as output:

gcs_conn = mavutil.mavlink_connection('tcp:localhost:15795', input=False)

https://mavlink.io/en/mavgen_python/#connection_string