px4添加自定义mavlink消息通信校验通不过解决办法

参照网上的很多的博客，例https://www.cnblogs.com/spyplus/p/7429751.html

照着往下改，遇到了两个问题

添加新的mavlink协议时，mavlink文件没有问题，但是找不到bulid目录下uorb/topics下找不到头文件

#include <uORB/topics/ca_trajectory.h>

#include <v2.0/custom_messages/mavlink_msg_ca_trajectory.h>
           

再查找代码发现需要在msg目录下Cmake.list文件中加入

自定义的.msg文件。

这是第一个问题

第二个问题是添加好自定义的mavlink消息接受代码，试着用Tools目录下mavlink_px4.py文件自定义好消息发送消息时发现，消息接收不到，

跟踪打印发现，在mavlink_receiver.cpp文件下

在这里进行了消息的校验，

for (ssize_t i = 0; i < nread; i++) {
    if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) {
        ……
           

通过循环mavlink_parse_char函数中crc校验buf[i]

继续跟踪下去mablink_helps.h文件中

mavlink_parse_char函数中调用

mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)

在这个函数中又调用到了

mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
                                                 mavlink_status_t* status,
                                                 uint8_t c,
                                                 mavlink_message_t* r_message,
                                                 mavlink_status_t* r_mavlink_status)
           

在这个程序中，通过switch (status->parse_state)

而status其实就是mavlink协议每一位标志，每次循环读取buf【i】完切到下一位

第一位识别到0xFE进入mavlink_start_checksum(rxmsg);函数，给rxmsg->checksum数给了一个0xFFFF

接下来对读取buf[i]进入mavlink_update_checksum(rxmsg, c)调用crc_accumulate(c, &msg->checksum);

这个c就是buf[i]，最后执行这个函数

static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
        /*Accumulate one byte of data into the CRC*/
        uint8_t tmp;

        tmp = data ^ (uint8_t)(*crcAccum &0xff);     
        tmp ^= (tmp<<4);      
        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);     
}
           

这个函数就是crc校验，将寄存器中每一个字节的数都累计起来，最后问题出在哪里了

在我们发送消息的mavlink_px4.py文件下我们知道校验位需要加上msgidcrc值，这个值是生成mavlink头文件里写好的

例如MAVLINK_MSG_ID_136_CRC 1。每个id对应crc值

回头继续看接受消息这端

当所有的数都校验完后执行到

case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {

在这里，调用了一个函数mavlink_get_msg_entry(rxmsg->msgid);  

MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
{
	static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
        /*
	  use a bisection search to find the right entry. A perfect hash may be better
	  Note that this assumes the table is sorted by msgid
	*/
        uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
        while (low < high) {
            uint32_t mid = (low+1+high)/2;
            if (msgid < mavlink_message_crcs[mid].msgid) {
                high = mid-1;
                continue;
            }
            if (msgid > mavlink_message_crcs[mid].msgid) {
                low = mid;
                continue;
            }
            low = mid;
            break;
        }
        if (mavlink_message_crcs[low].msgid != msgid) {
            // msgid is not in the table
            return NULL;
        }
        return &mavlink_message_crcs[low];
}
           

这个函数最关键的是根据msgid号去找匹配id号的crc标识，在MAVLINK_MESSAGE_CRCS这个二维矩阵里边，有所有初始定义好的msg消息参数，和mavlink目录下头文件里的所有消息一致，从0到322，

第一位是消息的id （MAVLINK_MSG_ID_TERRAIN_REPORT 136），

第二位是该消息id的crc位（MAVLINK_MSG_ID_136_CRC 1），

第三位是消息的长度（MAVLINK_MSG_ID_136_LEN 22），

后面三位都是零

在这个矩阵中并没有我们自定义消息的id号，所有执行这个函数会返回一个0，这样肯定校验失败，所以需要在声明这个矩阵的文件下依照格式添上我们自定义的消息段，这个文件下

mavlink/include/mavlink/v2.0/standard/standard.h

在这里依照消息id的顺序，将消息数组添加上，添加完成后，再次调试就会发现消息发送成功。