mirror of https://github.com/gnss-sdr/gnss-sdr
1332 lines
41 KiB
C++
1332 lines
41 KiB
C++
/*!
|
|
* \file rtklib_sbas.cc
|
|
* \brief sbas functions
|
|
* \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.
|
|
*
|
|
* SPDX-License-Identifier: BSD-2-Clause
|
|
*
|
|
*
|
|
* References :
|
|
* [1] RTCA/DO-229C, Minimum operational performanc standards for global
|
|
* positioning system/wide area augmentation system airborne equipment,
|
|
* RTCA inc, November 28, 2001
|
|
* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service
|
|
* Interface Specification for QZSS, Japan Aerospace Exploration Agency,
|
|
* July 31, 2009
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "rtklib_sbas.h"
|
|
#include "rtklib_rtkcmn.h"
|
|
#include <cmath> // for lround
|
|
#include <cstring>
|
|
|
|
/* extract field from line ---------------------------------------------------*/
|
|
char *getfield(char *p, int pos)
|
|
{
|
|
for (pos--; pos > 0; pos--, p++)
|
|
{
|
|
if (!(p = strchr(p, ',')))
|
|
{
|
|
return nullptr;
|
|
}
|
|
}
|
|
return p;
|
|
}
|
|
|
|
|
|
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/
|
|
double varfcorr(int udre)
|
|
{
|
|
const double var[14] = {
|
|
0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326,
|
|
5.1968, 20.7870, 230.9661, 2078.695};
|
|
return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0;
|
|
}
|
|
|
|
|
|
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
|
|
double varicorr(int give)
|
|
{
|
|
const double var[15] = {
|
|
0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315,
|
|
1.1974, 1.8709, 3.326, 20.787, 187.0826};
|
|
return 0 < give && give <= 15 ? var[give - 1] : 0.0;
|
|
}
|
|
|
|
|
|
/* fast correction degradation -----------------------------------------------*/
|
|
double degfcorr(int ai)
|
|
{
|
|
const double degf[16] = {
|
|
0.00000, 0.00005, 0.00009, 0.00012, 0.00015, 0.00020, 0.00030, 0.00045,
|
|
0.00060, 0.00090, 0.00150, 0.00210, 0.00270, 0.00330, 0.00460, 0.00580};
|
|
return 0 < ai && ai <= 15 ? degf[ai] : 0.0058;
|
|
}
|
|
|
|
|
|
/* decode type 1: prn masks --------------------------------------------------*/
|
|
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int n;
|
|
int sat;
|
|
|
|
trace(4, "decode_sbstype1:\n");
|
|
|
|
for (i = 1, n = 0; i <= 210 && n < MAXSAT; i++)
|
|
{
|
|
if (getbitu(msg->msg, 13 + i, 1))
|
|
{
|
|
if (i <= 37)
|
|
{
|
|
sat = satno(SYS_GPS, i); /* 0- 37: gps */
|
|
}
|
|
else if (i <= 61)
|
|
{
|
|
sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */
|
|
}
|
|
else if (i <= 119)
|
|
{
|
|
sat = 0; /* 62-119: future gnss */
|
|
}
|
|
else if (i <= 138)
|
|
{
|
|
sat = satno(SYS_SBS, i); /* 120-138: geo/waas */
|
|
}
|
|
else if (i <= 182)
|
|
{
|
|
sat = 0; /* 139-182: reserved */
|
|
}
|
|
else if (i <= 192)
|
|
{
|
|
sat = satno(SYS_SBS, i + 10); /* 183-192: qzss ref [2] */
|
|
}
|
|
else if (i <= 202)
|
|
{
|
|
sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */
|
|
}
|
|
else
|
|
{
|
|
sat = 0; /* 203- : reserved */
|
|
}
|
|
sbssat->sat[n++].sat = sat;
|
|
}
|
|
}
|
|
sbssat->iodp = getbitu(msg->msg, 224, 2);
|
|
sbssat->nsat = n;
|
|
|
|
trace(5, "decode_sbstype1: nprn=%d iodp=%d\n", n, sbssat->iodp);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode type 2-5,0: fast corrections ---------------------------------------*/
|
|
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int j;
|
|
int iodf;
|
|
int type;
|
|
int udre;
|
|
double prc;
|
|
double dt;
|
|
gtime_t t0;
|
|
|
|
trace(4, "decode_sbstype2:\n");
|
|
|
|
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 16, 2)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
type = getbitu(msg->msg, 8, 6);
|
|
iodf = getbitu(msg->msg, 14, 2);
|
|
|
|
for (i = 0; i < 13; i++)
|
|
{
|
|
if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat)
|
|
{
|
|
break;
|
|
}
|
|
udre = getbitu(msg->msg, 174 + 4 * i, 4);
|
|
t0 = sbssat->sat[j].fcorr.t0;
|
|
prc = sbssat->sat[j].fcorr.prc;
|
|
sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow);
|
|
sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125F;
|
|
sbssat->sat[j].fcorr.udre = udre + 1;
|
|
dt = timediff(sbssat->sat[j].fcorr.t0, t0);
|
|
if (t0.time == 0 || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0)
|
|
{
|
|
sbssat->sat[j].fcorr.rrc = 0.0;
|
|
sbssat->sat[j].fcorr.dt = 0.0;
|
|
}
|
|
else
|
|
{
|
|
sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc - prc) / dt;
|
|
sbssat->sat[j].fcorr.dt = dt;
|
|
}
|
|
sbssat->sat[j].fcorr.iodf = iodf;
|
|
}
|
|
trace(5, "decode_sbstype2: type=%d iodf=%d\n", type, iodf);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode type 6: integrity info ---------------------------------------------*/
|
|
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int iodf[4];
|
|
int udre;
|
|
|
|
trace(4, "decode_sbstype6:\n");
|
|
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
iodf[i] = getbitu(msg->msg, 14 + i * 2, 2);
|
|
}
|
|
for (i = 0; i < sbssat->nsat && i < MAXSAT; i++)
|
|
{
|
|
if (sbssat->sat[i].fcorr.iodf != iodf[i / 39])
|
|
{
|
|
continue;
|
|
}
|
|
udre = getbitu(msg->msg, 22 + i * 4, 4);
|
|
sbssat->sat[i].fcorr.udre = udre + 1;
|
|
}
|
|
trace(5, "decode_sbstype6: iodf=%d %d %d %d\n", iodf[0], iodf[1], iodf[2], iodf[3]);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode type 7: fast correction degradation factor -------------------------*/
|
|
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
|
|
trace(4, "decode_sbstype7\n");
|
|
|
|
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 18, 2)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
sbssat->tlat = getbitu(msg->msg, 14, 4);
|
|
|
|
for (i = 0; i < sbssat->nsat && i < MAXSAT; i++)
|
|
{
|
|
sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i * 4, 4);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode type 9: geo navigation message -------------------------------------*/
|
|
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
|
|
{
|
|
seph_t seph = {0, {0, 0}, {0, 0}, 0, 0, {}, {}, {}, 0.0, 0.0};
|
|
int i;
|
|
int sat;
|
|
int t;
|
|
|
|
trace(4, "decode_sbstype9:\n");
|
|
|
|
if (!(sat = satno(SYS_SBS, msg->prn)))
|
|
{
|
|
trace(2, "invalid prn in sbas type 9: prn=%3d\n", msg->prn);
|
|
return 0;
|
|
}
|
|
t = static_cast<int>(getbitu(msg->msg, 22, 13)) * 16 - msg->tow % 86400;
|
|
if (t <= -43200)
|
|
{
|
|
t += 86400;
|
|
}
|
|
else if (t > 43200)
|
|
{
|
|
t -= 86400;
|
|
}
|
|
seph.sat = sat;
|
|
seph.t0 = gpst2time(msg->week, msg->tow + t);
|
|
seph.tof = gpst2time(msg->week, msg->tow);
|
|
seph.sva = getbitu(msg->msg, 35, 4);
|
|
seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */
|
|
|
|
seph.pos[0] = getbits(msg->msg, 39, 30) * 0.08;
|
|
seph.pos[1] = getbits(msg->msg, 69, 30) * 0.08;
|
|
seph.pos[2] = getbits(msg->msg, 99, 25) * 0.4;
|
|
seph.vel[0] = getbits(msg->msg, 124, 17) * 0.000625;
|
|
seph.vel[1] = getbits(msg->msg, 141, 17) * 0.000625;
|
|
seph.vel[2] = getbits(msg->msg, 158, 18) * 0.004;
|
|
seph.acc[0] = getbits(msg->msg, 176, 10) * 0.0000125;
|
|
seph.acc[1] = getbits(msg->msg, 186, 10) * 0.0000125;
|
|
seph.acc[2] = getbits(msg->msg, 196, 10) * 0.0000625;
|
|
|
|
seph.af0 = getbits(msg->msg, 206, 12) * TWO_N31;
|
|
seph.af1 = getbits(msg->msg, 218, 8) * TWO_N39 / 2.0;
|
|
|
|
i = msg->prn - MINPRNSBS;
|
|
if (!nav->seph || fabs(timediff(nav->seph[i].t0, seph.t0)) < 1e-3)
|
|
{ /* not change */
|
|
return 0;
|
|
}
|
|
nav->seph[NSATSBS + i] = nav->seph[i]; /* previous */
|
|
nav->seph[i] = seph; /* current */
|
|
|
|
trace(5, "decode_sbstype9: prn=%d\n", msg->prn);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode type 18: ionospheric grid point masks ------------------------------*/
|
|
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
|
|
{
|
|
const sbsigpband_t *p;
|
|
int i;
|
|
int j;
|
|
int n;
|
|
int m;
|
|
int band = getbitu(msg->msg, 18, 4);
|
|
|
|
trace(4, "decode_sbstype18:\n");
|
|
|
|
if (0 <= band && band <= 8)
|
|
{
|
|
p = IGPBAND1[band];
|
|
m = 8;
|
|
}
|
|
else if (9 <= band && band <= 10)
|
|
{
|
|
p = IGPBAND2[band - 9];
|
|
m = 5;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
sbsion[band].iodi = static_cast<int16_t>(getbitu(msg->msg, 22, 2));
|
|
|
|
for (i = 1, n = 0; i <= 201; i++)
|
|
{
|
|
if (!getbitu(msg->msg, 23 + i, 1))
|
|
{
|
|
continue;
|
|
}
|
|
for (j = 0; j < m; j++)
|
|
{
|
|
if (i < p[j].bits || p[j].bite < i)
|
|
{
|
|
continue;
|
|
}
|
|
sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x;
|
|
sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits];
|
|
break;
|
|
}
|
|
}
|
|
sbsion[band].nigp = n;
|
|
|
|
trace(5, "decode_sbstype18: band=%d nigp=%d\n", band, n);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode half long term correction (vel code=0) -----------------------------*/
|
|
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int n = getbitu(msg->msg, p, 6);
|
|
|
|
trace(4, "decode_longcorr0:\n");
|
|
|
|
if (n == 0 || n > MAXSAT)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9 * i, 9) * 0.125;
|
|
sbssat->sat[n - 1].lcorr.dvel[i] = 0.0;
|
|
}
|
|
sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 41, 10) * TWO_N31;
|
|
sbssat->sat[n - 1].lcorr.daf1 = 0.0;
|
|
sbssat->sat[n - 1].lcorr.t0 = gpst2time(msg->week, msg->tow);
|
|
|
|
trace(5, "decode_longcorr0:sat=%2d\n", sbssat->sat[n - 1].sat);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode half long term correction (vel code=1) -----------------------------*/
|
|
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int n = getbitu(msg->msg, p, 6);
|
|
int t;
|
|
|
|
trace(4, "decode_longcorr1:\n");
|
|
|
|
if (n == 0 || n > MAXSAT)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i * 11, 11) * 0.125;
|
|
sbssat->sat[n - 1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i * 8, 8) * TWO_N11;
|
|
}
|
|
sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 47, 11) * TWO_N31;
|
|
sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8) * TWO_N39;
|
|
t = static_cast<int>(getbitu(msg->msg, p + 90, 13)) * 16 - msg->tow % 86400;
|
|
if (t <= -43200)
|
|
{
|
|
t += 86400;
|
|
}
|
|
else if (t > 43200)
|
|
{
|
|
t -= 86400;
|
|
}
|
|
sbssat->sat[n - 1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);
|
|
|
|
trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n - 1].sat);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode half long term correction ------------------------------------------*/
|
|
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
|
|
{
|
|
trace(4, "decode_longcorrh:\n");
|
|
|
|
if (getbitu(msg->msg, p, 1) == 0)
|
|
{ /* vel code=0 */
|
|
if (sbssat->iodp == static_cast<int>(getbitu(msg->msg, p + 103, 2)))
|
|
{
|
|
return decode_longcorr0(msg, p + 1, sbssat) &&
|
|
decode_longcorr0(msg, p + 52, sbssat);
|
|
}
|
|
}
|
|
else if (sbssat->iodp == static_cast<int>(getbitu(msg->msg, p + 104, 2)))
|
|
{
|
|
return decode_longcorr1(msg, p + 1, sbssat);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* decode type 24: mixed fast/long term correction ---------------------------*/
|
|
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
int i;
|
|
int j;
|
|
int iodf;
|
|
int blk;
|
|
int udre;
|
|
|
|
trace(4, "decode_sbstype24:\n");
|
|
|
|
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 110, 2)))
|
|
{
|
|
return 0; /* check IODP */
|
|
}
|
|
|
|
blk = getbitu(msg->msg, 112, 2);
|
|
iodf = getbitu(msg->msg, 114, 2);
|
|
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
if ((j = 13 * blk + i) >= sbssat->nsat)
|
|
{
|
|
break;
|
|
}
|
|
udre = getbitu(msg->msg, 86 + 4 * i, 4);
|
|
|
|
sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow);
|
|
sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i * 12, 12) * 0.125F;
|
|
sbssat->sat[j].fcorr.udre = udre + 1;
|
|
sbssat->sat[j].fcorr.iodf = iodf;
|
|
}
|
|
return decode_longcorrh(msg, 120, sbssat);
|
|
}
|
|
|
|
|
|
/* decode type 25: long term satellite error correction ----------------------*/
|
|
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
|
|
{
|
|
trace(4, "decode_sbstype25:\n");
|
|
|
|
return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat);
|
|
}
|
|
|
|
|
|
/* decode type 26: ionospheric deley corrections -----------------------------*/
|
|
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
|
|
{
|
|
int i;
|
|
int j;
|
|
int block;
|
|
int delay;
|
|
int give;
|
|
int band = getbitu(msg->msg, 14, 4);
|
|
|
|
trace(4, "decode_sbstype26:\n");
|
|
|
|
if (band > MAXBAND || sbsion[band].iodi != static_cast<int>(getbitu(msg->msg, 217, 2)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
block = getbitu(msg->msg, 18, 4);
|
|
|
|
for (i = 0; i < 15; i++)
|
|
{
|
|
if ((j = block * 15 + i) >= sbsion[band].nigp)
|
|
{
|
|
continue;
|
|
}
|
|
give = getbitu(msg->msg, 22 + i * 13 + 9, 4);
|
|
|
|
delay = getbitu(msg->msg, 22 + i * 13, 9);
|
|
sbsion[band].igp[j].t0 = gpst2time(msg->week, msg->tow);
|
|
sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0F : delay * 0.125F;
|
|
sbsion[band].igp[j].give = give + 1;
|
|
|
|
if (sbsion[band].igp[j].give >= 16)
|
|
{
|
|
sbsion[band].igp[j].give = 0;
|
|
}
|
|
}
|
|
trace(5, "decode_sbstype26: band=%d block=%d\n", band, block);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* update sbas corrections -----------------------------------------------------
|
|
* update sbas correction parameters in navigation data with a sbas message
|
|
* args : sbsmg_t *msg I sbas message
|
|
* nav_t *nav IO navigation data
|
|
* return : message type (-1: error or not supported type)
|
|
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
|
|
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
|
|
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
|
|
*-----------------------------------------------------------------------------*/
|
|
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
|
|
{
|
|
int type = getbitu(msg->msg, 8, 6);
|
|
int stat = -1;
|
|
|
|
trace(3, "sbsupdatecorr: type=%d\n", type);
|
|
|
|
if (msg->week == 0)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
switch (type)
|
|
{
|
|
case 0:
|
|
stat = decode_sbstype2(msg, &nav->sbssat);
|
|
break;
|
|
case 1:
|
|
stat = decode_sbstype1(msg, &nav->sbssat);
|
|
break;
|
|
case 2:
|
|
case 3:
|
|
case 4:
|
|
case 5:
|
|
stat = decode_sbstype2(msg, &nav->sbssat);
|
|
break;
|
|
case 6:
|
|
stat = decode_sbstype6(msg, &nav->sbssat);
|
|
break;
|
|
case 7:
|
|
stat = decode_sbstype7(msg, &nav->sbssat);
|
|
break;
|
|
case 9:
|
|
stat = decode_sbstype9(msg, nav);
|
|
break;
|
|
case 18:
|
|
stat = decode_sbstype18(msg, nav->sbsion);
|
|
break;
|
|
case 24:
|
|
stat = decode_sbstype24(msg, &nav->sbssat);
|
|
break;
|
|
case 25:
|
|
stat = decode_sbstype25(msg, &nav->sbssat);
|
|
break;
|
|
case 26:
|
|
stat = decode_sbstype26(msg, nav->sbsion);
|
|
break;
|
|
case 63:
|
|
break; /* null message */
|
|
|
|
/*default: trace(2, "unsupported sbas message: type=%d\n", type); break;*/
|
|
}
|
|
return stat ? type : -1;
|
|
}
|
|
|
|
|
|
/* read sbas log file --------------------------------------------------------*/
|
|
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
|
|
sbs_t *sbs)
|
|
{
|
|
sbsmsg_t *sbs_msgs;
|
|
int i;
|
|
int week;
|
|
int prn;
|
|
int ch;
|
|
int msg;
|
|
unsigned int b;
|
|
double tow;
|
|
double ep[6] = {};
|
|
char buff[256];
|
|
char *p;
|
|
gtime_t time;
|
|
FILE *fp;
|
|
|
|
trace(3, "readmsgs: file=%s sel=%d\n", file, sel);
|
|
|
|
if (!(fp = fopen(file, "re")))
|
|
{
|
|
trace(2, "sbas message file open error: %s\n", file);
|
|
return;
|
|
}
|
|
while (fgets(buff, sizeof(buff), fp))
|
|
{
|
|
if (sscanf(buff, "%d %lf %d", &week, &tow, &prn) == 3 && (p = strstr(buff, ": ")))
|
|
{
|
|
p += 2; /* rtklib form */
|
|
}
|
|
else if (sscanf(buff, "%d %lf %lf %lf %lf %lf %lf %d",
|
|
&prn, ep, ep + 1, ep + 2, ep + 3, ep + 4, ep + 5, &msg) == 8)
|
|
{
|
|
/* ems (EGNOS Message Service) form */
|
|
ep[0] += ep[0] < 70.0 ? 2000.0 : 1900.0;
|
|
tow = time2gpst(epoch2time(ep), &week);
|
|
p = buff + (msg >= 10 ? 25 : 24);
|
|
}
|
|
else if (!strncmp(buff, "#RAWWAASFRAMEA", 14))
|
|
{ /* NovAtel OEM4/V */
|
|
if (!(p = getfield(buff, 6)))
|
|
{
|
|
continue;
|
|
}
|
|
if (sscanf(p, "%d,%lf", &week, &tow) < 2)
|
|
{
|
|
continue;
|
|
}
|
|
if (!(p = strchr(++p, ';')))
|
|
{
|
|
continue;
|
|
}
|
|
if (sscanf(++p, "%d,%d", &ch, &prn) < 2)
|
|
{
|
|
continue;
|
|
}
|
|
if (!(p = getfield(p, 4)))
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
else if (!strncmp(buff, "$FRMA", 5))
|
|
{ /* NovAtel OEM3 */
|
|
if (!(p = getfield(buff, 2)))
|
|
{
|
|
continue;
|
|
}
|
|
if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3)
|
|
{
|
|
continue;
|
|
}
|
|
if (!(p = getfield(p, 6)))
|
|
{
|
|
continue;
|
|
}
|
|
if (week < WEEKOFFSET)
|
|
{
|
|
week += WEEKOFFSET;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (sel != 0 && sel != prn)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
time = gpst2time(week, tow);
|
|
|
|
if (!screent(time, ts, te, 0.0))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (sbs->n >= sbs->nmax)
|
|
{
|
|
sbs->nmax = sbs->nmax == 0 ? 1024 : sbs->nmax * 2;
|
|
if (!(sbs_msgs = static_cast<sbsmsg_t *>(realloc(sbs->msgs, sbs->nmax * sizeof(sbsmsg_t)))))
|
|
{
|
|
trace(1, "readsbsmsg malloc error: nmax=%d\n", sbs->nmax);
|
|
free(sbs->msgs);
|
|
sbs->msgs = nullptr;
|
|
sbs->n = sbs->nmax = 0;
|
|
fclose(fp);
|
|
return;
|
|
}
|
|
sbs->msgs = sbs_msgs;
|
|
}
|
|
sbs->msgs[sbs->n].week = week;
|
|
sbs->msgs[sbs->n].tow = static_cast<int>(std::lround(tow));
|
|
sbs->msgs[sbs->n].prn = prn;
|
|
for (i = 0; i < 29; i++)
|
|
{
|
|
sbs->msgs[sbs->n].msg[i] = 0;
|
|
}
|
|
for (i = 0; *(p - 1) && *p && i < 29; p += 2, i++)
|
|
{
|
|
if (sscanf(p, "%2X", &b) == 1)
|
|
{
|
|
sbs->msgs[sbs->n].msg[i] = static_cast<unsigned char>(b);
|
|
}
|
|
}
|
|
sbs->msgs[sbs->n++].msg[28] &= 0xC0;
|
|
}
|
|
fclose(fp);
|
|
}
|
|
|
|
|
|
/* compare sbas messages -----------------------------------------------------*/
|
|
int cmpmsgs(const void *p1, const void *p2)
|
|
{
|
|
const auto *q1 = static_cast<const sbsmsg_t *>(p1);
|
|
const auto *q2 = static_cast<const sbsmsg_t *>(p2);
|
|
return q1->week != q2->week ? q1->week - q2->week : (q1->tow < q2->tow ? -1 : (q1->tow > q2->tow ? 1 : q1->prn - q2->prn));
|
|
}
|
|
|
|
|
|
/* read sbas message file ------------------------------------------------------
|
|
* read sbas message file
|
|
* args : char *file I sbas message file (wind-card * is expanded)
|
|
* int sel I sbas satellite prn number selection (0:all)
|
|
* (gtime_t ts I start time)
|
|
* (gtime_t te I end time )
|
|
* sbs_t *sbs IO sbas messages
|
|
* return : number of sbas messages
|
|
* notes : sbas message are appended and sorted. before calling the function,
|
|
* sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially
|
|
* sbs->n=sbs->nmax=0, sbs->msgs=NULL)
|
|
* only the following file extensions after wild card expanded are valid
|
|
* to read. others are skipped
|
|
* .sbs, .SBS, .ems, .EMS
|
|
*-----------------------------------------------------------------------------*/
|
|
int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
|
|
sbs_t *sbs)
|
|
{
|
|
char *efiles[MAXEXFILE] = {};
|
|
char *ext;
|
|
int i;
|
|
int n;
|
|
|
|
trace(3, "sbsreadmsgt: file=%s sel=%d\n", file, sel);
|
|
|
|
for (i = 0; i < MAXEXFILE; i++)
|
|
{
|
|
if (!(efiles[i] = static_cast<char *>(malloc(MAXSTRPATH + 255))))
|
|
{
|
|
for (i--; i >= 0; i--)
|
|
{
|
|
free(efiles[i]);
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
/* expand wild card in file path */
|
|
n = expath(file, efiles, MAXEXFILE);
|
|
|
|
for (i = 0; i < n; i++)
|
|
{
|
|
if (!(ext = strrchr(efiles[i], '.')))
|
|
{
|
|
continue;
|
|
}
|
|
if (strcmp(ext, ".sbs") != 0 && strcmp(ext, ".SBS") != 0 &&
|
|
strcmp(ext, ".ems") != 0 && strcmp(ext, ".EMS") != 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
readmsgs(efiles[i], sel, ts, te, sbs);
|
|
}
|
|
for (i = 0; i < MAXEXFILE; i++)
|
|
{
|
|
free(efiles[i]);
|
|
}
|
|
|
|
/* sort messages */
|
|
if (sbs->n > 0)
|
|
{
|
|
qsort(sbs->msgs, sbs->n, sizeof(sbsmsg_t), cmpmsgs);
|
|
}
|
|
return sbs->n;
|
|
}
|
|
|
|
|
|
int sbsreadmsg(const char *file, int sel, sbs_t *sbs)
|
|
{
|
|
gtime_t ts = {0, 0};
|
|
gtime_t te = {0, 0};
|
|
|
|
trace(3, "sbsreadmsg: file=%s sel=%d\n", file, sel);
|
|
|
|
return sbsreadmsgt(file, sel, ts, te, sbs);
|
|
}
|
|
|
|
|
|
/* output sbas messages --------------------------------------------------------
|
|
* output sbas message record to output file in rtklib sbas log format
|
|
* args : FILE *fp I output file pointer
|
|
* sbsmsg_t *sbsmsg I sbas messages
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg)
|
|
{
|
|
int i;
|
|
int type = sbsmsg->msg[1] >> 2;
|
|
|
|
trace(4, "sbsoutmsg:\n");
|
|
|
|
fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type);
|
|
for (i = 0; i < 29; i++)
|
|
{
|
|
fprintf(fp, "%02X", sbsmsg->msg[i]);
|
|
}
|
|
fprintf(fp, "\n");
|
|
}
|
|
|
|
|
|
/* search igps ---------------------------------------------------------------*/
|
|
void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sbsion_t *ion,
|
|
const sbsigp_t **igp, double *x, double *y)
|
|
{
|
|
int i;
|
|
int latp[2];
|
|
int lonp[4];
|
|
double lat = pos[0] * R2D;
|
|
double lon = pos[1] * R2D;
|
|
const sbsigp_t *p;
|
|
|
|
trace(4, "searchigp: pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
|
|
|
|
if (lon >= 180.0)
|
|
{
|
|
lon -= 360.0;
|
|
}
|
|
if (-55.0 <= lat && lat < 55.0)
|
|
{
|
|
latp[0] = static_cast<int>(floor(lat / 5.0)) * 5;
|
|
latp[1] = latp[0] + 5;
|
|
lonp[0] = lonp[1] = static_cast<int>(floor(lon / 5.0)) * 5;
|
|
lonp[2] = lonp[3] = lonp[0] + 5;
|
|
*x = (lon - lonp[0]) / 5.0;
|
|
*y = (lat - latp[0]) / 5.0;
|
|
}
|
|
else
|
|
{
|
|
latp[0] = static_cast<int>(floor((lat - 5.0) / 10.0)) * 10 + 5;
|
|
latp[1] = latp[0] + 10;
|
|
lonp[0] = lonp[1] = static_cast<int>(floor(lon / 10.0)) * 10;
|
|
lonp[2] = lonp[3] = lonp[0] + 10;
|
|
*x = (lon - lonp[0]) / 10.0;
|
|
*y = (lat - latp[0]) / 10.0;
|
|
if (75.0 <= lat && lat < 85.0)
|
|
{
|
|
lonp[1] = static_cast<int>(floor(lon / 90.0)) * 90;
|
|
lonp[3] = lonp[1] + 90;
|
|
}
|
|
else if (-85.0 <= lat && lat < -75.0)
|
|
{
|
|
lonp[0] = static_cast<int>(floor((lon - 50.0) / 90.0)) * 90 + 40;
|
|
lonp[2] = lonp[0] + 90;
|
|
}
|
|
else if (lat >= 85.0)
|
|
{
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
lonp[i] = static_cast<int>(floor(lon / 90.0)) * 90;
|
|
}
|
|
}
|
|
else if (lat < -85.0)
|
|
{
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
lonp[i] = static_cast<int>(floor((lon - 50.0) / 90.0)) * 90 + 40;
|
|
}
|
|
}
|
|
}
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
if (lonp[i] == 180)
|
|
{
|
|
lonp[i] = -180;
|
|
}
|
|
}
|
|
for (i = 0; i <= MAXBAND; i++)
|
|
{
|
|
for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++)
|
|
{
|
|
if (p->t0.time == 0)
|
|
{
|
|
continue;
|
|
}
|
|
if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0)
|
|
{
|
|
igp[0] = p;
|
|
}
|
|
else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0)
|
|
{
|
|
igp[1] = p;
|
|
}
|
|
else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0)
|
|
{
|
|
igp[2] = p;
|
|
}
|
|
else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0)
|
|
{
|
|
igp[3] = p;
|
|
}
|
|
if (igp[0] && igp[1] && igp[2] && igp[3])
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/* sbas ionospheric delay correction -------------------------------------------
|
|
* compute sbas ionosphric delay correction
|
|
* args : gtime_t time I time
|
|
* nav_t *nav I navigation data
|
|
* double *pos I receiver position {lat,lon,height} (rad/m)
|
|
* double *azel I satellite azimuth/elavation angle (rad)
|
|
* double *delay O slant ionospheric delay (L1) (m)
|
|
* double *var O variance of ionospheric delay (m^2)
|
|
* return : status (1:ok, 0:no correction)
|
|
* notes : before calling the function, sbas ionosphere correction parameters
|
|
* in navigation data (nav->sbsion) must be set by calling
|
|
* sbsupdatecorr()
|
|
*-----------------------------------------------------------------------------*/
|
|
int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
|
|
const double *azel, double *delay, double *var)
|
|
{
|
|
const double re = 6378.1363;
|
|
const double hion = 350.0;
|
|
int i;
|
|
int err = 0;
|
|
double fp;
|
|
double posp[2];
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
double t;
|
|
double w[4] = {};
|
|
const sbsigp_t *igp[4] = {}; /* {ws,wn,es,en} */
|
|
|
|
trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D,
|
|
azel[0] * R2D, azel[1] * R2D);
|
|
|
|
*delay = *var = 0.0;
|
|
if (pos[2] < -100.0 || azel[1] <= 0)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
/* ipp (ionospheric pierce point) position */
|
|
fp = ionppp(pos, azel, re, hion, posp);
|
|
|
|
/* search igps around ipp */
|
|
searchigp(time, posp, nav->sbsion, igp, &x, &y);
|
|
|
|
/* weight of igps */
|
|
if (igp[0] && igp[1] && igp[2] && igp[3])
|
|
{
|
|
w[0] = (1.0 - x) * (1.0 - y);
|
|
w[1] = (1.0 - x) * y;
|
|
w[2] = x * (1.0 - y);
|
|
w[3] = x * y;
|
|
}
|
|
else if (igp[0] && igp[1] && igp[2])
|
|
{
|
|
w[1] = y;
|
|
w[2] = x;
|
|
if ((w[0] = 1.0 - w[1] - w[2]) < 0.0)
|
|
{
|
|
err = 1;
|
|
}
|
|
}
|
|
else if (igp[0] && igp[2] && igp[3])
|
|
{
|
|
w[0] = 1.0 - x;
|
|
w[3] = y;
|
|
if ((w[2] = 1.0 - w[0] - w[3]) < 0.0)
|
|
{
|
|
err = 1;
|
|
}
|
|
}
|
|
else if (igp[0] && igp[1] && igp[3])
|
|
{
|
|
w[0] = 1.0 - y;
|
|
w[3] = x;
|
|
if ((w[1] = 1.0 - w[0] - w[3]) < 0.0)
|
|
{
|
|
err = 1;
|
|
}
|
|
}
|
|
else if (igp[1] && igp[2] && igp[3])
|
|
{
|
|
w[1] = 1.0 - x;
|
|
w[2] = 1.0 - y;
|
|
if ((w[3] = 1.0 - w[1] - w[2]) < 0.0)
|
|
{
|
|
err = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
err = 1;
|
|
}
|
|
|
|
if (err)
|
|
{
|
|
trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f\n", posp[0] * R2D,
|
|
posp[1] * R2D);
|
|
return 0;
|
|
}
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
if (!igp[i])
|
|
{
|
|
continue;
|
|
}
|
|
t = timediff(time, igp[i]->t0);
|
|
*delay += w[i] * igp[i]->delay;
|
|
*var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t);
|
|
}
|
|
*delay *= fp;
|
|
*var *= fp * fp;
|
|
|
|
trace(5, "sbsioncorr: dion=%7.2f sig=%7.2f\n", *delay, sqrt(*var));
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* get meterological parameters ----------------------------------------------*/
|
|
void getmet(double lat, double *met)
|
|
{
|
|
static const double metprm[][10] = {/* lat=15,30,45,60,75 */
|
|
{1013.25, 299.65, 26.31, 6.30E-3, 2.77, 0.00, 0.00, 0.00, 0.00E-3, 0.00},
|
|
{1017.25, 294.15, 21.79, 6.05E-3, 3.15, -3.75, 7.00, 8.85, 0.25E-3, 0.33},
|
|
{1015.75, 283.15, 11.66, 5.58E-3, 2.57, -2.25, 11.00, 7.24, 0.32E-3, 0.46},
|
|
{1011.75, 272.15, 6.78, 5.39E-3, 1.81, -1.75, 15.00, 5.36, 0.81E-3, 0.74},
|
|
{1013.00, 263.65, 4.11, 4.53E-3, 1.55, -0.50, 14.50, 3.39, 0.62E-3, 0.30}};
|
|
int i;
|
|
int j;
|
|
double a;
|
|
lat = fabs(lat);
|
|
if (lat <= 15.0)
|
|
{
|
|
for (i = 0; i < 10; i++)
|
|
{
|
|
met[i] = metprm[0][i];
|
|
}
|
|
}
|
|
else if (lat >= 75.0)
|
|
{
|
|
for (i = 0; i < 10; i++)
|
|
{
|
|
met[i] = metprm[4][i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
j = static_cast<int>(lat / 15.0);
|
|
a = (lat - j * 15.0) / 15.0;
|
|
for (i = 0; i < 10; i++)
|
|
{
|
|
met[i] = (1.0 - a) * metprm[j - 1][i] + a * metprm[j][i];
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/* tropospheric delay correction -----------------------------------------------
|
|
* compute sbas tropospheric delay correction (mops model)
|
|
* args : gtime_t time I time
|
|
* double *pos I receiver position {lat,lon,height} (rad/m)
|
|
* double *azel I satellite azimuth/elavation (rad)
|
|
* double *var O variance of troposphric error (m^2)
|
|
* return : slant tropospheric delay (m)
|
|
*-----------------------------------------------------------------------------*/
|
|
double sbstropcorr(gtime_t time, const double *pos, const double *azel,
|
|
double *var)
|
|
{
|
|
const double k1 = 77.604;
|
|
const double k2 = 382000.0;
|
|
const double rd = 287.054;
|
|
const double gm = 9.784;
|
|
const double g = 9.80665;
|
|
static double pos_[3] = {};
|
|
static double zh = 0.0;
|
|
static double zw = 0.0;
|
|
int i;
|
|
double c;
|
|
double met[10];
|
|
double sinel = sin(azel[1]);
|
|
double h = pos[2];
|
|
double m;
|
|
|
|
trace(4, "sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D,
|
|
azel[0] * R2D, azel[1] * R2D);
|
|
|
|
if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0)
|
|
{
|
|
*var = 0.0;
|
|
return 0.0;
|
|
}
|
|
if (zh == 0.0 || fabs(pos[0] - pos_[0]) > 1e-7 || fabs(pos[1] - pos_[1]) > 1e-7 ||
|
|
fabs(pos[2] - pos_[2]) > 1.0)
|
|
{
|
|
getmet(pos[0] * R2D, met);
|
|
c = cos(2.0 * GNSS_PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25);
|
|
for (i = 0; i < 5; i++)
|
|
{
|
|
met[i] -= met[i + 5] * c;
|
|
}
|
|
zh = 1e-6 * k1 * rd * met[0] / gm;
|
|
zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1];
|
|
zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3]));
|
|
zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0);
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
pos_[i] = pos[i];
|
|
}
|
|
}
|
|
m = 1.001 / sqrt(0.002001 + sinel * sinel);
|
|
*var = 0.12 * 0.12 * m * m;
|
|
return (zh + zw) * m;
|
|
}
|
|
|
|
|
|
/* long term correction ------------------------------------------------------*/
|
|
int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
|
|
double *drs, double *ddts)
|
|
{
|
|
const sbssatp_t *p;
|
|
double t;
|
|
int i;
|
|
|
|
trace(3, "sbslongcorr: sat=%2d\n", sat);
|
|
|
|
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
|
|
{
|
|
if (p->sat != sat || p->lcorr.t0.time == 0)
|
|
{
|
|
continue;
|
|
}
|
|
t = timediff(time, p->lcorr.t0);
|
|
if (fabs(t) > MAXSBSAGEL)
|
|
{
|
|
trace(2, "sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
|
|
time_str(time, 0), sat, t);
|
|
return 0;
|
|
}
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t;
|
|
}
|
|
*ddts = p->lcorr.daf0 + p->lcorr.daf1 * t;
|
|
|
|
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
|
|
sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT_M_S);
|
|
|
|
return 1;
|
|
}
|
|
/* if sbas satellite without correction, no correction applied */
|
|
if (satsys(sat, nullptr) == SYS_SBS)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
trace(2, "no sbas long-term correction: %s sat=%2d\n", time_str(time, 0), sat);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* fast correction -----------------------------------------------------------*/
|
|
int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
|
|
double *prc, double *var)
|
|
{
|
|
const sbssatp_t *p;
|
|
double t;
|
|
|
|
trace(3, "sbsfastcorr: sat=%2d\n", sat);
|
|
|
|
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
|
|
{
|
|
if (p->sat != sat)
|
|
{
|
|
continue;
|
|
}
|
|
if (p->fcorr.t0.time == 0)
|
|
{
|
|
break;
|
|
}
|
|
t = timediff(time, p->fcorr.t0) + sbssat->tlat;
|
|
|
|
/* expire age of correction or UDRE==14 (not monitored) */
|
|
if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15)
|
|
{
|
|
continue;
|
|
}
|
|
*prc = p->fcorr.prc;
|
|
#ifdef RRCENA
|
|
if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt)
|
|
{
|
|
*prc += p->fcorr.rrc * t;
|
|
}
|
|
#endif
|
|
*var = varfcorr(p->fcorr.udre) + degfcorr(p->fcorr.ai) * t * t / 2.0;
|
|
|
|
trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n", sat,
|
|
*prc, sqrt(*var), t);
|
|
return 1;
|
|
}
|
|
trace(2, "no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* sbas satellite ephemeris and clock correction -------------------------------
|
|
* correct satellite position and clock bias with sbas satellite corrections
|
|
* args : gtime_t time I reception time
|
|
* int sat I satellite
|
|
* nav_t *nav I navigation data
|
|
* double *rs IO sat position and corrected {x,y,z} (ecef) (m)
|
|
* double *dts IO sat clock bias and corrected (s)
|
|
* double *var O sat position and clock variance (m^2)
|
|
* return : status (1:ok,0:no correction)
|
|
* notes : before calling the function, sbas satellite correction parameters
|
|
* in navigation data (nav->sbssat) must be set by calling
|
|
* sbsupdatecorr().
|
|
* satellite clock correction include long-term correction and fast
|
|
* correction.
|
|
* sbas clock correction is usually based on L1C/A code. TGD or DCB has
|
|
* to be considered for other codes
|
|
*-----------------------------------------------------------------------------*/
|
|
int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
|
|
double *dts, double *var)
|
|
{
|
|
double drs[3] = {};
|
|
double dclk = 0.0;
|
|
double prc = 0.0;
|
|
int i;
|
|
|
|
trace(3, "sbssatcorr : sat=%2d\n", sat);
|
|
|
|
/* sbas long term corrections */
|
|
if (!sbslongcorr(time, sat, &nav->sbssat, drs, &dclk))
|
|
{
|
|
return 0;
|
|
}
|
|
/* sbas fast corrections */
|
|
if (!sbsfastcorr(time, sat, &nav->sbssat, &prc, var))
|
|
{
|
|
return 0;
|
|
}
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
rs[i] += drs[i];
|
|
}
|
|
|
|
dts[0] += dclk + prc / SPEED_OF_LIGHT_M_S;
|
|
|
|
trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n",
|
|
sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT_M_S, *var);
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* decode sbas message ---------------------------------------------------------
|
|
* decode sbas message frame words and check crc
|
|
* args : gtime_t time I reception time
|
|
* int prn I sbas satellite prn number
|
|
* unsigned int *word I message frame words (24bit x 10)
|
|
* sbsmsg_t *sbsmsg O sbas message
|
|
* return : status (1:ok,0:crc error)
|
|
*-----------------------------------------------------------------------------*/
|
|
int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
|
|
sbsmsg_t *sbsmsg)
|
|
{
|
|
int i;
|
|
int j;
|
|
unsigned char f[29];
|
|
double tow;
|
|
|
|
trace(5, "sbsdecodemsg: prn=%d\n", prn);
|
|
|
|
if (time.time == 0)
|
|
{
|
|
return 0;
|
|
}
|
|
tow = time2gpst(time, &sbsmsg->week);
|
|
sbsmsg->tow = static_cast<int>(tow + DTTOL);
|
|
sbsmsg->prn = prn;
|
|
for (i = 0; i < 7; i++)
|
|
{
|
|
for (j = 0; j < 4; j++)
|
|
{
|
|
sbsmsg->msg[i * 4 + j] = static_cast<unsigned char>(words[i] >> ((3 - j) * 8));
|
|
}
|
|
}
|
|
sbsmsg->msg[28] = static_cast<unsigned char>(words[7] >> 18) & 0xC0;
|
|
for (i = 28; i > 0; i--)
|
|
{
|
|
f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i - 1] << 2);
|
|
}
|
|
f[0] = sbsmsg->msg[0] >> 6;
|
|
|
|
return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */
|
|
}
|