51 lines
1.0 KiB
C++
51 lines
1.0 KiB
C++
#include "PacketCRC.h"
|
|
|
|
PacketCRC::PacketCRC() : crc(CRC16(0x755B, 0x0, 0xBA68, false, false)) {
|
|
// Constructor implementation (if needed)
|
|
}
|
|
|
|
PacketCRC::~PacketCRC() {
|
|
// Destructor implementation (if needed)
|
|
}
|
|
|
|
bool PacketCRC::check(const uint8_t* packet) {
|
|
uint8_t length = packet[1];
|
|
switch(length) {
|
|
case 0x11:
|
|
case 0x15:
|
|
case 0x19:
|
|
break;
|
|
default:
|
|
return false;
|
|
}
|
|
|
|
uint16_t result = calculate(packet);
|
|
uint8_t checksum1 = (uint8_t)((result & 0xFF00) >> 8);
|
|
uint8_t checksum2 = (uint8_t)(result & 0x00FF);
|
|
|
|
if (packet[length + 2] == checksum1 && packet[length + 3] == checksum2) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
uint16_t PacketCRC::calculate(const uint8_t* packet) {
|
|
crc.restart();
|
|
uint8_t length = packet[1];
|
|
switch(length) {
|
|
case 0x11:
|
|
crc.setXorOut(0x539E);
|
|
break;
|
|
case 0x15:
|
|
crc.setXorOut(0x3B0E);
|
|
break;
|
|
case 0x19:
|
|
crc.setXorOut(0x5C02);
|
|
break;
|
|
default:
|
|
return 0x0000;
|
|
}
|
|
crc.add(packet, length + 2);
|
|
return crc.calc();
|
|
} |