mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity
gnss-sdr/src/algorithms/libs/rtklib/rtklib_preceph.cc

902 lines
31 KiB
C++
Raw Blame History

/*!
* \file rtklib_preceph.cc
* \brief precise ephemeris and clock 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.
*
* 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.
*
*
* References :
* [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
* 12 February, 2007
* [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
* 27 August, 1998
* [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
* [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
* Space Technology Library, 2004
*
*----------------------------------------------------------------------------*/
#include "rtklib_preceph.h"
#include "rtklib_rtkcmn.h"
#include <cstring>
/* satellite code to satellite system ----------------------------------------*/
int code2sys(char code)
{
if (code == 'G' || code == ' ') return SYS_GPS;
if (code == 'R') return SYS_GLO;
if (code == 'E') return SYS_GAL; /* extension to sp3-c */
if (code == 'J') return SYS_QZS; /* extension to sp3-c */
if (code == 'C') return SYS_BDS; /* extension to sp3-c */
if (code == 'L') return SYS_LEO; /* extension to sp3-c */
return SYS_NONE;
}
/* read sp3 header -----------------------------------------------------------*/
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
double *bfact, char *tsys)
{
int i, j, k = 0, ns = 0, sys, prn;
char buff[1024];
trace(3, "readsp3h:\n");
for (i = 0; i < 22; i++)
{
if (!fgets(buff, sizeof(buff), fp)) break;
if (i == 0)
{
*type = buff[2];
if (str2time(buff, 3, 28, time)) return 0;
}
else if (2 <= i && i <= 6)
{
if (i == 2)
{
ns = static_cast<int>(str2num(buff, 4, 2));
}
for (j = 0; j < 17 && k < ns; j++)
{
sys = code2sys(buff[9 + 3 * j]);
prn = static_cast<int>(str2num(buff, 10 + 3 * j, 2));
if (k < MAXSAT) sats[k++] = satno(sys, prn);
}
}
else if (i == 12)
{
strncpy(tsys, buff + 9, 3);
tsys[3] = '\0';
}
else if (i == 14)
{
bfact[0] = str2num(buff, 3, 10);
bfact[1] = str2num(buff, 14, 12);
}
}
return ns;
}
/* add precise ephemeris -----------------------------------------------------*/
int addpeph(nav_t *nav, peph_t *peph)
{
peph_t *nav_peph;
if (nav->ne >= nav->nemax)
{
nav->nemax += 256;
if (!(nav_peph = static_cast<peph_t *>(realloc(nav->peph, sizeof(peph_t) * nav->nemax))))
{
trace(1, "readsp3b malloc error n=%d\n", nav->nemax);
free(nav->peph);
nav->peph = nullptr;
nav->ne = nav->nemax = 0;
return 0;
}
nav->peph = nav_peph;
}
nav->peph[nav->ne++] = *peph;
return 1;
}
/* read sp3 body -------------------------------------------------------------*/
void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, const double *bfact,
char *tsys, int index, int opt, nav_t *nav)
{
peph_t peph;
gtime_t time;
double val, std, base;
int i, j, sat, sys, prn, n = ns * (type == 'P' ? 1 : 2), pred_o, pred_c, v;
char buff[1024];
trace(3, "readsp3b: type=%c ns=%d index=%d opt=%d\n", type, ns, index, opt);
while (fgets(buff, sizeof(buff), fp))
{
if (!strncmp(buff, "EOF", 3)) break;
if (buff[0] != '*' || str2time(buff, 3, 28, &time))
{
trace(2, "sp3 invalid epoch %31.31s\n", buff);
continue;
}
if (!strcmp(tsys, "UTC")) time = utc2gpst(time); /* utc->gpst */
peph.time = time;
peph.index = index;
for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < 4; j++)
{
peph.pos[i][j] = 0.0;
peph.std[i][j] = 0.0f;
peph.vel[i][j] = 0.0;
peph.vst[i][j] = 0.0f;
}
for (j = 0; j < 3; j++)
{
peph.cov[i][j] = 0.0f;
peph.vco[i][j] = 0.0f;
}
}
for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++)
{
if (strlen(buff) < 4 || (buff[0] != 'P' && buff[0] != 'V')) continue;
sys = buff[1] == ' ' ? SYS_GPS : code2sys(buff[1]);
prn = static_cast<int>(str2num(buff, 2, 2));
if (sys == SYS_SBS)
prn += 100;
else if (sys == SYS_QZS)
prn += 192; /* extension to sp3-c */
if (!(sat = satno(sys, prn))) continue;
if (buff[0] == 'P')
{
pred_c = strlen(buff) >= 76 && buff[75] == 'P';
pred_o = strlen(buff) >= 80 && buff[79] == 'P';
}
for (j = 0; j < 4; j++)
{
/* read option for predicted value */
if (j < 3 && (opt & 1) && pred_o) continue;
if (j < 3 && (opt & 2) && !pred_o) continue;
if (j == 3 && (opt & 1) && pred_c) continue;
if (j == 3 && (opt & 2) && !pred_c) continue;
val = str2num(buff, 4 + j * 14, 14);
std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3);
if (buff[0] == 'P')
{ /* position */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
{
peph.pos[sat - 1][j] = val * (j < 3 ? 1000.0 : 1e-6);
v = 1; /* valid epoch */
}
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
{
peph.std[sat - 1][j] = static_cast<float>(std::pow(base, std) * (j < 3 ? 1e-3 : 1e-12));
}
}
else if (v)
{ /* velocity */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
{
peph.vel[sat - 1][j] = val * (j < 3 ? 0.1 : 1e-10);
}
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
{
peph.vst[sat - 1][j] = static_cast<float>(std::pow(base, std) * (j < 3 ? 1e-7 : 1e-16));
}
}
}
}
if (v)
{
if (!addpeph(nav, &peph)) return;
}
}
}
/* compare precise ephemeris -------------------------------------------------*/
int cmppeph(const void *p1, const void *p2)
{
auto *q1 = (peph_t *)p1, *q2 = (peph_t *)p2;
double tt = timediff(q1->time, q2->time);
return tt < -1e-9 ? -1 : (tt > 1e-9 ? 1 : q1->index - q2->index);
}
/* combine precise ephemeris -------------------------------------------------*/
void combpeph(nav_t *nav, int opt)
{
int i, j, k, m;
trace(3, "combpeph: ne=%d\n", nav->ne);
qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph);
if (opt & 4) return;
for (i = 0, j = 1; j < nav->ne; j++)
{
if (fabs(timediff(nav->peph[i].time, nav->peph[j].time)) < 1e-9)
{
for (k = 0; k < MAXSAT; k++)
{
if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0) continue;
for (m = 0; m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
for (m = 0; m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
for (m = 0; m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
for (m = 0; m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m];
}
}
else if (++i < j)
nav->peph[i] = nav->peph[j];
}
nav->ne = i + 1;
trace(4, "combpeph: ne=%d\n", nav->ne);
}
/* read sp3 precise ephemeris file ---------------------------------------------
* read sp3 precise ephemeris/clock files and set them to navigation data
* args : char *file I sp3-c precise ephemeris file
* (wind-card * is expanded)
* nav_t *nav IO navigation data
* int opt I options (1: only observed + 2: only predicted +
* 4: not combined)
* return : none
* notes : see ref [1]
* precise ephemeris is appended and combined
* nav->peph and nav->ne must by properly initialized before calling the
* function
* only files with extensions of .sp3, .SP3, .eph* and .EPH* are read
*-----------------------------------------------------------------------------*/
void readsp3(const char *file, nav_t *nav, int opt)
{
FILE *fp;
gtime_t time = {0, 0};
double bfact[2] = {};
int i, j, n, ns, sats[MAXSAT] = {};
char *efiles[MAXEXFILE], *ext, type = ' ', tsys[4] = "";
trace(3, "readpephs: file=%s\n", file);
for (i = 0; i < MAXEXFILE; i++)
{
if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{
for (i--; i >= 0; i--) free(efiles[i]);
return;
}
}
/* expand wild card in file path */
n = expath(file, efiles, MAXEXFILE);
for (i = j = 0; i < n; i++)
{
if (!(ext = strrchr(efiles[i], '.'))) continue;
if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") &&
!strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue;
if (!(fp = fopen(efiles[i], "re")))
{
trace(2, "sp3 file open error %s\n", efiles[i]);
continue;
}
/* read sp3 header */
ns = readsp3h(fp, &time, &type, sats, bfact, tsys);
/* read sp3 body */
readsp3b(fp, type, sats, ns, bfact, tsys, j++, opt, nav);
fclose(fp);
}
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]);
/* combine precise ephemeris */
if (nav->ne > 0) combpeph(nav, opt);
}
/* read satellite antenna parameters -------------------------------------------
* read satellite antenna parameters
* args : char *file I antenna parameter file
* gtime_t time I time
* nav_t *nav IO navigation data
* return : status (1:ok,0:error)
* notes : only support antex format for the antenna parameter file
*-----------------------------------------------------------------------------*/
int readsap(const char *file, gtime_t time, nav_t *nav)
{
pcvs_t pcvs = {0, 0, (pcv_t *){nullptr}};
pcv_t pcv0 = {0, {}, {}, {0, 0}, {0, 0}, {{}, {}}, {{}, {}}}, *pcv;
int i;
trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0));
if (!readpcv(file, &pcvs)) return 0;
for (i = 0; i < MAXSAT; i++)
{
pcv = searchpcv(i + 1, "", time, &pcvs);
nav->pcvs[i] = pcv ? *pcv : pcv0;
}
free(pcv);
return 1;
}
/* read dcb parameters file --------------------------------------------------*/
int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
{
FILE *fp;
double cbias;
char buff[256], str1[32], str2[32] = "";
int i, j, sat, type = 0;
trace(3, "readdcbf: file=%s\n", file);
if (!(fp = fopen(file, "re")))
{
trace(2, "dcb parameters file open error: %s\n", file);
return 0;
}
while (fgets(buff, sizeof(buff), fp))
{
if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES"))
type = 1;
else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES"))
type = 2;
else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES"))
type = 3;
if (!type || sscanf(buff, "%s %s", str1, str2) < 1) continue;
if ((cbias = str2num(buff, 26, 9)) == 0.0) continue;
if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R")))
{ /* receiver dcb */
for (i = 0; i < MAXRCV; i++)
{
if (!strcmp(sta[i].name, str2)) break;
}
if (i < MAXRCV)
{
j = !strcmp(str1, "G") ? 0 : 1;
nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
}
}
else if ((sat = satid2no(str1)))
{ /* satellite dcb */
nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
}
}
fclose(fp);
return 1;
}
/* read dcb parameters ---------------------------------------------------------
* read differential code bias (dcb) parameters
* args : char *file I dcb parameters file (wild-card * expanded)
* nav_t *nav IO navigation data
* sta_t *sta I station info data to inport receiver dcb
* (NULL: no use)
* return : status (1:ok,0:error)
* notes : currently only p1-c1 bias of code *.dcb file
*-----------------------------------------------------------------------------*/
int readdcb(const char *file, nav_t *nav, const sta_t *sta)
{
int i, j, n;
char *efiles[MAXEXFILE] = {};
trace(3, "readdcb : file=%s\n", file);
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < 3; j++)
{
nav->cbias[i][j] = 0.0;
}
for (i = 0; i < MAXEXFILE; i++)
{
if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{
for (i--; i >= 0; i--) free(efiles[i]);
return 0;
}
}
n = expath(file, efiles, MAXEXFILE);
for (i = 0; i < n; i++)
{
readdcbf(efiles[i], nav, sta);
}
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]);
return 1;
}
/* add satellite fcb ---------------------------------------------------------*/
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std)
{
fcbd_t *nav_fcb;
int i, j;
if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf - 1].ts)) <= 1e-3)
{
for (i = 0; i < 3; i++)
{
nav->fcb[nav->nf - 1].bias[sat - 1][i] = bias[i];
nav->fcb[nav->nf - 1].std[sat - 1][i] = std[i];
}
return 1;
}
if (nav->nf >= nav->nfmax)
{
nav->nfmax = nav->nfmax <= 0 ? 2048 : nav->nfmax * 2;
if (!(nav_fcb = static_cast<fcbd_t *>(realloc(nav->fcb, sizeof(fcbd_t) * nav->nfmax))))
{
free(nav->fcb);
nav->nf = nav->nfmax = 0;
return 0;
}
nav->fcb = nav_fcb;
}
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < 3; j++)
{
nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0;
}
for (i = 0; i < 3; i++)
{
nav->fcb[nav->nf].bias[sat - 1][i] = bias[i];
nav->fcb[nav->nf].std[sat - 1][i] = std[i];
}
nav->fcb[nav->nf].ts = ts;
nav->fcb[nav->nf++].te = te;
return 1;
}
/* read satellite fcb file ---------------------------------------------------*/
int readfcbf(const char *file, nav_t *nav)
{
FILE *fp;
gtime_t ts, te;
double ep1[6], ep2[6], bias[3] = {}, std[3] = {};
char buff[1024], str[32], *p;
int sat;
trace(3, "readfcbf: file=%s\n", file);
if (!(fp = fopen(file, "re")))
{
trace(2, "fcb parameters file open error: %s\n", file);
return 0;
}
while (fgets(buff, sizeof(buff), fp))
{
if ((p = strchr(buff, '#'))) *p = '\0';
if (sscanf(buff,
"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
"%lf %lf %lf %lf %lf %lf",
ep1, ep1 + 1, ep1 + 2, ep1 + 3, ep1 + 4, ep1 + 5,
ep2, ep2 + 1, ep2 + 2, ep2 + 3, ep2 + 4, ep2 + 5, str, bias, std, bias + 1, std + 1,
bias + 2, std + 2) < 17) continue;
if (!(sat = satid2no(str))) continue;
ts = epoch2time(ep1);
te = epoch2time(ep2);
if (!addfcb(nav, ts, te, sat, bias, std))
{
fclose(fp);
return 0;
}
}
fclose(fp);
return 1;
}
/* compare satellite fcb -----------------------------------------------------*/
int cmpfcb(const void *p1, const void *p2)
{
auto *q1 = (fcbd_t *)p1, *q2 = (fcbd_t *)p2;
double tt = timediff(q1->ts, q2->ts);
return tt < -1e-3 ? -1 : (tt > 1e-3 ? 1 : 0);
}
/* read satellite fcb data -----------------------------------------------------
* read satellite fractional cycle bias (dcb) parameters
* args : char *file I fcb parameters file (wild-card * expanded)
* nav_t *nav IO navigation data
* return : status (1:ok,0:error)
* notes : fcb data appended to navigation data
*-----------------------------------------------------------------------------*/
int readfcb(const char *file, nav_t *nav)
{
char *efiles[MAXEXFILE] = {};
int i, n;
trace(3, "readfcb : file=%s\n", file);
for (i = 0; i < MAXEXFILE; i++)
{
if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{
for (i--; i >= 0; i--) free(efiles[i]);
return 0;
}
}
n = expath(file, efiles, MAXEXFILE);
for (i = 0; i < n; i++)
{
readfcbf(efiles[i], nav);
}
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]);
if (nav->nf > 1)
{
qsort(nav->fcb, nav->nf, sizeof(fcbd_t), cmpfcb);
}
return 1;
}
/* polynomial interpolation by Neville's algorithm ---------------------------*/
double interppol(const double *x, double *y, int n)
{
int i, j;
for (j = 1; j < n; j++)
{
for (i = 0; i < n - j; i++)
{
y[i] = (x[i + j] * y[i] - x[i] * y[i + 1]) / (x[i + j] - x[i]);
}
}
return y[0];
}
/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *vare, double *varc)
{
double t[NMAX + 1], p[3][NMAX + 1], c[2], *pos, std = 0.0, s[3], sinl, cosl;
int i, j, k, index;
trace(4, "pephpos : time=%s sat=%2d\n", time_str(time, 3), sat);
rs[0] = rs[1] = rs[2] = dts[0] = 0.0;
if (nav->ne < NMAX + 1 ||
timediff(time, nav->peph[0].time) < -MAXDTE ||
timediff(time, nav->peph[nav->ne - 1].time) > MAXDTE)
{
trace(3, "no prec ephem %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
/* binary search */
for (i = 0, j = nav->ne - 1; i < j;)
{
k = (i + j) / 2;
if (timediff(nav->peph[k].time, time) < 0.0)
i = k + 1;
else
j = k;
}
index = i <= 0 ? 0 : i - 1;
/* polynomial interpolation for orbit */
i = index - (NMAX + 1) / 2;
if (i < 0)
i = 0;
else if (i + NMAX >= nav->ne)
i = nav->ne - NMAX - 1;
for (j = 0; j <= NMAX; j++)
{
t[j] = timediff(nav->peph[i + j].time, time);
if (norm_rtk(nav->peph[i + j].pos[sat - 1], 3) <= 0.0)
{
trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
}
for (j = 0; j <= NMAX; j++)
{
pos = nav->peph[i + j].pos[sat - 1];
#if 0
p[0][j] = pos[0];
p[1][j] = pos[1];
#else
/* correciton for earh rotation ver.2.4.0 */
sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]);
cosl = cos(DEFAULT_OMEGA_EARTH_DOT * t[j]);
p[0][j] = cosl * pos[0] - sinl * pos[1];
p[1][j] = sinl * pos[0] + cosl * pos[1];
#endif
p[2][j] = pos[2];
}
for (i = 0; i < 3; i++)
{
rs[i] = interppol(t, p[i], NMAX + 1);
}
if (vare)
{
for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat - 1][i];
std = norm_rtk(s, 3);
/* extrapolation error for orbit */
if (t[0] > 0.0)
std += EXTERR_EPH * std::pow(t[0], 2.0) / 2.0;
else if (t[NMAX] < 0.0)
std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0;
*vare = std::pow(std, 2.0);
}
/* linear interpolation for clock */
t[0] = timediff(time, nav->peph[index].time);
t[1] = timediff(time, nav->peph[index + 1].time);
c[0] = nav->peph[index].pos[sat - 1][3];
c[1] = nav->peph[index + 1].pos[sat - 1][3];
if (t[0] <= 0.0)
{
if ((dts[0] = c[0]) != 0.0)
{
std = nav->peph[index].std[sat - 1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
}
}
else if (t[1] >= 0.0)
{
if ((dts[0] = c[1]) != 0.0)
{
std = nav->peph[index + 1].std[sat - 1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
}
}
else if (c[0] != 0.0 && c[1] != 0.0)
{
dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
i = t[0] < -t[1] ? 0 : 1;
std = nav->peph[index + i].std[sat - 1][3] + EXTERR_CLK * fabs(t[i]);
}
else
{
dts[0] = 0.0;
}
if (varc) *varc = std::pow(std, 2.0);
return 1;
}
/* satellite clock by precise clock ------------------------------------------*/
int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
double *varc)
{
double t[2], c[2], std;
int i, j, k, index;
trace(4, "pephclk : time=%s sat=%2d\n", time_str(time, 3), sat);
if (nav->nc < 2 ||
timediff(time, nav->pclk[0].time) < -MAXDTE ||
timediff(time, nav->pclk[nav->nc - 1].time) > MAXDTE)
{
trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat);
return 1;
}
/* binary search */
for (i = 0, j = nav->nc - 1; i < j;)
{
k = (i + j) / 2;
if (timediff(nav->pclk[k].time, time) < 0.0)
i = k + 1;
else
j = k;
}
index = i <= 0 ? 0 : i - 1;
/* linear interpolation for clock */
t[0] = timediff(time, nav->pclk[index].time);
t[1] = timediff(time, nav->pclk[index + 1].time);
c[0] = nav->pclk[index].clk[sat - 1][0];
c[1] = nav->pclk[index + 1].clk[sat - 1][0];
if (t[0] <= 0.0)
{
if ((dts[0] = c[0]) == 0.0) return 0;
std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
}
else if (t[1] >= 0.0)
{
if ((dts[0] = c[1]) == 0.0) return 0;
std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
}
else if (c[0] != 0.0 && c[1] != 0.0)
{
dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
i = t[0] < -t[1] ? 0 : 1;
std = nav->pclk[index + i].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]);
}
else
{
trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
if (varc) *varc = std::pow(std, 2.0);
return 1;
}
/* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef
* args : gtime_t time I time (gpst)
* double *rs I satellite position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* int sat I satellite number
* nav_t *nav I navigation data
* double *dant I satellite antenna phase center offset (ecef)
* {dx,dy,dz} (m) (iono-free LC value)
* return : none
*-----------------------------------------------------------------------------*/
void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
double *dant)
{
const double *lam = nav->lam[sat - 1];
const pcv_t *pcv = nav->pcvs + sat - 1;
double ex[3], ey[3], ez[3], es[3], r[3], rsun[3], gmst, erpv[5] = {};
double gamma, C1, C2, dant1, dant2;
int i, j = 0, k = 1;
trace(4, "satantoff: time=%s sat=%2d\n", time_str(time, 3), sat);
/* sun position in ecef */
sunmoonpos(gpst2utc(time), erpv, rsun, nullptr, &gmst);
/* unit vectors of satellite fixed coordinates */
for (i = 0; i < 3; i++) r[i] = -rs[i];
if (!normv3(r, ez)) return;
for (i = 0; i < 3; i++) r[i] = rsun[i] - rs[i];
if (!normv3(r, es)) return;
cross3(ez, es, r);
if (!normv3(r, ey)) return;
cross3(ey, ez, ex);
if (NFREQ >= 3 && (satsys(sat, nullptr) & (SYS_GAL | SYS_SBS))) k = 2;
if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0) return;
gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0);
C1 = gamma / (gamma - 1.0);
C2 = -1.0 / (gamma - 1.0);
/* iono-free LC */
for (i = 0; i < 3; i++)
{
dant1 = pcv->off[j][0] * ex[i] + pcv->off[j][1] * ey[i] + pcv->off[j][2] * ez[i];
dant2 = pcv->off[k][0] * ex[i] + pcv->off[k][1] * ey[i] + pcv->off[k][2] * ez[i];
dant[i] = C1 * dant1 + C2 * dant2;
}
}
/* satellite position/clock by precise ephemeris/clock -------------------------
* compute satellite position/clock with precise ephemeris/clock
* args : gtime_t time I time (gpst)
* int sat I satellite number
* nav_t *nav I navigation data
* int opt I sat position option
* (0: center of mass, 1: antenna phase center)
* double *rs O sat position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* double *dts O sat clock {bias,drift} (s|s/s)
* double *var IO sat position and clock error variance (m)
* (NULL: no output)
* return : status (1:ok,0:error or data outage)
* notes : clock includes relativistic correction but does not contain code bias
* before calling the function, nav->peph, nav->ne, nav->pclk and
* nav->nc must be set by calling readsp3(), readrnx() or readrnxt()
* if precise clocks are not set, clocks in sp3 are used instead
*-----------------------------------------------------------------------------*/
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
double *rs, double *dts, double *var)
{
double rss[3], rst[3], dtss[1], dtst[1], dant[3] = {}, vare = 0.0, varc = 0.0, tt = 1e-3;
int i;
trace(4, "peph2pos: time=%s sat=%2d opt=%d\n", time_str(time, 3), sat, opt);
if (sat <= 0 || MAXSAT < sat) return 0;
/* satellite position and clock bias */
if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) ||
!pephclk(time, sat, nav, dtss, &varc)) return 0;
time = timeadd(time, tt);
if (!pephpos(time, sat, nav, rst, dtst, nullptr, nullptr) ||
!pephclk(time, sat, nav, dtst, nullptr)) return 0;
/* satellite antenna offset correction */
if (opt)
{
satantoff(time, rss, sat, nav, dant);
}
for (i = 0; i < 3; i++)
{
rs[i] = rss[i] + dant[i];
rs[i + 3] = (rst[i] - rss[i]) / tt;
}
/* relativistic effect correction */
if (dtss[0] != 0.0)
{
dts[0] = dtss[0] - 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[1] = (dtst[0] - dtss[0]) / tt;
}
else
{ /* no precise clock */
dts[0] = dts[1] = 0.0;
}
if (var) *var = vare + varc;
return 1;
}
Reference in New Issue View Git Blame Copy Permalink