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