使用pymavlink库接收和发送mavlink消息

Question

我用Python在QGC(地面控制站)和车辆之间创建了一个代理。代码如下：

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)

我需要从QGC接收消息，然后将它们转发到车辆，还需要从车辆接收消息并将它们转发到QGC。当我运行代码时，我得到了这个error。

有能帮我的人吗？

Answer 1

如果您在发送之前打印消息，您会注意到，当您尝试发送BAD_DATA消息类型时，它总是失败。

所以这应该可以解决这个问题(vcl_msg也是如此)：

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

PD:我注意到您没有指定tcp作为输入或输出，它缺省为input。则表示两个连接都是输入。我建议将GCS连接设置为输出：

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

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

Answer 2

为了成功地转发MAVLink，需要做一些事情。我假设你需要一个可用的到GCS的连接，比如QGroundControl或MissionPlanner。我使用QGC，并且我的设计使用它进行了基本的测试。

请注意，这是用Python3编写的。这段代码没有经过测试，但我有一个(复杂得多的)版本经过测试并正常工作。

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)

备注

确保你的循环没有被阻塞

循环必须快速检查消息是否从一个连接或另一个连接可用，而不是等待消息从单个连接可用。否则，在阻塞连接有消息之前，另一个连接上的消息将不会通过。

检查消息有效性

检查您是否实际收到了有效的消息，而不是BAD_DATA消息。尝试发送BAD_DATA将会崩溃

确保收件人获得有关发件人的正确信息

默认情况下，在发送消息时，PyMAVLink将对系统和组件ID(通常保留为零)进行编码，而不是对消息中的ID进行编码。收到此消息的GCS可能会感到困惑(即QGC)，并且不能正确连接到车辆(尽管在MAVLink检查器中显示消息)。

这可以通过黑客攻击PyMAVLink来修复，使您的系统和组件ID与转发的消息相匹配。如有必要，可以在发送消息后恢复此操作。查看示例以了解我是如何做到这一点的。

循环更新率

重要的是，更新速度足够快，足以处理高流量条件(特别是下载参数)，但它也不应该占用CPU。我发现1000的更新率已经足够好了。