diff --git a/libraries/AP_GPS/RTCM3_Parser.cpp b/libraries/AP_GPS/RTCM3_Parser.cpp new file mode 100644 index 0000000000..64587fb063 --- /dev/null +++ b/libraries/AP_GPS/RTCM3_Parser.cpp @@ -0,0 +1,191 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + RTCMv3 parser, used to support moving baseline RTK mode between two + GPS modules +*/ + +#include +#include "RTCM3_Parser.h" + +// reset state +void RTCM3_Parser::reset(void) +{ + pkt_len = 0; + pkt_bytes = 0; + found_len = 0; +} + +// clear previous packet +void RTCM3_Parser::clear_packet(void) +{ + if (found_len == 0) { + return; + } + // clear previous packet + if (pkt_bytes > found_len) { + memmove(&pkt[0], &pkt[found_len], pkt_bytes-found_len); + pkt_bytes -= found_len; + } else { + pkt_bytes = 0; + } + found_len = 0; + pkt_len = 0; +} + +// return length of found packet +uint16_t RTCM3_Parser::get_len(const uint8_t *&bytes) const +{ + if (found_len > 0) { + bytes = &pkt[0]; + } + return found_len; +} + +// return ID of found packet +uint16_t RTCM3_Parser::get_id(void) const +{ + if (found_len == 0) { + return 0; + } + return (pkt[3]<<8 | pkt[4]) >> 4; +} + +// look for preamble to try to resync +void RTCM3_Parser::resync(void) +{ + const uint8_t *p = (const uint8_t *)memchr(&pkt[1], RTCMv3_PREAMBLE, pkt_bytes-1); + if (p != nullptr) { + const uint16_t idx = p - &pkt[0]; + memmove(&pkt[0], p, pkt_bytes - idx); + pkt_bytes -= idx; + if (pkt_bytes >= 3) { + pkt_len = (pkt[1]<<8 | pkt[2]) & 0x3ff; + } else { + pkt_len = 0; + } + } else { + reset(); + } +} + +// parse packet +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); + if (crc1 != crc2) { + resync(); + return false; + } + + // we got a good packet + found_len = pkt_len+6; + return true; +} + +// read in one byte, return true if a full packet is available +bool RTCM3_Parser::read(uint8_t byte) +{ + clear_packet(); + + if (pkt_bytes > 0 && pkt[0] != RTCMv3_PREAMBLE) { + resync(); + } + + if (pkt_bytes == 0 && byte != RTCMv3_PREAMBLE) { + // discard + return false; + } + + if (pkt_bytes >= sizeof(pkt)) { + // too long, resync + resync(); + } + + pkt[pkt_bytes++] = byte; + + if (pkt_len == 0 && pkt_bytes >= 3) { + pkt_len = (pkt[1]<<8 | pkt[2]) & 0x3ff; + if (pkt_len == 0) { + resync(); + } + } + + if (pkt_len != 0 && pkt_bytes >= pkt_len + 6) { + // got header, packet body and parity + return parse(); + } + + // need more bytes + 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 + */ +#include +#include +#include +#include +#include +#include + +int main(int argc, const char *argv[]) +{ + const char *fname = argv[1]; + int fd = ::open(fname, O_RDONLY); + if (fd == -1) { + perror(fname); + ::exit(1); + } + RTCM3_Parser parser {}; + uint8_t b; + while (::read(fd, &b, 1) == 1) { + if (parser.read(b)) { + const uint8_t *bytes; + printf("packet len %u ID %u\n", parser.get_len(bytes), parser.get_id()); + } + } + return 0; +} +#endif diff --git a/libraries/AP_GPS/RTCM3_Parser.h b/libraries/AP_GPS/RTCM3_Parser.h new file mode 100644 index 0000000000..f1e606870a --- /dev/null +++ b/libraries/AP_GPS/RTCM3_Parser.h @@ -0,0 +1,59 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + RTCMv3 parser, used to support moving baseline RTK mode between two + GPS modules +*/ + +#include + +class RTCM3_Parser { +public: + // process one byte, return true if packet found + bool read(uint8_t b); + + // reset internal state + void reset(void); + + // clear previous packet + void clear_packet(void); + + // return length of found packet + uint16_t get_len(const uint8_t *&bytes) const; + + // return ID of found packet + uint16_t get_id(void) const; + +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]; + + // number of bytes in pkt[] + uint16_t pkt_bytes; + + // length from header + uint16_t pkt_len; + + // length of found packet + uint16_t found_len; + + bool parse(void); + void resync(void); + uint32_t crc24(const uint8_t *bytes, uint16_t len); +}; + diff --git a/modules/mavlink b/modules/mavlink index d2cc7dbff6..6e08c0757a 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit d2cc7dbff67f8c318a40e1ef57a99488b4737fab +Subproject commit 6e08c0757a1e90c461a8caf1a8d5b724abf3ca57