[pixhawk筆記]7-MAVLink添加自定義消息

本文轉載自查看原文 2017-09-01 22:50 1616 mavlink/ 學習筆記/ uORB/ pixhawk

前一篇學習了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>
    
    
    <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里面看不到該消息發布的數據，哪位高手能告訴我哪錯了……

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 MFC 如何添加自定義消息推送的通知和自定義消息區別 c#如何處理自定義消息 springboot自定義消息轉換器HttpMessageConverter 釘釘通過userid發送自定義消息給用戶 ROS 自定義消息類型方法 Kafka中Producer端封裝自定義消息 kafka發送自定義消息體（對象、數組） ROS--自定義消息類型 c#自定義消息框