AP_Math: Add fletcher16 CRC

CarbonixUAV · Nov 17, 2023 · 78d82a8 · 78d82a8
1 parent 5eb4329
commit 78d82a8
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 2 deletions.
diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp
@@ -354,7 +354,7 @@ uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc)
  * @param [in] len size of buffer
  * @return CRC value
  */
-uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len)
+uint16_t calc_crc_modbus(const uint8_t *buf, uint16_t len)
 {
     uint16_t crc = 0xFFFF;
     for (uint16_t pos = 0; pos < len; pos++) {
@@ -372,6 +372,18 @@ uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len)
     return crc;
 }
 
+// fletcher 16 implementation
+uint16_t crc_fletcher16(const uint8_t *buffer, uint32_t len) {
+    uint16_t c0 = 0;
+    uint16_t c1 = 0;
+    for (uint32_t i = 0; i < len; i++) {
+        c0 = (c0 + buffer[i]) % 255;
+        c1 = (c1 + c0) % 255;
+    }
+
+    return (c1 << 8) | c0;
+}
+
 // FNV-1a implementation
 #define FNV_1_PRIME_64 1099511628211UL
 void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash)

diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h
@@ -42,7 +42,9 @@ uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
 // https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf
 uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc);
 
-uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
+uint16_t calc_crc_modbus(const uint8_t *buf, uint16_t len);
+
+uint16_t crc_fletcher16(const uint8_t * buffer, uint32_t len);
 
 // generate 64bit FNV1a hash from buffer
 #define FNV_1_OFFSET_BASIS_64 14695981039346656037UL