提取出的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;
}