//CRC16_MODBUS
uint16_t cal_CRC16(const uint8_t* p_data, uint32_t size)
{
uint16_t crc = 0xFFFF;
for (int pos = 0; pos < size; pos++)
{
crc ^= (uint16_t)p_data[pos]; // XOR byte into least sig. byte of crc
for (int i = 8; i != 0; i--) // Loop over each bit
{
if ((crc & 0x0001) != 0) // If the LSB is set
{
crc >>= 1; // Shift right and XOR 0xA001
crc ^= 0xA001;
}
else // Else LSB is not set
{
crc >>= 1; // Just shift right
}
}
}
crc = ((crc & 0x00ff) << 8) | ((crc & 0xff00) >> 8);
//DBG_printf("%s crc = 0x%04x\n", __func__, crc);
return crc;
}