mavlink CRC算法程序

提取出的mavlink CRC校驗算法程序如下：

// test.cpp : 定義控制檯應用程序的入口點。
#include <stdint.h>
#include "stdafx.h"
#include <stdlib.h>
//
#define X25_INIT_CRC 0xffff
/** 
 * Calculating CRC-16 in 'C' 
 * @para addr, start of data 
 * @para num, length of data 
 * @para crc, incoming CRC 
 */  

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);
}

static inline void crc_init(uint16_t* crcAccum)
{
        *crcAccum = X25_INIT_CRC;
}

static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
{
        uint16_t crcTmp;
        crc_init(&crcTmp);
    while (length--) {
                crc_accumulate(*pBuffer++, &crcTmp);
        }
        return crcTmp;
}

static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
{
    const uint8_t *p = (const uint8_t *)pBuffer;
    while (length--) {
                crc_accumulate(*p++, crcAccum);
        }
}
int _tmain(int argc, _TCHAR* argv[])
{
unsigned char buff[5];
    unsigned char packet[3];
    uint16_t checksum;
    uint8_t ck[2];
    buff[0] = 0xFE;// stx
    buff[1] = 0x03;//Length of payload
    buff[2] = 0x00;//Sequence of packet
    buff[3] = 0x4C;//sysid    ID of message sender system/aircraft
    buff[4] = 0xBE;//compid  ID of the message sender component
    buff[5] = 0x4D;//msgid   ID of message in payload
    packet[0] = 0x00;//payload
    packet[1] = 0x00;//payload
    packet[2] = 0x01;//payload
    checksum = crc_calculate((const uint8_t*)&buff[1], 5);
    crc_accumulate_buffer(&checksum,  (const char *)packet,3);
    crc_accumulate(143, &checksum);
    ck[0] = (uint8_t)(checksum & 0xFF);
    ck[1] = (uint8_t)(checksum >> 8);
    printf("%02x\n", ck[0]);  
    printf("%02x\n", ck[1]); 
system("pause");
    return 0;
}