AP_GPS/AP_Math: Move the CRC24 to the AP_Math class

libraries/AP_GPS/RTCM3_Parser.cpp

@@ -18,6 +18,7 @@
 */
 
 #include <string.h>
+#include <AP_Math/AP_Math.h>
 #include "RTCM3_Parser.h"
 
 // reset state
@@ -86,7 +87,7 @@ bool RTCM3_Parser::parse(void)
 {
     const uint8_t *parity = &pkt[pkt_len+3];
     uint32_t crc1 = (parity[0] << 16) | (parity[1] << 8) | parity[2];
-    uint32_t crc2 = crc24(pkt, pkt_len+3);
+    uint32_t crc2 = crc_crc24(pkt, pkt_len+3);
     if (crc1 != crc2) {
         resync();
         return false;
@@ -134,31 +135,6 @@ bool RTCM3_Parser::read(uint8_t byte)
     return false;
 }
 
-/*
-  calculate 24 bit RTCMv3 crc. We take an approach that saves memory
-  and flash at the cost of higher CPU load. This makes it appropriate
-  for use in the f103 AP_Periph nodes
-  On a F765 this costs us approx 2ms of CPU per second of 5Hz all
-  constellation RTCM data
-*/
-uint32_t RTCM3_Parser::crc24(const uint8_t *bytes, uint16_t len)
-{
-    uint32_t crc = 0;
-    while (len--) {
-        uint8_t b = *bytes++;
-        const uint8_t idx = (crc>>16) ^ b;
-        uint32_t crct = idx<<16;
-        for (uint8_t j=0; j<8; j++) {
-            crct <<= 1;
-            if (crct & 0x1000000) {
-                crct ^= POLYCRC24;
-            }
-        }
-        crc = ((crc<<8)&0xFFFFFF) ^ crct;
-    }
-    return crc;
-}
-
 #ifdef RTCM_MAIN_TEST
 /*
   parsing test, taking a raw file captured from UART to u-blox F9

libraries/AP_GPS/RTCM3_Parser.h

@@ -38,7 +38,6 @@ class RTCM3_Parser {
 
 private:
     const uint8_t RTCMv3_PREAMBLE = 0xD3;
-    const uint32_t POLYCRC24 = 0x1864CFB;
 
     // raw packet, we shouldn't need over 300 bytes for the MB configs we use
     uint8_t pkt[300];
@@ -54,6 +53,5 @@ class RTCM3_Parser {
 
     bool parse(void);
     void resync(void);
-    uint32_t crc24(const uint8_t *bytes, uint16_t len);
 };
 

libraries/AP_Math/crc.cpp

@@ -302,3 +302,22 @@ void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash)
         *hash *= FNV_1_PRIME_64;
     }
 }
+
+// calculate 24 bit crc. We take an approach that saves memory and flash at the cost of higher CPU load.
+uint32_t crc_crc24(const uint8_t *bytes, uint16_t len)
+{
+    uint32_t crc = 0;
+    while (len--) {
+        uint8_t b = *bytes++;
+        const uint8_t idx = (crc>>16) ^ b;
+        uint32_t crct = idx<<16;
+        for (uint8_t j=0; j<8; j++) {
+            crct <<= 1;
+            if (crct & 0x1000000) {
+                crct ^= POLYCRC24;
+            }
+        }
+        crc = ((crc<<8)&0xFFFFFF) ^ crct;
+    }
+    return crc;
+}

libraries/AP_Math/crc.h

@@ -25,6 +25,7 @@ uint16_t crc_xmodem_update(uint16_t crc, uint8_t data);
 uint16_t crc_xmodem(const uint8_t *data, uint16_t len);
 uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size);
 uint32_t crc32_small(uint32_t crc, const uint8_t *buf, uint32_t size);
+uint32_t crc_crc24(const uint8_t *bytes, uint16_t len);
 
 // Copyright (C) 2010 Swift Navigation Inc.
 // Contact: Fergus Noble <fergus@swift-nav.com>
@@ -36,3 +37,4 @@ uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
 #define FNV_1_OFFSET_BASIS_64 14695981039346656037UL
 void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash);
 
+static const uint32_t POLYCRC24 = 0x1864CFB;