rtcm.c

gpsd, a popular GPS daemon.

	    m->loss_warn = (unsigned)csp->los_warning;
	    m->time_unhealthy = (unsigned)(csp->tou / TU_SCALE);
	}
	break;
    case 7:	/* A */
	for (w = 0; w < (RTCM_WORDS_MAX - 2)/ 3; w++) {
	    struct station_t *np = &tp->msg_data.almanac.station[n++];
	    struct b_station_t *mp = &msg->msg_type.type7.almanac[w];

	    mp->w3.lat = (int) round(np->latitude / LA_SCALE);
	    sval = (int) round(np->longitude / LO_SCALE);
	    /*@ -shiftimplementation @*/
	    mp->w3.lon_h = sval >> 8;
	    /*@ +shiftimplementation @*/
	    mp->w4.lon_l = (unsigned)sval & 0xff;
	    mp->w4.range = np->range;
	    uval = (unsigned) round(((np->frequency-FREQ_OFFSET) / FREQ_SCALE));
	    mp->w4.freq_h = uval >> 6;
	    mp->w5.freq_l = uval & 0x3f;
	    mp->w5.health = np->health;
	    mp->w5.station_id = np->station_id;
	    mp->w5.bit_rate = 0;
	    for (uval = 0; uval < (unsigned)(sizeof(tx_speed)/sizeof(tx_speed[0])); uval++)
		if (tx_speed[uval] == np->bitrate) {
		    mp->w5.bit_rate = uval;
		    break;
		}
	    if (mp->w5.bit_rate == 0)
		return false;
	}
	tp->msg_data.almanac.nentries = n;
	break;
    case 16:	/* T */
	/*@ -boolops @*/
	for (w = 0; w < RTCM_WORDS_MAX - 2; w++){
	    if (!tp->msg_data.message[n]) {
		break;
	    }
	    msg->msg_type.type16.txt[w].byte1 = (unsigned)tp->msg_data.message[n++];
	    if (!tp->msg_data.message[n]) {
		break;
	    }
	    msg->msg_type.type16.txt[w].byte2 = (unsigned)tp->msg_data.message[n++];
	    if (!tp->msg_data.message[n]) {
		break;
	    }
	    msg->msg_type.type16.txt[w].byte3 = (unsigned)tp->msg_data.message[n++];
	}
	msg->w2.frmlen = w+1;
	/*@ +boolops @*/
	break;

    default:	/* U */
	memcpy(msg->msg_type.rtcm_msgunk, tp->msg_data.words, (RTCM_WORDS_MAX-2)*sizeof(isgps30bits_t));
	break;
    }

    /* compute parity for each word in the message */
    for (w = 0; w < tp->length; w++)
	wp[w].parity = isgps_parity(buf[w]);

    /* FIXME: must do inversion here */
    return true;
}

static bool preamble_match(isgps30bits_t *w)
{
    return (((struct rtcm_msghw1 *)w)->preamble == PREAMBLE_PATTERN);
}

static bool length_check(struct gps_packet_t *lexer)
{
    return lexer->isgps.bufindex >= 2 
	&& lexer->isgps.bufindex >= ((struct rtcm_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u;
}

enum isgpsstat_t rtcm_decode(struct gps_packet_t *lexer, unsigned int c)
{
    return isgps_decode(lexer, 
			preamble_match, 
			length_check, 
			RTCM_WORDS_MAX, 
			c);
}

void rtcm_dump(struct rtcm_t *rtcm, /*@out@*/char buf[], size_t buflen)
/* dump the contents of a parsed RTCM104 message */
{
    unsigned int n;

    (void)snprintf(buf, buflen, "H\t%u\t%u\t%0.1f\t%u\t%u\t%u\n",
	   rtcm->type,
	   rtcm->refstaid,
	   rtcm->zcount,
	   rtcm->seqnum,
	   rtcm->length,
	   rtcm->stathlth);

    switch (rtcm->type) {
    case 1:
    case 9:
	for (n = 0; n < rtcm->msg_data.ranges.nentries; n++) {
	    struct rangesat_t *rsp = &rtcm->msg_data.ranges.sat[n];
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "S\t%u\t%u\t%u\t%0.1f\t%0.3f\t%0.3f\n",
			   rsp->ident,
			   rsp->udre,
			   rsp->issuedata,
			   rtcm->zcount,
			   rsp->rangerr,
			   rsp->rangerate);
	}
	break;

    case 3:
	if (rtcm->msg_data.ecef.valid)
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "R\t%.2f\t%.2f\t%.2f\n",
			   rtcm->msg_data.ecef.x, 
			   rtcm->msg_data.ecef.y,
			   rtcm->msg_data.ecef.z);
	break;

    case 4:
	if (rtcm->msg_data.reference.valid)
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "D\t%s\t%1d\t%s\t%.1f\t%.1f\t%.1f\n",
			   (rtcm->msg_data.reference.system==gps) ? "GPS"
			   : ((rtcm->msg_data.reference.system==glonass) ? "GLONASS"
			      : "UNKNOWN"),
			   rtcm->msg_data.reference.sense,
			   rtcm->msg_data.reference.datum,
			   rtcm->msg_data.reference.dx,
			   rtcm->msg_data.reference.dy,
			   rtcm->msg_data.reference.dz);
	break;

    case 5:
	for (n = 0; n < rtcm->msg_data.conhealth.nentries; n++) {
	    struct consat_t *csp = &rtcm->msg_data.conhealth.sat[n];
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n",
			   csp->ident,
			   (unsigned)csp->iodl,
			   (unsigned)csp->health,
			   csp->snr,
			   (unsigned)csp->health_en,
			   (unsigned)csp->new_data,
			   (unsigned)csp->los_warning,
			   csp->tou);
	}
	break;

    case 6: 			/* NOP msg */
	(void)strlcat(buf, "N\n", buflen);
	break;

    case 7:
	for (n = 0; n < rtcm->msg_data.almanac.nentries; n++) {
	    struct station_t *ssp = &rtcm->msg_data.almanac.station[n];
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "A\t%.4f\t%.4f\t%u\t%.1f\t%u\t%u\t%u\n",
			   ssp->latitude,
			   ssp->longitude,
			   ssp->range,
			   ssp->frequency,
			   ssp->health,
			   ssp->station_id,
			   ssp->bitrate);
	}
	break;
    case 16:
	(void)snprintf(buf + strlen(buf), buflen - strlen(buf),
		       "T\t\"%s\"\n", rtcm->msg_data.message);
	break;

    default:
	for (n = 0; n < rtcm->length; n++)
	    (void)snprintf(buf + strlen(buf), buflen - strlen(buf),
			   "U\t0x%08x\n", rtcm->msg_data.words[n]);
	break;
    }

}

int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf)
/* merge a line of data into an RTCM structure, return 0 if done */
{
    int fldcount, v;
    unsigned n;
    char buf2[BUFSIZ]; /* stdio.h says BUFSIZ=1024. True everywhere? */

    /*@ -usedef @*/
    switch (rtcmp->type) {
    case 0:
	fldcount = sscanf(buf, "H\t%u\t%u\t%lf\t%u\t%u\t%u\n",
			&rtcmp->type,
			&rtcmp->refstaid,
			&rtcmp->zcount,
			&rtcmp->seqnum,
			&rtcmp->length,
			&rtcmp->stathlth);
	if (fldcount != 6)
	    return -1;
	else
	    return 1;
	//break;

    case 1:
    case 9:
	{
	    struct rangesat_t *rsp = &rtcmp->msg_data.ranges.sat[rtcmp->msg_data.ranges.nentries++];
	    /* we ignore the third (zcount) field, it's in the parent */
	    fldcount = sscanf(buf,
			      "S\t%u\t%u\t%u\t%*f\t%lf\t%lf\n",
			      &rsp->ident,
			      &rsp->udre,
			      &rsp->issuedata,
			      &rsp->rangerr,
			      &rsp->rangerate);
	    if (fldcount != 5 || (rtcmp->type != 1 && rtcmp->type != 9))
		return (int)(-rtcmp->type-1);
	    else if (rtcmp->msg_data.ranges.nentries != rtcmp->length*3/5)
		return (int)(rtcmp->type+1);
	    else
		return 0;
	}
	//break;

    case 3:
	fldcount = sscanf(buf,
			  "R\t%lf\t%lf\t%lf\n",
			  &rtcmp->msg_data.ecef.x, 
			  &rtcmp->msg_data.ecef.y,
			  &rtcmp->msg_data.ecef.z);
	if (fldcount != 3 || rtcmp->type != 3)
	    return -4;
	else {
	    rtcmp->msg_data.ecef.valid = true;
	    return 0;
	}
	//break;

    case 4:
	fldcount = sscanf(buf,
			   "D\t%1023s\t%1d\t%5s\t%lf\t%lf\t%lf\n",
			  buf2,
			  &v,
			  (char *)&rtcmp->msg_data.reference.datum,
			  &rtcmp->msg_data.reference.dx,
			  &rtcmp->msg_data.reference.dy,
			  &rtcmp->msg_data.reference.dz);
	if (fldcount != 6 || rtcmp->type != 4)
	    return -5;
	else {
	    if (strcmp(buf2, "GPS") == 0)
		rtcmp->msg_data.reference.system = gps;
	    else if (strcmp(buf2, "GLONASS") == 0)
		rtcmp->msg_data.reference.system = glonass;
	    else
		rtcmp->msg_data.reference.system = unknown;
	    rtcmp->msg_data.reference.sense = (v == 1) ? global : ((v == 0) ? local : invalid);
	    rtcmp->msg_data.reference.valid = true;
	    return 0;
	}
	//break;

    case 5:
	{
	    struct consat_t *csp = &rtcmp->msg_data.conhealth.sat[rtcmp->msg_data.conhealth.nentries++];
	    unsigned int iodl, new_data, los_warning;

	    fldcount = sscanf(buf,
			      "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n",
			      &csp->ident,
			      &iodl,
			      &csp->health,
			      &csp->snr,
			      &csp->health_en,
			      &new_data,
			      &los_warning,
			      &csp->tou);
	    csp->iodl = iodl > 0;
	    csp->new_data = new_data > 0;
	    csp->los_warning = los_warning > 0;
	    if (fldcount != 8 || rtcmp->type != 5)
		return -6;
	    else if (rtcmp->msg_data.conhealth.nentries < rtcmp->length)
		return 6;
	    else
		return 0;
	}
	//break;

    case 6: 			/* NOP msg */
	if (buf[0] != 'N')
	    return -7;
	else
	    return 0;
	//break;

    case 7:
	{
	    struct station_t *ssp = &rtcmp->msg_data.almanac.station[rtcmp->msg_data.almanac.nentries++];
	    fldcount = sscanf(buf,
			      "A\t%lf\t%lf\t%u\t%lf\t%u\t%u\t%u\n",
			      &ssp->latitude,
			      &ssp->longitude,
			      &ssp->range,
			      &ssp->frequency,
			      &ssp->health,
			      &ssp->station_id,
			      &ssp->bitrate);
	    if (fldcount != 7 || rtcmp->type != 7)
		return 8;
	    else if (rtcmp->msg_data.almanac.nentries < rtcmp->length/3)
		return 8;
	    else
		return 0;
	}
	//break;

    case 16:
	fldcount = sscanf(buf, "T\t\"%[^\"]\"\n", rtcmp->msg_data.message);
	if (fldcount != 1)
	    return 16;
	else
	    return 0;
	//break;

    default:
	for (n = 0; n < DIMENSION(rtcmp->msg_data.words); n++)
	    if (rtcmp->msg_data.words[n] == 0)
		break;
	if (n >= DIMENSION(rtcmp->msg_data.words))
	    return 0;
	else {
	    fldcount = sscanf(buf, "U\t0x%08x\n", &v);
	    if (fldcount != 1)
		return (int)(-rtcmp->type-1);
	    else {
		rtcmp->msg_data.words[n] = (isgps30bits_t)v;
		if (n == rtcmp->length-1)
		    return 0;
		else
		    return (int)(rtcmp->type+1);
	    }
	}
	//break;
    }
    /*@ +usedef @*/
}

#ifdef __UNUSED__
void rtcm_output_magnavox(isgps30bits_t *ip, FILE *fp)
/* ship an RTCM message in the format emitted by Magnavox DGPS receivers */
{
    static uint     sqnum = 0;

    ((struct rtcm_msg_t *) ip)->w2.sqnum = sqnum++;
    sqnum &= 0x7;

    isgps_output_magnavox(ip, ((struct rtcm_msg_t *) ip)->w2.frmlen + 2, fp);
}
#endif /* __UNUSED__ */

#endif /* RTCM104_ENABLE */