CRC16 calculation for Modbus RTU

crc16_modbus_rtu.c

//data[] is byte array to calculate CRC16
byte res[8]; //result is stored in this array

void ModRTU_CRC(byte data[]) {
  
  uint16_t crc = 0xFFFF;
  for (int pos = 0; pos < 6; pos++) { //6 is data array length
    res[pos] = data[pos];
    crc ^= (uint16_t)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
    }
  }
  res[6] = lowByte(crc);
  res[7] = highByte(crc);

}