用于项目需求,需要在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接收和处理另起篇章吧。