Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   RTCMv3 parser, used to support moving baseline RTK mode between two
      17             :   GPS modules
      18             : */
      19             : 
      20             : #include <string.h>
      21             : #include <AP_Math/AP_Math.h>
      22             : #include "RTCM3_Parser.h"
      23             : 
      24             : // reset state
      25       18995 : void RTCM3_Parser::reset(void)
      26             : {
      27       18995 :     pkt_len = 0;
      28       18995 :     pkt_bytes = 0;
      29       18995 :     found_len = 0;
      30       18995 : }
      31             : 
      32             : // clear previous packet
      33     1180068 : void RTCM3_Parser::clear_packet(void)
      34             : {
      35     1180068 :     if (found_len == 0) {
      36     1180068 :         return;
      37             :     }
      38             :     // clear previous packet
      39           0 :     if (pkt_bytes > found_len) {
      40           0 :         memmove(&pkt[0], &pkt[found_len], pkt_bytes-found_len);
      41           0 :         pkt_bytes -= found_len;
      42             :     } else {
      43           0 :         pkt_bytes = 0;
      44             :     }
      45           0 :     found_len = 0;
      46           0 :     pkt_len = 0;
      47             : }
      48             : 
      49             : // return length of found packet
      50       21896 : uint16_t RTCM3_Parser::get_len(const uint8_t *&bytes) const
      51             : {
      52       21896 :     if (found_len > 0) {
      53           0 :         bytes = &pkt[0];
      54             :     }
      55       21896 :     return found_len;
      56             : }
      57             : 
      58             : // return ID of found packet
      59           0 : uint16_t RTCM3_Parser::get_id(void) const
      60             : {
      61           0 :     if (found_len == 0) {
      62           0 :         return 0;
      63             :     }
      64           0 :     return (pkt[3]<<8 | pkt[4]) >> 4;
      65             : }
      66             : 
      67             : // look for preamble to try to resync
      68          35 : void RTCM3_Parser::resync(void)
      69             : {
      70          35 :     const uint8_t *p = (const uint8_t *)memchr(&pkt[1], RTCMv3_PREAMBLE, pkt_bytes-1);
      71          35 :     if (p != nullptr) {
      72           2 :         const uint16_t idx = p - &pkt[0];
      73           2 :         memmove(&pkt[0], p, pkt_bytes - idx);
      74           2 :         pkt_bytes -= idx;
      75           2 :         if (pkt_bytes >= 3) {
      76           2 :             pkt_len = (pkt[1]<<8 | pkt[2]) & 0x3ff;
      77             :         } else {
      78           0 :             pkt_len = 0;
      79             :         }
      80             :     } else {
      81          33 :         reset();
      82             :     }
      83          35 : }
      84             : 
      85             : // parse packet
      86          28 : bool RTCM3_Parser::parse(void)
      87             : {
      88          28 :     const uint8_t *parity = &pkt[pkt_len+3];
      89          28 :     uint32_t crc1 = (parity[0] << 16) | (parity[1] << 8) | parity[2];
      90          28 :     uint32_t crc2 = crc_crc24(pkt, pkt_len+3);
      91          28 :     if (crc1 != crc2) {
      92          28 :         resync();
      93          28 :         return false;
      94             :     }
      95             : 
      96             :     // we got a good packet
      97           0 :     found_len = pkt_len+6;
      98           0 :     return true;
      99             : }
     100             : 
     101             : // read in one byte, return true if a full packet is available
     102     1180068 : bool RTCM3_Parser::read(uint8_t byte)
     103             : {
     104     1180068 :     clear_packet();
     105             : 
     106     1180068 :     if (pkt_bytes > 0 && pkt[0] != RTCMv3_PREAMBLE) {
     107           0 :         resync();
     108             :     }
     109             : 
     110     1180068 :     if (pkt_bytes == 0 && byte != RTCMv3_PREAMBLE) {
     111             :         // discard
     112     1174371 :         return false;
     113             :     }
     114             : 
     115        5697 :     if (pkt_bytes >= sizeof(pkt)) {
     116             :         // too long, resync
     117           0 :         resync();
     118             :     }
     119             : 
     120        5697 :     pkt[pkt_bytes++] = byte;
     121             : 
     122        5697 :     if (pkt_len == 0 && pkt_bytes >= 3) {
     123         156 :         pkt_len = (pkt[1]<<8 | pkt[2]) & 0x3ff;
     124         156 :         if (pkt_len == 0) {
     125           7 :             resync();
     126             :         }
     127             :     }
     128             : 
     129        5697 :     if (pkt_len != 0 && pkt_bytes >= pkt_len + 6) {
     130             :         // got header, packet body and parity
     131          28 :         return parse();
     132             :     }
     133             : 
     134             :     // need more bytes
     135        5669 :     return false;
     136             : }
     137             : 
     138             : #ifdef RTCM_MAIN_TEST
     139             : /*
     140             :   parsing test, taking a raw file captured from UART to u-blox F9
     141             :  */
     142             : #include <sys/types.h>
     143             : #include <sys/stat.h>
     144             : #include <fcntl.h>
     145             : #include <stdlib.h>
     146             : #include <unistd.h>
     147             : #include <stdio.h>
     148             : 
     149             : int main(int argc, const char *argv[])
     150             : {
     151             :     const char *fname = argv[1];
     152             :     int fd = ::open(fname, O_RDONLY);
     153             :     if (fd == -1) {
     154             :         perror(fname);
     155             :         ::exit(1);
     156             :     }
     157             :     RTCM3_Parser parser {};
     158             :     uint8_t b;
     159             :     while (::read(fd, &b, 1) == 1) {
     160             :         if (parser.read(b)) {
     161             :             const uint8_t *bytes;
     162             :             printf("packet len %u ID %u\n", parser.get_len(bytes), parser.get_id());
     163             :         }
     164             :     }
     165             :     return 0;
     166             : }
     167             : #endif