[pixhawk笔记]7-MAVLink添加自定义消息

前一篇学习了uORB，用于px4中各个模块的进程间通信，下来学习MAVLink，用于飞控和地面站之间的通信。教程中主要给出了使用MAVLink的发送和接收消息的方法。
完整的MAVLink消息列表见该网页。

• 创建一个自定义MAVLink消息
  假设存在/msg/ca_trajectory.msg定义了ca_trajectory的uORB主题。（笔者下载到的代码中没有自定义的ca_trajectory主题）
  如果没有，自行在/msg文件夹下面添加ca_trajectory.msg，笔者添加内容如下（该内容来自于Fantasy大神的博客）：

  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/custom_message/mavlink_msg_ca_trajectory.h中存在ca_trajectory的自定义mavlink消息。（笔者下载的代码中也没有该部分）　　
  没有的话可以自己添加自定义mavlink消息，教程见官方文档。
  先自定义在mavlink/include/mavlink/v2.0/message_definitions/下创建自定义消息custom_message.xml文件，与ca_trajectory中结构一致，内容如下：

  <?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语言源文件。
  没有mavlink generator的同学可以用git下载并使用：

  git clone https://github.com/mavlink/mavlink mavlink-generator
  cd mavlink-generator
  python mavgenerate.py
  

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

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

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

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

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

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

    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分支的函数类型都有所不同，具体以下载代码为准，可以参考代码中已有的其他消息例子来编写。

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

    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和mavlink_receiver.cpp来实现，加入一个消息处理函数，并调用他。
  • 在mavlink_receiver.h中加入如下代码：
    • 添加自定义消息头文件：

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

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

      orb_advert_t _ca_traj_msg_pub;

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

      void handle_message_ca_trajectory_msg(mavlink_message_t *msg);

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

    • 实现消息处理函数

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

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

      但是，在nsh里面看不到该消息发布的数据，哪位高手能告诉我哪错了……