aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: Common modbus crc method

Browse Source
This commit is contained in:
murata 2019-04-19 19:45:13 +09:00 committed by Andrew Tridgell
parent 27e802eaa6
commit c5a471e778
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Math/crc.cpp
View File

@ -225,7 +225,33 @@ static const uint16_t crc16tab[256] = {
uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc)
{
for (uint32_t i = 0; i < len; i++)
for (uint32_t i = 0; i < len; i++) {
crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
}
return crc;
}
/**
* Calculate Modbus CRC16 for array of bytes
*
* @param [in] buf input buffer
* @param [in] len size of buffer
* @return CRC value
*/
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len)
{
uint16_t crc = 0xFFFF;
for (uint16_t pos = 0; pos < len; pos++) {
crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
for (uint8_t 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
}
}
}
return crc;
}

5
libraries/AP_Math/crc.h
View File

@ -25,4 +25,7 @@ uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size);
// Copyright (C) 2010 Swift Navigation Inc.
// Contact: Fergus Noble <fergus@swift-nav.com>
uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);