mirror of https://github.com/ArduPilot/ardupilot
49 lines
1.3 KiB
C#
49 lines
1.3 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
|
|
namespace ArdupilotMega.Mavlink
|
|
{
|
|
class MavlinkCRC
|
|
{
|
|
const int X25_INIT_CRC = 0xffff;
|
|
const int X25_VALIDATE_CRC = 0xf0b8;
|
|
|
|
public static ushort crc_accumulate(byte b, ushort crc)
|
|
{
|
|
unchecked
|
|
{
|
|
byte ch = (byte)(b ^ (byte)(crc & 0x00ff));
|
|
ch = (byte)(ch ^ (ch << 4));
|
|
return (ushort)((crc >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4));
|
|
}
|
|
}
|
|
|
|
public static ushort crc_calculate(byte[] pBuffer, int length)
|
|
{
|
|
if (length < 1)
|
|
{
|
|
return 0xffff;
|
|
}
|
|
// For a "message" of length bytes contained in the unsigned char array
|
|
// pointed to by pBuffer, calculate the CRC
|
|
// crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed
|
|
|
|
ushort crcTmp;
|
|
int i;
|
|
|
|
crcTmp = X25_INIT_CRC;
|
|
|
|
for (i = 1; i < length; i++) // skips header U
|
|
{
|
|
crcTmp = crc_accumulate(pBuffer[i], crcTmp);
|
|
//Console.WriteLine(crcTmp + " " + pBuffer[i] + " " + length);
|
|
}
|
|
|
|
return (crcTmp);
|
|
}
|
|
|
|
}
|
|
}
|