mavlink增加自定义消息

mavlink作为PX4以及APM两大开源飞控的通讯协议，应用非常广泛。在进行开源飞控二次开发时，增加自定义消息非常普遍。比如在offboard模式下，将视觉避障信息或者雷达信息发送给飞控，这时候就需要在mavlink中自定义一些消息了。

• 创建一个自定义MAVLink消息
  在/msg文件夹下面添加ca_trajectory.msg，添加内容如下

  1 2 3 4 5 6

  uint64_t time_start_usec uint64_t time_stop_usec uint32_t coefficients uint16_t seq_id #TOPICS ca_trajectory

  先自定义在mavlink/include/mavlink/v2.0/message_definitions/下创建自定义消息custom_message.xml文件，与ca_trajectory中结构一致，内容如下：

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

  <? 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 >

  注意需要添加一个timestamp的成员，因为在编译时为了记录，会增加一个timestamp的成员。然后使用mavlink generator生成c语言源文件。

• 1.去git 官网上下载源码

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

  2.进入代码目录并更新子模块

  cd mavlink

  git submodule update --init --recursive

  3.编译并运行python脚本

  python -m mavgenerate

  完成以上步骤即可。

  XML文件定位可以直接定位到mavlink/include/mavlink/v2.0/message_definitions/custom_message.xml，Out目录定位到mavlink/include/mavlink/v2.0/common，注意语言选择C，协议选择2.0，如下图所示：

  

  然后点击Generate即可生成c代码源文件。

• 发送自定义MAVLink消息
  自定义信息的发送主要通过修改src/modules/mavlink/下的mavlink_messsages.cpp文件来实现。
  • 添加自定义消息的头文件

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

    注意添加到已有包含文件的尾部，否则编译时可能会出现类型为定义的错误。

  • 创建自定义消息对应的类

    1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67

    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 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_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); return true ; } return false ; } };

    注意该类的函数成员返回类型和MAVLink v1.0稍有不同，笔者发现下载到的master分支和stable分支的函数类型都有所不同，具体以下载代码为准，可以参考代码中已有的其他消息例子来编写。

  • 在附加流类中添加该自定义项

    1 2 3 4 5

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

• • 在mavlink_main.cpp中加入自定义消息的更新频率：

    1	configure_stream( "CA_TRAJECTORY" , 100.0f);

• 接收自定义MAVLink消息
  接收自定义消息主要通过修改 mavlink_receiver.h 和 mavlink_receiver.cpp 来实现，加入一个消息处理函数，并调用他。
  • 在mavlink_receiver.h中加入如下代码：
    • 添加自定义消息头文件：

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

    • 在类中添加一个uORB消息发布者

      1	orb_advert_t _ca_traj_msg_pub;

    • 在类Mavlink_Receiver中增加一个消息处理函数

      1	void handle_message_ca_trajectory_msg(mavlink_message_t *msg);

  • 在mavlink_receiver.cpp中加入如下代码：

    • 实现消息处理函数

      1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

      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() 中调用消息处理函数

      1 2 3 4 5 6 7 8 9 10 11

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

本文由 在线网速测试 整理编辑，转载请注明出处，原文链接：https://www.wangsu123.cn/news/18767.html。