gnss-sdr/src/algorithms/libs/rtklib/rtklib_sbas.cc

/*!
 * \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 */
}