You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
43 lines
1.2 KiB
43 lines
1.2 KiB
public partial class MAVLink
|
|
{
|
|
public 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
|
|
{
|
|
crcTmp = crc_accumulate(pBuffer[i], crcTmp);
|
|
//Console.WriteLine(crcTmp + " " + pBuffer[i] + " " + length);
|
|
}
|
|
|
|
return (crcTmp);
|
|
}
|
|
|
|
}
|
|
}
|