PX4自定义Mavlink消息（一）px4发送

最新推荐文章于 2024-05-16 23:27:41 发布

Sense97

最新推荐文章于 2024-05-16 23:27:41 发布

阅读量3.1k

点赞数 6

分类专栏： PX4二次开发 QGroundControl地面端二次开发文章标签： qt linux c++

本文链接：https://blog.csdn.net/Sense97/article/details/121996291

版权

QGroundControl地面端二次开发同时被 2 个专栏收录

4 篇文章 2 订阅

订阅专栏

PX4二次开发

2 篇文章 7 订阅

订阅专栏

用于项目需求，需要在PX4和QGC新增自定义消息用于通信控制，看了很多有关文章，我就不明白，为啥代码都是复制来复制去的，大部分的文章连变量名的起的一样。。。一点帮助都没有，还是决定自己研究，就当作给自己写了个备忘录吧

首先，第一步先添加一个uORB topic，在源码根目录下的msg文件中，添加.msg文件，用于生成对应的topic的头文件（这里你需要了解px4的uORB通信机制）,文件和代码如下：

uint64 timestamp        #time since system start (microseconds)
uint8 data1
uint8 data2

# TOPICS vehicle_zkrt

这里需要特别特别注意，最后一行的#和TOPICS还有vehicle_zkrt之间都是只有一个空格，多了少了都不行，编译的时候会报错，如果对变量定义不清楚，可以参考msg文件夹下其他的.msg文件。做完这步后，在msg文件下的CMakeList.txt文件下添加你的.msg文件，这样编译源码的时候才会根据你的.msg文件自动在Firmware/build/px4_fmu-v5_default/msg/tmp/headers下生成相应的消息同文件，此时，uORB topic添加完毕。如图：

接下来就是第二步，自定义zkrt_message.xml文件，路径如下：Firmware/mavlink/include/mavlink/v2.0/message_definitions/zkrt_message.xml

这里可以看到我的message id设置的166，网上大多数都是设置的166，但是qgc接收会有一定问题，因为apm固件里面有用到166，只是px4没有用到：

<?xml version="1.0"?>
<mavlink>
    <include>common.xml</include>
    <!-- NOTE: If the included file already contains a version tag, remove the version tag here, else uncomment to enable. -->
    <!--<version>3</version>-->
    <enums>
    </enums>
    <messages>
        <message id="166" name="VEHICLE_ZKRT">
            <description>This message encodes all of the raw rudder sensor data from the USV.</description>
            <field type="uint64_t" name="timestamp">Timestamp in milliseconds since system boot</field>
            <field type="uint8_t" name="data1">start time, unit usec.</field>
            <field type="uint8_t" name="data2">stop time, unit usec.</field>
        </message>
    </messages>
</mavlink>

然后下载mavlink消息生成器：

git clone https://github.com/mavlink/mavlink.git --recursive

下载完成后如下图：

在控制台敲python3 -m mavgenerate就可以弹出生成器的窗口了，当然，也有可能报错，缺少一些库文件，我这里缺少python3-tk, 直接安装就可以了sudo apt-get install python3-tk，少什么就装什么，一般不会有太大问题，如下图：

XML路径选择你刚才自定义的.xml文件，Out路径则选择Firmware/mavlink/include/mavlink/v2.0,点击Generate后会弹窗，不管他，继续点ok就行。

上图为生成器自动的生成的文件夹zkrt_message里面的头文件，我们要用到的是mavlink_msg_vehicle_zkrt.h这个头文件里面的mavlink消息打包函数。

在src/modules/mavlink/streams目录下定义VEHICLE_ZKRT.hpp头文件：

#ifndef VEHICLE_ZKRT_HPP
#define VEHICLE_ZKRT_HPP

#include <uORB/topics/vehicle_zkrt.h> //包含uorb消息结构体的头文件
#include <v2.0/zkrt_message/mavlink_msg_vehicle_zkrt.h> //包含生成器生成的头文件
#include <modules/mavlink/mavlink_stream.h> //自定义类继承与MavlinkStream，所以要包含
#include <v2.0/mavlink_types.h>
#include <uORB/topics/vehicle_zkrt.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/uORB.h>

class MavlinkStreamVehicleZkrt: public MavlinkStream
{
public:
    const char *get_name() const
    {
        return MavlinkStreamVehicleZkrt::get_name_static();
    }

    static const char *get_name_static()
    {
        return "VEHICLE_ZKRT";
    }

    static uint16_t get_id_static()
    {
        return MAVLINK_MSG_ID_VEHICLE_ZKRT;
    }

    uint16_t get_id()
    {
        return get_id_static();
    }

    static MavlinkStream *new_instance(Mavlink *mavlink)
    {
        return  new   MavlinkStreamVehicleZkrt (mavlink);
    }

    unsigned get_size()
    {
        return MAVLINK_MSG_ID_VEHICLE_ZKRT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
    }

private:

    explicit MavlinkStreamVehicleZkrt(Mavlink *mavlink) : MavlinkStream(mavlink) {}

    bool send() override //用于PX4真正发送的函数
    {
        struct vehicle_zkrt_s _vehicle_zkrt;    //定义uorb消息结构体

        if (true) {

		//int sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_zkrt));//订阅
		//orb_copy(ORB_ID(vehicle_zkrt), sensor_sub_fd, &_vehicle_zkrt);//获取消息数据

                uORB::Subscription _zkrt_sub{ORB_ID(vehicle_zkrt)};//订阅
			    _zkrt_sub.copy(&_vehicle_zkrt);//获取消息数据

           	    mavlink_vehicle_zkrt_t msg;//定义mavlink消息结构体

            	msg.timestamp = _vehicle_zkrt.timestamp;//这里uorb数据赋值到mavlink结构体上
            	msg.data1 = _vehicle_zkrt.data1;
            	msg.data2  = _vehicle_zkrt.data2;

           	 mavlink_msg_vehicle_zkrt_send_struct(_mavlink->get_channel(), &msg);//利用生成器生成的mavlink_msg_vehicle_zkrt.h头文件里面的这个函数将msg（mavlink结构体）封装成mavlink消息帧并发送；
            	return true;
        }
        return false;
    }
};
#endif

这个VEHICLE_ZKRT.hpp非常重要，建议不要复制粘贴，因为根据px4固件版本的不同，此代码会有所不用，所以一定要理解他再去修改此段代码，否则编译肯定会报错。

我们自定义的MavlinkStreamVehicleZkrt类是继承了MavlinkStream类，这个类里写了很多虚函数，其中send函数就是虚函数，我们这里需要重写send函数，此函数就是用于给px4发送自定义mavlink消息的函数，建议打开一个streams目录下其他.hpp头文件，模仿他的写法，自己写一个。

接下来就比较简单了，在mavlink_messages.cpp下添加：（记得添加头文件）

//添加头文件
#include"streams/VEHICLE_ZKRT.hpp"

static const StreamListItem streams_list[] = {

//添加代码
#if defined(VEHICLE_ZKRT_HPP)
create_stream_list_item<MavlinkStreamVehicleZkrt>(),
#endif // VEHICLE_ZKRT_HPP


#if defined(HEARTBEAT_HPP)
	create_stream_list_item<MavlinkStreamHeartbeat>(),
#endif // HEARTBEAT_HPP
#if defined(STATUSTEXT_HPP)
	create_stream_list_item<MavlinkStreamStatustext>(),
#endif // STATUSTEXT_HPP
#if defined(COMMAND_LONG_HPP)
	create_stream_list_item<MavlinkStreamCommandLong>(),
#endif // COMMAND_LONG_HPP

在mavlink_main.cpp中添加configure_stream_local("VEHICLE_ZKRT", 30.0f); 这里设置是30hz，也就是一秒钟发送三十次，建议一开始设置频率高一点，因为px4发送mavlink消息很多，如果你设置低了，很难找到自定义的消息，我是在MAVLINK_MODE_CONFIG模式下定义，因为我是用的usb调试的，大家可以试试这样在数传电台中可不可以传输。

case MAVLINK_MODE_CUSTOM:
		//stream nothing
		break;

	case MAVLINK_MODE_CONFIG: // USB 
		// Note: streams requiring low latency come first

		configure_stream_local("VEHICLE_ZKRT", 30.0f);//这里我是在config模式下定义
		configure_stream_local("TIMESYNC", 10.0f);
		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
		configure_stream_local("DISTANCE_SENSOR", 10.0f);
		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
		configure_stream_local("ODOMETRY", 30.0f);

接下来，编译烧录，px4就会以30hz的频率发送自定义消息帧，可以拿串口助手去接收一下，但是并不太方便，我就自己用qt写了个串口日志记录：

根据mavlink1.0的消息结构，大家随便找一帧数据包，发现sys_id和comp_id为固定的01 01，每个人可能不一样噢，那我的msg_id为166，16进制就为a6，就可以检索01 01 a6，这样就能找到px4发送的自定义消息帧了，到这里就成功了一半了，不过我的消息发出来会乱码，目前还没搞清楚是什么原因，按道理来说playload数据应该全是0才对。之后再去找原因吧，先用qgc接收了再说！

先这样吧！qgc接收和处理另起篇章吧。