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

2019-11-07 23:52:42

字体：大中小

来源：转载

供稿：网友

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

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

自定义uORB消息

ca_trajectory.msg

uint64%20time_start_usecuint64%20time_stop_usecuint32%20coefficientsuint16%20seq_id#TOPICS%20ca_trajectory自定义MAVLink消息

custom_messages.xml

<?xml%20version="1.0"?><mavlink>%20%20%20%20<include>common.xml</include>%20%20%20%20%20%20%20%20%20%20%20%20<enums>%20%20%20%20</enums>%20%20%20%20<messages>%20%20%20%20%20%20%20%20<message%20id="166"%20name="CA_TRAJECTORY">%20%20%20%20%20%20%20%20%20%20%20%20<description>This%20message%20encodes%20all%20of%20the%20raw%20rudder%20sensor%20data%20from%20the%20USV.</description>%20%20%20%20%20%20%20%20%20%20%20%20<field%20type="uint64_t"%20name="timestamp">Timestamp%20in%20milliseconds%20since%20system%20boot</field>%20%20%20%20%20%20%20%20%20%20%20%20<field%20type="uint64_t"%20name="time_start_usec">start%20time,%20unit%20usec.</field>%20%20%20%20%20%20%20%20%20%20%20%20<field%20type="uint64_t"%20name="time_stop_usec">stop%20time,%20unit%20usec.</field>%20%20%20%20%20%20%20%20%20%20%20%20<field%20type="uint32_t"%20name="coefficients">as%20it%20says.</field>%20%20%20%20%20%20%20%20%20%20%20%20<field%20type="uint16_t"%20name="seq_id">can%20not%20cheat%20any%20more.</field>%20%20%20%20%20%20%20%20</message>%20%20%20%20</messages></mavlink>

使用python%20-m%20mavgenerate打开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功能

voidMavlinkReceiver::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; ... }