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)
|
|
|
|
{
|
2019-08-12 22:19:31 +00:00
|
|
|
double tow;
|
|
|
|
double hour;
|
|
|
|
double sec;
|
2017-05-12 06:55:59 +00:00
|
|
|
int week;
|
|
|
|
|
|
|
|
/* if no time, get cpu time */
|
2019-02-11 20:13:02 +00:00
|
|
|
if (rtcm->time.time == 0)
|
|
|
|
{
|
|
|
|
rtcm->time = utc2gpst(timeget());
|
|
|
|
}
|
2017-05-13 18:01:08 +00:00
|
|
|
tow = time2gpst(rtcm->time, &week);
|
2018-03-03 01:03:39 +00:00
|
|
|
hour = floor(tow / 3600.0);
|
|
|
|
sec = tow - hour * 3600.0;
|
|
|
|
if (zcnt < sec - 1800.0)
|
2019-02-11 20:13:02 +00:00
|
|
|
{
|
|
|
|
zcnt += 3600.0;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
else if (zcnt > sec + 1800.0)
|
2019-02-11 20:13:02 +00:00
|
|
|
{
|
|
|
|
zcnt -= 3600.0;
|
|
|
|
}
|
2018-03-03 01:03:39 +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)
|
|
|
|
{
|
2019-08-12 22:19:31 +00:00
|
|
|
int i;
|
|
|
|
int j;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
for (i = 0; i < obs->n; i++)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2019-02-11 20:13:02 +00:00
|
|
|
if (obs->data[i].sat == sat)
|
|
|
|
{
|
|
|
|
return i; /* field already exists */
|
|
|
|
}
|
|
|
|
}
|
|
|
|
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;
|
2018-03-03 01:03:39 +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)
|
|
|
|
{
|
2019-08-12 22:19:31 +00:00
|
|
|
int i = 48;
|
|
|
|
int fact;
|
|
|
|
int udre;
|
|
|
|
int prn;
|
|
|
|
int sat;
|
|
|
|
int iod;
|
|
|
|
double prc;
|
|
|
|
double 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
while (i + 40 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
2019-02-11 20:13:02 +00:00
|
|
|
if (prn == 0)
|
|
|
|
{
|
|
|
|
prn = 32;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (prc == 0x80000000 || rrc == 0xFFFF8000)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
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);
|
2018-03-03 01:03:39 +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;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 96 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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 ------------------------------------------*/
|
2019-09-13 14:36:41 +00:00
|
|
|
int decode_type14(rtcm_t *rtcm, int custom_year)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
double zcnt;
|
2019-08-12 22:19:31 +00:00
|
|
|
int i = 48;
|
|
|
|
int week;
|
|
|
|
int hour;
|
|
|
|
int 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);
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
week = getbitu(rtcm->buff, i, 10);
|
|
|
|
i += 10;
|
|
|
|
hour = getbitu(rtcm->buff, i, 8);
|
|
|
|
i += 8;
|
2017-05-13 18:01:08 +00:00
|
|
|
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;
|
|
|
|
}
|
2019-09-13 14:36:41 +00:00
|
|
|
week = adjgpsweek(week, custom_year);
|
2018-03-03 01:03:39 +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)
|
|
|
|
{
|
2019-08-12 22:19:31 +00:00
|
|
|
int i = 48;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
while (i + 8 <= rtcm->len * 8 && n < 90)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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 -------------------------------------------*/
|
2019-09-13 14:36:41 +00:00
|
|
|
int decode_type17(rtcm_t *rtcm, int custom_year)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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,
|
2018-03-08 17:05:22 +00:00
|
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0};
|
2019-08-12 22:19:31 +00:00
|
|
|
double toc;
|
|
|
|
double sqrtA;
|
|
|
|
int i = 48;
|
|
|
|
int week;
|
|
|
|
int prn;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 480 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2019-02-11 20:13:02 +00:00
|
|
|
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;
|
2019-09-13 14:36:41 +00:00
|
|
|
eph.week = adjgpsweek(week, custom_year);
|
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;
|
2018-03-03 01:03:39 +00:00
|
|
|
eph.A = sqrtA * sqrtA;
|
|
|
|
rtcm->nav.eph[sat - 1] = eph;
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->ephsat = sat;
|
|
|
|
return 2;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
|
|
|
|
int decode_type18(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
gtime_t time;
|
2019-08-12 22:19:31 +00:00
|
|
|
double usec;
|
|
|
|
double cp;
|
|
|
|
double tt;
|
|
|
|
int i = 48;
|
|
|
|
int index;
|
|
|
|
int freq;
|
|
|
|
int sync = 1;
|
|
|
|
int code;
|
|
|
|
int sys;
|
|
|
|
int prn;
|
|
|
|
int sat;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (freq & 0x1)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
freq >>= 1;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
while (i + 48 <= rtcm->len * 8 && rtcm->obs.n < MAXOBS)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
2019-02-11 20:13:02 +00:00
|
|
|
if (prn == 0)
|
|
|
|
{
|
|
|
|
prn = 32;
|
|
|
|
}
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
time = timeadd(rtcm->time, usec * 1E-6);
|
2019-02-11 20:13:02 +00:00
|
|
|
if (sys)
|
|
|
|
{
|
|
|
|
time = utc2gpst(time); /* convert glonass time to gpst */
|
|
|
|
}
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
tt = timediff(rtcm->obs.data[0].time, time);
|
2018-03-03 01:03:39 +00:00
|
|
|
if (rtcm->obsflag || fabs(tt) > 1E-9)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
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
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
rtcm->obs.data[index].L[freq] = -cp / 256.0;
|
|
|
|
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] =
|
2018-03-03 01:03:39 +00:00
|
|
|
!freq ? (code ? CODE_L1P : CODE_L1C) : (code ? CODE_L2P : CODE_L2C);
|
|
|
|
rtcm->loss[sat - 1][freq] = loss;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
rtcm->obsflag = !sync;
|
2018-03-03 01:03:39 +00:00
|
|
|
return sync ? 0 : 1;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 19: rtk uncorrected pseudorange -------------------------------*/
|
|
|
|
int decode_type19(rtcm_t *rtcm)
|
|
|
|
{
|
|
|
|
gtime_t time;
|
2019-08-12 22:19:31 +00:00
|
|
|
double usec;
|
|
|
|
double pr;
|
|
|
|
double tt;
|
|
|
|
int i = 48;
|
|
|
|
int index;
|
|
|
|
int freq;
|
|
|
|
int sync = 1;
|
|
|
|
int code;
|
|
|
|
int sys;
|
|
|
|
int prn;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (freq & 0x1)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
freq >>= 1;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
while (i + 48 <= rtcm->len * 8 && rtcm->obs.n < MAXOBS)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
2019-02-11 20:13:02 +00:00
|
|
|
if (prn == 0)
|
|
|
|
{
|
|
|
|
prn = 32;
|
|
|
|
}
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
time = timeadd(rtcm->time, usec * 1E-6);
|
2019-02-11 20:13:02 +00:00
|
|
|
if (sys)
|
|
|
|
{
|
|
|
|
time = utc2gpst(time); /* convert glonass time to gpst */
|
|
|
|
}
|
2017-05-12 06:55:59 +00:00
|
|
|
|
2017-05-13 18:01:08 +00:00
|
|
|
tt = timediff(rtcm->obs.data[0].time, time);
|
2018-03-03 01:03:39 +00:00
|
|
|
if (rtcm->obsflag || fabs(tt) > 1E-9)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
|
|
|
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
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
rtcm->obs.data[index].P[freq] = pr * 0.02;
|
2017-05-12 06:55:59 +00:00
|
|
|
rtcm->obs.data[index].code[freq] =
|
2018-03-03 01:03:39 +00:00
|
|
|
!freq ? (code ? CODE_L1P : CODE_L1C) : (code ? CODE_L2P : CODE_L2C);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
rtcm->obsflag = !sync;
|
2018-03-03 01:03:39 +00:00
|
|
|
return sync ? 0 : 1;
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* decode type 22: extended reference station parameter ----------------------*/
|
|
|
|
int decode_type22(rtcm_t *rtcm)
|
|
|
|
{
|
2019-08-12 22:19:31 +00:00
|
|
|
double del[2][3] = {{0}};
|
|
|
|
double hgt = 0.0;
|
|
|
|
int i = 48;
|
|
|
|
int j;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (i + 24 <= rtcm->len * 8)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +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 */
|
2019-02-11 20:13:02 +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;
|
2019-08-12 22:19:31 +00:00
|
|
|
int staid;
|
|
|
|
int seqno;
|
|
|
|
int stah;
|
|
|
|
int ret = 0;
|
|
|
|
int 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
|
|
|
|
2018-03-03 01:03:39 +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);
|
2018-03-03 01:03:39 +00:00
|
|
|
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;
|
2018-03-03 01:03:39 +00:00
|
|
|
rtcm->stah = stah;
|
2017-05-12 06:55:59 +00:00
|
|
|
|
|
|
|
if (rtcm->outtype)
|
|
|
|
{
|
2019-08-23 18:48:52 +00:00
|
|
|
std::snprintf(rtcm->msgtype, sizeof(rtcm->msgtype), "RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
|
2018-03-03 01:03:39 +00:00
|
|
|
type, rtcm->len, zcnt, staid, seqno);
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
if (type == 3 || type == 22 || type == 23 || type == 24)
|
2017-05-12 06:55:59 +00:00
|
|
|
{
|
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)
|
2018-03-03 01:03:39 +00:00
|
|
|
{
|
|
|
|
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
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (1 <= type && type <= 99)
|
2019-02-11 20:13:02 +00:00
|
|
|
{
|
|
|
|
rtcm->nmsg2[type]++;
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
else
|
2019-02-11 20:13:02 +00:00
|
|
|
{
|
|
|
|
rtcm->nmsg2[0]++;
|
|
|
|
}
|
2017-05-12 06:55:59 +00:00
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|