2017-05-12 06:55:59 +00:00
|
|
|
/*!
|
|
|
|
* \file rtklib_rtcm2.cc
|
|
|
|
* \brief RTCM functions for v2
|
|
|
|
* \authors <ul>
|
|
|
|
* <li> 2007-2013, T. Takasu
|
|
|
|
* <li> 2017, Javier Arribas
|
|
|
|
* <li> 2017, Carles Fernandez
|
|
|
|
* </ul>
|
|
|
|
*
|
|
|
|
* This is a derived work from RTKLIB http://www.rtklib.com/
|
|
|
|
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
|
|
|
* released under the BSD 2-clause license with an additional exclusive clause
|
|
|
|
* that does not apply here. This additional clause is reproduced below:
|
|
|
|
*
|
|
|
|
* " The software package includes some companion executive binaries or shared
|
|
|
|
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
|
|
|
* original ones of these software. "
|
|
|
|
*
|
|
|
|
* Neither the executive binaries nor the shared libraries are required by, used
|
|
|
|
* or included in GNSS-SDR.
|
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
* Copyright (C) 2007-2013, T. Takasu
|
|
|
|
* Copyright (C) 2017, Javier Arribas
|
|
|
|
* Copyright (C) 2017, Carles Fernandez
|
|
|
|
* All rights reserved.
|
|
|
|
*
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
* modification, are permitted provided that the following conditions are
|
|
|
|
* met:
|
|
|
|
*
|
|
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
*
|
|
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
|
|
* documentation and/or other materials provided with the distribution.
|
|
|
|
*
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
|
|
|
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
|
|
|
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
|
|
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
|
|
|
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
|
|
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
|
|
|
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
|
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
|
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*
|
|
|
|
*
|
|
|
|
*----------------------------------------------------------------------------*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include "rtklib_rtcm2.h"
|
|
|
|
#include "rtklib_rtkcmn.h"
|
|
|
|
|
|
|
|
|
|
|
|
/* adjust hourly rollover of rtcm 2 time -------------------------------------*/
|
|
|
|
void adjhour(rtcm_t *rtcm, double zcnt)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
double tow, hour, sec;
|
2017-05-12 06:55:59 +00:00
|
|
|
int week;
|
|
|
|
|
|
|
|
/* if no time, get cpu time */
|
|
|
|
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
|
2017-05-13 18:01:08 +00:00
|
|
|
tow = time2gpst(rtcm->time, &week);
|
2017-05-12 06:55:59 +00:00
|
|
|
hour = floor(tow/3600.0);
|
|
|
|
sec = tow-hour*3600.0;
|
|
|
|
if (zcnt<sec-1800.0) zcnt += 3600.0;
|
|
|
|
else if (zcnt>sec+1800.0) zcnt-=3600.0;
|
2017-05-13 18:01:08 +00:00
|
|
|
rtcm->time = gpst2time(week, hour*3600+zcnt);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* get observation data index ------------------------------------------------*/
|
|
|
|
int obsindex(obs_t *obs, gtime_t time, int sat)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
int i, j;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
for (i = 0; i<obs->n; i++)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
if (obs->data[i].sat == sat) return i; /* field already exists */
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i >= MAXOBS) return -1; /* overflow */
|
2017-05-12 06:55:59 +00:00
|
|
|
|
|
|
|
/* add new field */
|
|
|
|
obs->data[i].time = time;
|
|
|
|
obs->data[i].sat = sat;
|
2017-05-13 18:01:08 +00:00
|
|
|
for (j = 0; j<NFREQ; j++)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
obs->data[i].L[j] = obs->data[i].P[j] = 0.0;
|
|
|
|
obs->data[i].D[j] = 0.0;
|
|
|
|
obs->data[i].SNR[j] = obs->data[i].LLI[j] = obs->data[i].code[j] = 0;
|
|
|
|
}
|
|
|
|
obs->n++;
|
|
|
|
return i;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 1/9: differential gps correction/partial correction set -------*/
|
|
|
|
int decode_type1(rtcm_t *rtcm)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
int i = 48, fact, udre, prn, sat, iod;
|
|
|
|
double prc, rrc;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type1: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
while (i+40 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
fact = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
udre = getbitu(rtcm->buff, i, 2); i += 2;
|
|
|
|
prn = getbitu(rtcm->buff, i, 5); i += 5;
|
|
|
|
prc = getbits(rtcm->buff, i, 16); i += 16;
|
|
|
|
rrc = getbits(rtcm->buff, i, 8); i += 8;
|
|
|
|
iod = getbits(rtcm->buff, i, 8); i += 8;
|
2017-05-12 06:55:59 +00:00
|
|
|
if (prn == 0) prn = 32;
|
|
|
|
if (prc == 0x80000000||rrc == 0xFFFF8000)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n", prn);
|
2017-05-12 06:55:59 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (rtcm->dgps)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
sat = satno(SYS_GPS, prn);
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->dgps[sat-1].t0 = rtcm->time;
|
|
|
|
rtcm->dgps[sat-1].prc = prc*(fact?0.32:0.02);
|
|
|
|
rtcm->dgps[sat-1].rrc = rrc*(fact?0.032:0.002);
|
|
|
|
rtcm->dgps[sat-1].iod = iod;
|
|
|
|
rtcm->dgps[sat-1].udre = udre;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return 7;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 3: reference station parameter --------------------------------*/
|
|
|
|
int decode_type3(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
int i = 48;
|
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type3: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+96 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
rtcm->sta.pos[0] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
|
|
|
|
rtcm->sta.pos[1] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
|
|
|
|
rtcm->sta.pos[2] = getbits(rtcm->buff, i, 32)*0.01;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 3 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 14: gps time of week ------------------------------------------*/
|
|
|
|
int decode_type14(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
double zcnt;
|
2017-05-13 18:01:08 +00:00
|
|
|
int i = 48, week, hour, leaps;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type14: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
zcnt = getbitu(rtcm->buff, 24, 13);
|
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
week = getbitu(rtcm->buff, i, 10); i += 10;
|
|
|
|
hour = getbitu(rtcm->buff, i, 8); i += 8;
|
|
|
|
leaps = getbitu(rtcm->buff, i, 6);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 14 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
week = adjgpsweek(week);
|
2017-05-13 18:01:08 +00:00
|
|
|
rtcm->time = gpst2time(week, hour*3600.0+zcnt*0.6);
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->nav.leaps = leaps;
|
|
|
|
return 6;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 16: gps special message ---------------------------------------*/
|
|
|
|
int decode_type16(rtcm_t *rtcm)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
int i = 48, n = 0;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type16: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
while (i+8 <= rtcm->len*8 && n<90)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
rtcm->msg[n++] = getbitu(rtcm->buff, i, 8); i += 8;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
rtcm->msg[n] = '\0';
|
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(3, "rtcm2 16 message: %s\n", rtcm->msg);
|
2017-05-12 06:55:59 +00:00
|
|
|
return 9;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 17: gps ephemerides -------------------------------------------*/
|
|
|
|
int decode_type17(rtcm_t *rtcm)
|
|
|
|
{
|
2017-05-13 18:25:38 +00:00
|
|
|
eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
|
|
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
|
2017-05-13 18:01:08 +00:00
|
|
|
double toc, sqrtA;
|
|
|
|
int i = 48, week, prn, sat;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type17: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+480 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
week = getbitu(rtcm->buff, i, 10); i += 10;
|
|
|
|
eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14;
|
|
|
|
eph.iode = getbitu(rtcm->buff, i, 8); i += 8;
|
|
|
|
toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16;
|
|
|
|
eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16;
|
|
|
|
eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8;
|
|
|
|
eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
|
|
|
|
eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16;
|
|
|
|
eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
|
|
|
|
eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32;
|
|
|
|
eph.cus = getbits(rtcm->buff, i, 16); i += 16;
|
|
|
|
sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32;
|
|
|
|
eph.toes = getbitu(rtcm->buff, i, 16); i += 16;
|
|
|
|
eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
|
|
|
|
eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
|
|
|
|
eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
|
|
|
|
eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
|
|
|
|
eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
|
|
|
|
eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
|
|
|
|
eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24;
|
|
|
|
eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
|
|
|
|
eph.iodc = getbitu(rtcm->buff, i, 10); i += 10;
|
|
|
|
eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22;
|
|
|
|
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
|
|
|
|
eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8;
|
|
|
|
eph.code = getbitu(rtcm->buff, i, 2); i += 2;
|
|
|
|
eph.sva = getbitu(rtcm->buff, i, 4); i += 4;
|
|
|
|
eph.svh = getbitu(rtcm->buff, i, 6); i += 6;
|
|
|
|
eph.flag = getbitu(rtcm->buff, i, 1);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 17 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
if (prn == 0) prn = 32;
|
2017-05-13 18:01:08 +00:00
|
|
|
sat = satno(SYS_GPS, prn);
|
2017-05-12 06:55:59 +00:00
|
|
|
eph.sat = sat;
|
|
|
|
eph.week = adjgpsweek(week);
|
2017-05-13 18:01:08 +00:00
|
|
|
eph.toe = gpst2time(eph.week, eph.toes);
|
|
|
|
eph.toc = gpst2time(eph.week, toc);
|
2017-05-12 06:55:59 +00:00
|
|
|
eph.ttr = rtcm->time;
|
|
|
|
eph.A = sqrtA*sqrtA;
|
|
|
|
rtcm->nav.eph[sat-1] = eph;
|
|
|
|
rtcm->ephsat = sat;
|
|
|
|
return 2;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
|
|
|
|
int decode_type18(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
gtime_t time;
|
2017-05-13 18:01:08 +00:00
|
|
|
double usec, cp, tt;
|
|
|
|
int i = 48, index, freq, sync = 1, code, sys, prn, sat, loss;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type18: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
|
|
|
|
usec = getbitu(rtcm->buff, i, 20); i += 20;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 18 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
if (freq&0x1)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 18 not supported frequency: freq=%d\n", freq);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
freq>>= 1;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
sync = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
code = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
sys = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
|
|
|
|
loss = getbitu(rtcm->buff, i, 5); i += 5;
|
|
|
|
cp = getbits(rtcm->buff, i, 32); i += 32;
|
2017-05-12 06:55:59 +00:00
|
|
|
if (prn == 0) prn = 32;
|
2017-05-13 18:01:08 +00:00
|
|
|
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 18 satellite number error: sys=%d prn=%d\n", sys, prn);
|
2017-05-12 06:55:59 +00:00
|
|
|
continue;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
time = timeadd(rtcm->time, usec*1E-6);
|
2017-05-12 06:55:59 +00:00
|
|
|
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
|
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
tt = timediff(rtcm->obs.data[0].time, time);
|
2017-05-12 06:55:59 +00:00
|
|
|
if (rtcm->obsflag||fabs(tt)>1E-9)
|
|
|
|
{
|
|
|
|
rtcm->obs.n = rtcm->obsflag = 0;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
rtcm->obs.data[index].L[freq] = -cp/256.0;
|
2017-05-13 18:01:08 +00:00
|
|
|
rtcm->obs.data[index].LLI[freq] = rtcm->loss[sat-1][freq] != loss;
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->obs.data[index].code[freq] =
|
|
|
|
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
|
|
|
|
rtcm->loss[sat-1][freq] = loss;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
rtcm->obsflag = !sync;
|
|
|
|
return sync?0:1;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 19: rtk uncorrected pseudorange -------------------------------*/
|
|
|
|
int decode_type19(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
gtime_t time;
|
2017-05-13 18:01:08 +00:00
|
|
|
double usec, pr, tt;
|
|
|
|
int i = 48, index, freq, sync = 1, code, sys, prn, sat;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type19: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
|
|
|
|
usec = getbitu(rtcm->buff, i, 20); i += 20;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 19 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
if (freq&0x1)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 19 not supported frequency: freq=%d\n", freq);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
freq>>=1;
|
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
sync = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
code = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
sys = getbitu(rtcm->buff, i, 1); i += 1;
|
|
|
|
prn = getbitu(rtcm->buff, i, 5); i += 5+8;
|
|
|
|
pr = getbitu(rtcm->buff, i, 32); i += 32;
|
2017-05-12 06:55:59 +00:00
|
|
|
if (prn == 0) prn = 32;
|
2017-05-13 18:01:08 +00:00
|
|
|
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 19 satellite number error: sys=%d prn=%d\n", sys, prn);
|
2017-05-12 06:55:59 +00:00
|
|
|
continue;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
time = timeadd(rtcm->time, usec*1E-6);
|
2017-05-12 06:55:59 +00:00
|
|
|
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
|
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
tt = timediff(rtcm->obs.data[0].time, time);
|
2017-05-12 06:55:59 +00:00
|
|
|
if (rtcm->obsflag||fabs(tt)>1E-9)
|
|
|
|
{
|
|
|
|
rtcm->obs.n = rtcm->obsflag = 0;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
rtcm->obs.data[index].P[freq] = pr*0.02;
|
|
|
|
rtcm->obs.data[index].code[freq] =
|
|
|
|
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
rtcm->obsflag = !sync;
|
|
|
|
return sync?0:1;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 22: extended reference station parameter ----------------------*/
|
|
|
|
int decode_type22(rtcm_t *rtcm)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
double del[2][3] = {{0}}, hgt = 0.0;
|
|
|
|
int i = 48, j, noh;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(4, "decode_type22: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
del[0][0] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
|
|
|
|
del[0][1] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
|
|
|
|
del[0][2] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 22 length error: len=%d\n", rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
i += 5; noh = getbits(rtcm->buff, i, 1); i += 1;
|
|
|
|
hgt = noh?0.0:getbitu(rtcm->buff, i, 18)/25600.0;
|
2017-05-12 06:55:59 +00:00
|
|
|
i += 18;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if (i+24 <= rtcm->len*8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
del[1][0] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
|
|
|
|
del[1][1] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
|
|
|
|
del[1][2] = getbits(rtcm->buff, i, 8)/1600.0;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
rtcm->sta.deltype = 1; /* xyz */
|
2017-05-13 18:01:08 +00:00
|
|
|
for (j = 0; j<3; j++) rtcm->sta.del[j] = del[0][j];
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->sta.hgt = hgt;
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 23: antenna type definition record ----------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type23(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 24: antenna reference point (arp) -----------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type24(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 31: differential glonass correction ---------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type31(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 32: differential glonass reference station parameters ---------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type32(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 34: glonass partial differential correction set ---------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type34(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 36: glonass special message -----------------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type36(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 37: gnss system time offset -----------------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type37(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 59: proprietary message ---------------------------------------*/
|
2017-05-13 18:25:38 +00:00
|
|
|
int decode_type59(rtcm_t *rtcm __attribute((unused)))
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode rtcm ver.2 message -------------------------------------------------*/
|
|
|
|
int decode_rtcm2(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
double zcnt;
|
2017-05-13 18:01:08 +00:00
|
|
|
int staid, seqno, stah, ret = 0, type = getbitu(rtcm->buff, 8, 6);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(3, "decode_rtcm2: type=%2d len=%3d\n", type, rtcm->len);
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
if ((zcnt = getbitu(rtcm->buff, 24, 13)*0.6) >= 3600.0)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 modified z-count error: zcnt=%.1f\n", zcnt);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
adjhour(rtcm, zcnt);
|
|
|
|
staid = getbitu(rtcm->buff, 14, 10);
|
|
|
|
seqno = getbitu(rtcm->buff, 37, 3);
|
|
|
|
stah = getbitu(rtcm->buff, 45, 3);
|
|
|
|
if (seqno-rtcm->seqno != 1 && seqno-rtcm->seqno != -7)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 message outage: seqno=%d->%d\n", rtcm->seqno, seqno);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
rtcm->seqno = seqno;
|
|
|
|
rtcm->stah = stah;
|
|
|
|
|
|
|
|
if (rtcm->outtype)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
sprintf(rtcm->msgtype, "RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
|
|
|
|
type, rtcm->len, zcnt, staid, seqno);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
if (type == 3||type == 22||type == 23||type == 24)
|
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
if (rtcm->staid != 0 && staid != rtcm->staid)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 station id changed: %d->%d\n", rtcm->staid, staid);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
rtcm->staid = staid;
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if (rtcm->staid != 0 && staid != rtcm->staid)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
trace(2, "rtcm2 station id invalid: %d %d\n", staid, rtcm->staid);
|
2017-05-12 06:55:59 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
switch (type)
|
|
|
|
{
|
|
|
|
case 1: ret = decode_type1 (rtcm); break;
|
|
|
|
case 3: ret = decode_type3 (rtcm); break;
|
|
|
|
case 9: ret = decode_type1 (rtcm); break;
|
|
|
|
case 14: ret = decode_type14(rtcm); break;
|
|
|
|
case 16: ret = decode_type16(rtcm); break;
|
|
|
|
case 17: ret = decode_type17(rtcm); break;
|
|
|
|
case 18: ret = decode_type18(rtcm); break;
|
|
|
|
case 19: ret = decode_type19(rtcm); break;
|
|
|
|
case 22: ret = decode_type22(rtcm); break;
|
|
|
|
case 23: ret = decode_type23(rtcm); break; /* not supported */
|
|
|
|
case 24: ret = decode_type24(rtcm); break; /* not supported */
|
|
|
|
case 31: ret = decode_type31(rtcm); break; /* not supported */
|
|
|
|
case 32: ret = decode_type32(rtcm); break; /* not supported */
|
|
|
|
case 34: ret = decode_type34(rtcm); break; /* not supported */
|
|
|
|
case 36: ret = decode_type36(rtcm); break; /* not supported */
|
|
|
|
case 37: ret = decode_type37(rtcm); break; /* not supported */
|
|
|
|
case 59: ret = decode_type59(rtcm); break; /* not supported */
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
if (ret >= 0)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2017-05-13 18:01:08 +00:00
|
|
|
if (1 <= type && type <= 99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|