Pixhawk原生固件PX4之自定义MAVLink消息

欢迎交流~ 个人 Gitter 交流平台，点击直达：



本着想在PX4基础上加点什么东西的我又开始折腾了，先尝试用串口加传感器通过QGC查看，要是能在原固件上加点内容就棒哉了。先立Flag

自定义uORB消息

ca_trajectory.msg

uint64 time_start_usec
uint64 time_stop_usec
uint32 coefficients
uint16 seq_id

#TOPICS ca_trajectory

自定义MAVLink消息

custom_messages.xml

<?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="CA_TRAJECTORY">
<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="uint64_t" name="time_start_usec">start time, unit usec.</field>
<field type="uint64_t" name="time_stop_usec">stop time, unit usec.</field>
<field type="uint32_t" name="coefficients">as it says.</field>
<field type="uint16_t" name="seq_id">can not cheat any more.</field>
</message>
</messages>
</mavlink>

使用

python -m mavgenerate

打开mavlink消息生成器导入上面的xml文件，生成如下文件：



将生成的custom_messages文件夹拖到Firmware/mavlink/include/mavlink/v1.0目录下

发送自定义MAVLink消息

添加

mavlink

的头文件和uorb消息到mavlink_messages.cpp

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

在mavlink_messages.cpp中创建一个新的类

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

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

static uint8_t get_id_static()
{
return MAVLINK_MSG_ID_CA_TRAJECTORY;
}

uint8_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)
{}

void send(const hrt_abstime t)
{
struct ca_trajectory_s _ca_trajectory;    //make sure ca_trajectory_s is the definition of your uorb topic

if (_sub->update(&_ca_traj_time, &_ca_trajectory)) {

mavlink_ca_trajectory_t msg;//make sure mavlink_ca_trajectory_t is the definition of your custom mavlink message

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

mavlink_msg_ca_trajectory_send_struct(_mavlink->get_channel(), &msg);
}
}
};

附加流类

streams_list

的到mavlink_messages.cpp底部

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

最后在mavlink_main.cpp加入自定义的消息以及期望的更新频率

configure_stream("CA_TRAJECTORY", 100.0f);

接收自定义MAVLink消息

在mavlink_receiver.h中增加一个用来处理接收信息得函数

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

在 mavlink_receiver.h中增加一个处理类

MavlinkReceiver

中的输入mavlink消息的函数

void handle_message_ca_trajectory_msg(mavlink_message_t *msg);

在 mavlink_receiver.h中加入一个类

MavlinkReceiver

中的uORB消息发布者

orb_advert_t _ca_traj_msg_pub;

在mavlink_receiver.cpp中实现

handle_message_ca_trajectory_msg

功能

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_trajectory_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;
f.coefficients = traj.coefficients;

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);
}
}

最后确定函数在MavlinkReceiver::handle_message()中被调用

# 在mavlink_receiver.cpp文件中添加
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
switch (msg->msgid) {
...
case MAVLINK_MSG_ID_CA_TRAJECTORY:
handle_message_ca_trajectory_msg(msg);
break;
...
}

消息查看

将SD卡插入飞控连接电脑，然后会发现串口调试助手不停地吐MAVLink消息，如下所示（选择Hex方式接收，否则看到的都是乱码）：



可以很容易的解析出每一帧MAVLink消息的内容，依次按照下图所示的顺序

字节	内容	数值
0	起始标志位	0~255
1	载荷长度	0~255
2	包序号	0~255
3	SYSID	1~255
4	COMPID	0~255
5	MSGID	0~255
6~(n+6)	载荷	(0~255)字节
(n+7)~(n+8)	冗余校验	XXX

可以看到，并不是每一个MAVLink消息都是一直启动的，只有心跳包是时刻不断的发送消息的(1Hz)，其他的自启MAVLink消息规律暂时找不到，至少现在在串口读到的数据中还是找不到我定义的MSGID #166 = 0x6A。

要确认自定义的MAVLink真的存在，目前可以通过NSH查看

ls obj

与预想的直接可以在QGC地面站查看还有一定的差距。

大家有好办法的欢迎交流指导。

参考

1. PX4中文维基之定义MAVlink消息

2. 自定义uORB消息

3. 创建MAVLink消息

4. 生成MAVLink文件

                                          By Fantasy