Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Move the CRC24 to the AP_Math class

This commit is contained in:
murata 2020-04-14 19:29:33 +09:00 committed by Andrew Tridgell
parent 7c8df47ac9
commit 4c835a0df9
2 changed files with 2 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_GPS/RTCM3_Parser.cpp
View File

@ -18,6 +18,7 @@
*/ */
#include <string.h> #include <string.h>
#include <AP_Math/AP_Math.h>
#include "RTCM3_Parser.h" #include "RTCM3_Parser.h"
// reset state // reset state
@ -86,7 +87,7 @@ bool RTCM3_Parser::parse(void)
{ {
const uint8_t *parity = &pkt[pkt_len+3]; const uint8_t *parity = &pkt[pkt_len+3];
uint32_t crc1 = (parity[0] << 16) | (parity[1] << 8) | parity[2]; 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) { if (crc1 != crc2) {
resync(); resync();
return false; return false;
@ -134,31 +135,6 @@ bool RTCM3_Parser::read(uint8_t byte)
return false; 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 #ifdef RTCM_MAIN_TEST
/* /*
parsing test, taking a raw file captured from UART to u-blox F9 parsing test, taking a raw file captured from UART to u-blox F9

2
libraries/AP_GPS/RTCM3_Parser.h
View File

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