driver_rtcm2.c

   1 /*****************************************************************************
   2
   3 This is a decoder for RTCM-104 2.x, an obscure and complicated serial
   4 protocol used for broadcasting pseudorange corrections from
   5 differential-GPS reference stations.  The applicable
   6 standard is
   7
   8 RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE,
   9 RTCM PAPER 194-93/SC 104-STD
  10
  11 Ordering instructions are accessible from <http://www.rtcm.org/>
  12 under "Publications".  This describes version 2.1 of the RTCM specification.
  13 RTCM-104 was later incrementally revised up to a 2.3 level before being
  14 completely redesigned as level 3.0.
  15
  16 Also applicable is ITU-R M.823: "Technical characteristics of
  17 differential transmissions for global navigation satellite systems
  18 from maritime radio beacons in the frequency band 283.5 - 315 kHz in
  19 region 1 and 285 - 325 kHz in regions 2 & 3."
  20
  21 The RTCM 2.x protocol uses as a transport layer the GPS satellite downlink
  22 protocol described in IS-GPS-200, the Navstar GPS Interface
  23 Specification.  This code relies on the lower-level packet-assembly
  24 code for that protocol in isgps.c.
  25
  26 The lower layer's job is done when it has assembled a message of up to
  27 33 words of clean parity-checked data.  At this point this upper layer
  28 takes over.  struct rtcm2_msg_t is overlaid on the buffer and the bitfields
  29 are used to extract pieces of it.  Those pieces are copied and (where
  30 necessary) reassembled into a struct rtcm2_t.
  31
  32 This code and the contents of isgps.c are evolved from code by Wolfgang
  33 Rupprecht.  Wolfgang's decoder was loosely based on one written by
  34 John Sager in 1999 (in particular the dump function emits a close
  35 descendant of Sager's dump format).  Here are John Sager's original
  36 notes:
  37
  38 The RTCM decoder prints a legible representation of the input data.
  39 The RTCM SC-104 specification is copyrighted, so I cannot
  40 quote it - in fact, I have never read it! Most of the information
  41 used to develop the decoder came from publication ITU-R M.823.
  42 This is a specification of the data transmitted from LF DGPS
  43 beacons in the 300kHz band. M.823 contains most of those parts of
  44 RTCM SC-104 directly relevant to the air interface (there
  45 are one or two annoying and vital omissions!). Information
  46 about the serial interface format was gleaned from studying
  47 the output of a beacon receiver test program made available on
  48 Starlink's website.
  49
  50 This file is Copyright (c) 2010 by the GPSD project
  51 BSD terms apply: see the file COPYING in the distribution root for details.
  52
  53 *****************************************************************************/
  54
  55 #include <sys/types.h>
  56 #ifndef S_SPLINT_S
  57 #include <unistd.h>
  58 #endif /* S_SPLINT_S */
  59 #include <stdlib.h>
  60 #include <string.h>
  61 #include <stdbool.h>
  62 #include <stdio.h>
  63 #include <math.h>               /* for round() */
  64
  65 #include "gpsd.h"
  66 #include "driver_rtcm2.h"
  67
  68 #ifdef RTCM104V2_ENABLE
  69
  70 #define PREAMBLE_PATTERN 0x66
  71
  72 static unsigned int tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 };
  73
  74 #define DIMENSION(a) (unsigned)(sizeof(a)/sizeof(a[0]))
  75
  76 void rtcm2_unpack( /*@out@*/ struct rtcm2_t *tp, char *buf)
  77 /* break out the raw bits into the content fields */
  78 {
  79     int len;
  80     unsigned int n, w;
  81     struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf;
  82
  83     tp->type = msg->w1.msgtype;
  84     tp->length = msg->w2.frmlen;
  85     tp->zcount = msg->w2.zcnt * ZCOUNT_SCALE;
  86     tp->refstaid = msg->w1.refstaid;
  87     tp->seqnum = msg->w2.sqnum;
  88     tp->stathlth = msg->w2.stathlth;
  89
  90     len = (int)tp->length;
  91     n = 0;
  92     switch (tp->type) {
  93     case 1:
  94     case 9:
  95     {
  96         struct b_correction_t *m = &msg->msg_type.type1.corrections[0];
  97
  98         while (len >= 0) {
  99             if (len >= 2) {
 100                 tp->ranges.sat[n].ident = m->w3.satident1;
 101                 tp->ranges.sat[n].udre = m->w3.udre1;
 102                 tp->ranges.sat[n].issuedata = m->w4.issuedata1;
 103                 tp->ranges.sat[n].rangerr = m->w3.pc1 *
 104                     (m->w3.scale1 ? PCLARGE : PCSMALL);
 105                 tp->ranges.sat[n].rangerate = m->w4.rangerate1 *
 106                     (m->w3.scale1 ? RRLARGE : RRSMALL);
 107                 n++;
 108             }
 109             if (len >= 4) {
 110                 tp->ranges.sat[n].ident = m->w4.satident2;
 111                 tp->ranges.sat[n].udre = m->w4.udre2;
 112                 tp->ranges.sat[n].issuedata = m->w6.issuedata2;
 113                 tp->ranges.sat[n].rangerr = m->w5.pc2 *
 114                     (m->w4.scale2 ? PCLARGE : PCSMALL);
 115                 tp->ranges.sat[n].rangerate = m->w5.rangerate2 *
 116                     (m->w4.scale2 ? RRLARGE : RRSMALL);
 117                 n++;
 118             }
 119             if (len >= 5) {
 120                 tp->ranges.sat[n].ident = m->w6.satident3;
 121                 tp->ranges.sat[n].udre = m->w6.udre3;
 122                 tp->ranges.sat[n].issuedata = m->w7.issuedata3;
 123                 /*@ -shiftimplementation @*/
 124                 tp->ranges.sat[n].rangerr =
 125                     ((m->w6.pc3_h << 8) | (m->w7.pc3_l)) *
 126                     (m->w6.scale3 ? PCLARGE : PCSMALL);
 127                 tp->ranges.sat[n].rangerate =
 128                     m->w7.rangerate3 * (m->w6.scale3 ? RRLARGE : RRSMALL);
 129                 /*@ +shiftimplementation @*/
 130                 n++;
 131             }
 132             len -= 5;
 133             m++;
 134         }
 135         tp->ranges.nentries = n;
 136     }
 137         break;
 138     case 3:
 139     {
 140         struct rtcm2_msg3 *m = &msg->msg_type.type3;
 141
 142         if ((tp->ecef.valid = len >= 4)) {
 143             tp->ecef.x = ((m->w3.x_h << 8) | (m->w4.x_l)) * XYZ_SCALE;
 144             tp->ecef.y = ((m->w4.y_h << 16) | (m->w5.y_l)) * XYZ_SCALE;
 145             tp->ecef.z = ((m->w5.z_h << 24) | (m->w6.z_l)) * XYZ_SCALE;
 146         }
 147     }
 148         break;
 149     case 4:
 150         if ((tp->reference.valid = len >= 2)) {
 151             struct rtcm2_msg4 *m = &msg->msg_type.type4;
 152
 153             tp->reference.system =
 154                 (m->w3.dgnss == 0) ? NAVSYSTEM_GPS :
 155                 ((m->w3.dgnss == 1) ? NAVSYSTEM_GLONASS : NAVSYSTEM_UNKNOWN);
 156             tp->reference.sense =
 157                 (m->w3.dat != 0) ? SENSE_GLOBAL : SENSE_LOCAL;
 158             if (m->w3.datum_alpha_char1) {
 159                 tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char1);
 160             }
 161             if (m->w3.datum_alpha_char2) {
 162                 tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char2);
 163             }
 164             if (m->w4.datum_sub_div_char1) {
 165                 tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char1);
 166             }
 167             if (m->w4.datum_sub_div_char2) {
 168                 tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char2);
 169             }
 170             if (m->w4.datum_sub_div_char3) {
 171                 tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char3);
 172             }
 173             tp->reference.datum[n++] = '\0';
 174             if (len >= 4) {
 175                 tp->reference.dx = m->w5.dx * DXYZ_SCALE;
 176                 tp->reference.dy =
 177                     ((m->w5.dy_h << 8) | m->w6.dy_l) * DXYZ_SCALE;
 178                 tp->reference.dz = m->w6.dz * DXYZ_SCALE;
 179             } else
 180                 tp->reference.sense = SENSE_INVALID;
 181         }
 182         break;
 183     case 5:
 184         for (n = 0; n < (unsigned)len; n++) {
 185             struct consat_t *csp = &tp->conhealth.sat[n];
 186             struct b_health_t *m = &msg->msg_type.type5.health[n];
 187
 188             csp->ident = m->sat_id;
 189             csp->iodl = m->issue_of_data_link != 0;
 190             csp->health = m->data_health;
 191             /*@+ignoresigns@*/
 192             csp->snr = (int)(m->cn0 ? (m->cn0 + CNR_OFFSET) : SNR_BAD);
 193             /*@-ignoresigns@*/
 194             csp->health_en = m->health_enable != 0;
 195             csp->new_data = m->new_nav_data != 0;
 196             csp->los_warning = m->loss_warn != 0;
 197             csp->tou = m->time_unhealthy * TU_SCALE;
 198         }
 199         tp->conhealth.nentries = n;
 200         break;
 201     case 7:
 202         for (w = 0; w < (unsigned)len; w++) {
 203             struct station_t *np = &tp->almanac.station[n];
 204             struct b_station_t *mp = &msg->msg_type.type7.almanac[w];
 205
 206             np->latitude = mp->w3.lat * LA_SCALE;
 207             /*@-shiftimplementation@*/
 208             np->longitude = ((mp->w3.lon_h << 8) | mp->w4.lon_l) * LO_SCALE;
 209             /*@+shiftimplementation@*/
 210             np->range = mp->w4.range;
 211             np->frequency =
 212                 (((mp->w4.freq_h << 6) | mp->w5.freq_l) * FREQ_SCALE) +
 213                 FREQ_OFFSET;
 214             np->health = mp->w5.health;
 215             np->station_id = mp->w5.station_id,
 216                 np->bitrate = tx_speed[mp->w5.bit_rate];
 217             n++;
 218         }
 219         tp->almanac.nentries = (unsigned)(len / 3);
 220         break;
 221     case 16:
 222         /*@ -boolops @*/
 223         for (w = 0; w < (unsigned)len; w++) {
 224             if (!msg->msg_type.type16.txt[w].byte1) {
 225                 break;
 226             }
 227             tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte1);
 228             if (!msg->msg_type.type16.txt[w].byte2) {
 229                 break;
 230             }
 231             tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte2);
 232             if (!msg->msg_type.type16.txt[w].byte3) {
 233                 break;
 234             }
 235             tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte3);
 236         }
 237         /*@ +boolops @*/
 238         tp->message[n++] = '\0';
 239         break;
 240
 241     default:
 242         memcpy(tp->words, msg->msg_type.rtcm2_msgunk,
 243                (RTCM2_WORDS_MAX - 2) * sizeof(isgps30bits_t));
 244         break;
 245     }
 246 }
 247
 248 static bool preamble_match(isgps30bits_t * w)
 249 {
 250     return (((struct rtcm2_msghw1 *)w)->preamble == PREAMBLE_PATTERN);
 251 }
 252
 253 static bool length_check(struct gps_packet_t *lexer)
 254 {
 255     return lexer->isgps.bufindex >= 2
 256         && lexer->isgps.bufindex >=
 257         ((struct rtcm2_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u;
 258 }
 259
 260 enum isgpsstat_t rtcm2_decode(struct gps_packet_t *lexer, unsigned int c)
 261 {
 262     return isgps_decode(lexer,
 263                         preamble_match, length_check, RTCM2_WORDS_MAX, c);
 264 }
 265
 266 void rtcm2_sager_dump(const struct rtcm2_t *rtcm, /*@out@*/ char buf[],
 267                       size_t buflen)
 268 /* dump the contents of a parsed RTCM104 message */
 269 {
 270     unsigned int n;
 271
 272     (void)snprintf(buf, buflen, "H\t%u\t%u\t%0.1f\t%u\t%u\t%u\n",
 273                    rtcm->type,
 274                    rtcm->refstaid,
 275                    rtcm->zcount, rtcm->seqnum, rtcm->length, rtcm->stathlth);
 276
 277     switch (rtcm->type) {
 278     case 1:
 279     case 9:
 280         for (n = 0; n < rtcm->ranges.nentries; n++) {
 281             const struct rangesat_t *rsp = &rtcm->ranges.sat[n];
 282             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 283                            "S\t%u\t%u\t%u\t%0.1f\t%0.3f\t%0.3f\n",
 284                            rsp->ident,
 285                            rsp->udre,
 286                            rsp->issuedata,
 287                            rtcm->zcount, rsp->rangerr, rsp->rangerate);
 288         }
 289         break;
 290
 291     case 3:
 292         if (rtcm->ecef.valid)
 293             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 294                            "R\t%.2f\t%.2f\t%.2f\n",
 295                            rtcm->ecef.x, rtcm->ecef.y, rtcm->ecef.z);
 296         break;
 297
 298     case 4:
 299         if (rtcm->reference.valid)
 300             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 301                            "D\t%s\t%1d\t%s\t%.1f\t%.1f\t%.1f\n",
 302                            (rtcm->reference.system == NAVSYSTEM_GPS) ? "GPS"
 303                            : ((rtcm->reference.system ==
 304                                NAVSYSTEM_GLONASS) ? "GLONASS" : "UNKNOWN"),
 305                            rtcm->reference.sense, rtcm->reference.datum,
 306                            rtcm->reference.dx, rtcm->reference.dy,
 307                            rtcm->reference.dz);
 308         break;
 309
 310     case 5:
 311         for (n = 0; n < rtcm->conhealth.nentries; n++) {
 312             const struct consat_t *csp = &rtcm->conhealth.sat[n];
 313             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 314                            "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n",
 315                            csp->ident,
 316                            (unsigned)csp->iodl,
 317                            (unsigned)csp->health,
 318                            csp->snr,
 319                            (unsigned)csp->health_en,
 320                            (unsigned)csp->new_data,
 321                            (unsigned)csp->los_warning, csp->tou);
 322         }
 323         break;
 324
 325     case 6:                     /* NOP msg */
 326         (void)strlcat(buf, "N\n", buflen);
 327         break;
 328
 329     case 7:
 330         for (n = 0; n < rtcm->almanac.nentries; n++) {
 331             const struct station_t *ssp = &rtcm->almanac.station[n];
 332             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 333                            "A\t%.4f\t%.4f\t%u\t%.1f\t%u\t%u\t%u\n",
 334                            ssp->latitude,
 335                            ssp->longitude,
 336                            ssp->range,
 337                            ssp->frequency,
 338                            ssp->health, ssp->station_id, ssp->bitrate);
 339         }
 340         break;
 341     case 16:
 342         (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 343                        "T\t\"%s\"\n", rtcm->message);
 344         break;
 345
 346     default:
 347         for (n = 0; n < rtcm->length; n++)
 348             (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
 349                            "U\t0x%08x\n", rtcm->words[n]);
 350         break;
 351     }
 352
 353     (void)strlcat(buf, ".\n", buflen);
 354 }
 355
 356 #endif /* RTCM104V2_ENABLE */