1

我在 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。当我运行代码时，我得到了这个错误。

有谁能帮助我吗？

4

2 回答 2

0

如果您在发送之前打印您的消息，您会发现当您尝试发送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

于 2019-02-18T16:52:15.887 回答
0

为了成功转发 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 挂钩。我发现 1000hz 的更新率足够好。

于 2021-09-22T18:34:33.227 回答