MAVLink通讯

MAVLink 是一个针对无人机生态系统设计的非常轻量化的消息传递协议。

PX4 使用 MAVLink 实现与 QGroundControl （或者其它地面站软件）的通讯交流，同时也将其用于整合飞控板与飞控板之外的无人机部件：伴随计算机、支持 MAVLink 的摄像头等。

该协议定义了许多用于交换数据的标准消息和微型服务（microservices）（PX4 中用到了许多消息/服务，但不是全部）。

本教程介绍了如何为你自己新 "自定义" 的报文添加 PX4 支持。

创建自定义 MAVLink 消息

MAVLink开发者指南解释了如何定义新消息，并将它们构建到新的特定编程库中:

Your message needs to be generated as a C-library for MAVLink 2. 一旦你安装了MAVLink，你可以在命令行中使用以下命令:

python -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=generated/include/mavlink/v2.0 message_definitions/v1.0/custom_messages.xml

如果只是自己使用/测试，那么你只需要直接将生成的头文件拷贝到 Firmware/mavlink/include/mavlink/v2.0 文件夹下。

如果想让其他人可以更简单地测试你的修改，更好的做法则是将你生成的头文件加入 https://github.com/mavlink/c_library_v2 的一个分支中， PX4 developers can then update the submodule to your fork in the PX4-Autopilot repo before building.

发送自定义MAVLink消息

Step1. 首先，在mavlink_messages.cpp中添加自定义MAVLink消息和uORB消息的头文件：

Step2. 然后，在mavlink_messages.cpp中新建一个消息流的类：

#include <uORB/topics/ca_trajectory.h>
#include <v2.0/custom_messages/mavlink.h>

Step3. 接下来，在mavlink_messages.cpp的文件末尾，将这个消息流的类追加到treams_list。

class MavlinkStreamCaTrajectory : public MavlinkStream
{
public:
    const char *get_name() const
    {
        return MavlinkStreamCaTrajectory::get_name_static();
    }
    static const char *get_name_static()
    {
        return "CA_TRAJECTORY";
    }
    uint16_t get_id_static()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY;
    }
    uint16_t get_id()
    {
        return get_id_static();
    }
    static MavlinkStream *new_instance(Mavlink *mavlink)
    {
        return new MavlinkStreamCaTrajectory(mavlink);
    }
    unsigned get_size()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
    }

private:
    MavlinkOrbSubscription *_sub;
    uint64_t _ca_traj_time;

    /* do not allow top copying this class */
    MavlinkStreamCaTrajectory(MavlinkStreamCaTrajectory &);
    MavlinkStreamCaTrajectory& operator = (const MavlinkStreamCaTrajectory &);

protected:
    explicit MavlinkStreamCaTrajectory(Mavlink *mavlink) : MavlinkStream(mavlink),
        _sub(_mavlink->add_orb_subscription(ORB_ID(ca_trajectory))),  // make sure you enter the name of your uORB topic here
        _ca_traj_time(0)
    {}

    bool send(const hrt_abstime t)
    {
        struct ca_traj_struct_s _ca_trajectory;    //make sure ca_traj_struct_s is the definition of your uORB topic

        if (_sub->update(&_ca_traj_time, &_ca_trajectory)) {
            mavlink_ca_trajectory_t _msg_ca_trajectory;  //make sure mavlink_ca_trajectory_t is the definition of your custom MAVLink message

            _msg_ca_trajectory.timestamp = _ca_trajectory.timestamp;
            _msg_ca_trajectory.time_start_usec = _ca_trajectory.time_start_usec;
            _msg_ca_trajectory.time_stop_usec  = _ca_trajectory.time_stop_usec;
            _msg_ca_trajectory.coefficients =_ca_trajectory.coefficients;
            _msg_ca_trajectory.seq_id = _ca_trajectory.seq_id;

            mavlink_msg_ca_trajectory_send_struct(_mavlink->get_channel(), &_msg_ca_trajectory)
        }

        return true;
    }
};

Finally append the stream class to the streams_list at the bottom of mavlink_messages.cpp

StreamListItem *streams_list[] = {
...
StreamListItem *streams_list[] = {
...
new StreamListItem(&MavlinkStreamCaTrajectory::new_instance, &MavlinkStreamCaTrajectory::get_name_static, &MavlinkStreamCaTrajectory::get_id_static),
nullptr
};

Then make sure to enable the stream, for example by adding the following line to the startup script (e.g. /ROMFS/px4fmu_common/init.d-posix/rcS on NuttX or ROMFS/px4fmu_common/init.d-posix/rcS) on SITL. Note that -r configures the streaming rate and -u identifies the MAVLink channel on UDP port 14556).

mavlink stream -r 50 -s CA_TRAJECTORY -u 14556

You can use the uorb top [<message_name>] command to verify in real-time that your message is published and the rate (see uORB Messaging). This approach can also be used to test incoming messages that publish a uORB topic (for other messages you might use printf in your code and test in SITL).

To see the message on QGroundControl you will need to build it with your MAVLink library, and then verify that the message is received using MAVLink Inspector Widget (or some other MAVLink tool).

接收自定义MAVLink消息

Step3. 在mavlink_receiver.h中，向类MavlinkReceiver中添加uORB发布者类型的成员变量：

Step4. 在mavlink_receiver.cpp中给出函数handle_message_ca_trajectory_msg的具体实现：

#include <uORB/topics/ca_trajectory.h>
#include <v2.0/custom_messages/mavlink_msg_ca_trajectory.h>

Step5. 最后，确保上述自定义函数在MavlinkReceiver::handle_message()中被调用：

void handle_message_ca_trajectory_msg(mavlink_message_t *msg);

有时候需要创建一个内容尚未完全定义的自定义 MAVlink 消息。

orb_advert_t _ca_traj_msg_pub;

Implement the handle_message_ca_trajectory_msg function in mavlink_receiver.cpp

void MavlinkReceiver::handle_message_ca_trajectory_msg(mavlink_message_t *msg)
{
    mavlink_ca_trajectory_t traj;
    mavlink_msg_ca_trajectory_decode(msg, &traj);

    struct ca_traj_struct_s f;
    memset(&f, 0, sizeof(f));

    f.timestamp = hrt_absolute_time();
    f.seq_id = traj.seq_id;
    f.time_start_usec = traj.time_start_usec;
    f.time_stop_usec = traj.time_stop_usec;
    for(int i=0;i<28;i++)
        f.coefficients[i] = traj.coefficients[i];

    if (_ca_traj_msg_pub == nullptr) {
        _ca_traj_msg_pub = orb_advertise(ORB_ID(ca_trajectory), &f);

    } else {
        orb_publish(ORB_ID(ca_trajectory), _ca_traj_msg_pub, &f);
    }
}

and finally make sure it is called in MavlinkReceiver::handle_message()

MavlinkReceiver::handle_message(mavlink_message_t *msg)
 {
    switch (msg->msgid) {
        ...
    case MAVLINK_MSG_ID_CA_TRAJECTORY:
        handle_message_ca_trajectory_msg(msg);
        break;
        ...
    }
    case MAVLINK_MSG_ID_CA_TRAJECTORY:
        handle_message_ca_trajectory_msg(msg);
        break;
        ...
    }

另一种自定义MAVlink消息的办法

Sometimes there is the need for a custom MAVLink message with content that is not fully defined.

You can get the port number with mavlink status which will output (amongst others) transport protocol: UDP (<port number>). An example would be: 一个例子是：

An alternative - and temporary - solution is to re-purpose debug messages. Instead of creating a custom MAVLink message CA_TRAJECTORY, you can send a message DEBUG_VECT with the string key CA_TRAJ and data in the x, y and z fields. See this tutorial. for an example usage of debug messages.

常规信息

设置流速率（streaming rate）

Sometimes it is useful to increase the streaming rate of individual topics (e.g. for inspection in QGC). This can be achieved by typing the following line in the shell:

mavlink stream -u <port number> -s <mavlink topic name> -r <rate>

You can get the port number with mavlink status which will output (amongst others) transport protocol: UDP (<port number>). An example would be:

mavlink stream -u 14556 -s OPTICAL_FLOW_RAD -r 300

PreviousuORB 消息参考 NextRTPS/DDS 接口: PX4-Fast RTPS(DDS) 桥

Last updated 2 years ago