mirror of https://github.com/gnss-sdr/gnss-sdr
1006 lines
30 KiB
C++
1006 lines
30 KiB
C++
/*!
|
|
* \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.
|
|
*
|
|
* SPDX-License-Identifier: BSD-2-Clause
|
|
*
|
|
*
|
|
* 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;
|
|
int j;
|
|
int k = 0;
|
|
int ns = 0;
|
|
int sys;
|
|
int 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;
|
|
double std;
|
|
double base;
|
|
int i;
|
|
int j;
|
|
int sat;
|
|
int sys;
|
|
int prn;
|
|
int n = ns * (type == 'P' ? 1 : 2);
|
|
int pred_o;
|
|
int pred_c;
|
|
int 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)
|
|
{
|
|
const auto *q1 = static_cast<const peph_t *>(p1);
|
|
const auto *q2 = static_cast<const 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;
|
|
int j;
|
|
int k;
|
|
int 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;
|
|
int j;
|
|
int n;
|
|
int ns;
|
|
int sats[MAXSAT] = {};
|
|
char *efiles[MAXEXFILE];
|
|
char *ext;
|
|
char type = ' ';
|
|
char tsys[4] = "";
|
|
|
|
trace(3, "readpephs: file=%s\n", file);
|
|
|
|
for (i = 0; i < MAXEXFILE; i++)
|
|
{
|
|
if (!(efiles[i] = static_cast<char *>(malloc(MAXSTRPATH + 255))))
|
|
{
|
|
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{};
|
|
pcv_t pcv0{};
|
|
pcv_t *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];
|
|
char str1[32];
|
|
char str2[32] = "";
|
|
int i;
|
|
int j;
|
|
int sat;
|
|
int 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_M_S; /* ns -> m */
|
|
}
|
|
}
|
|
else if ((sat = satid2no(str1)))
|
|
{ /* satellite dcb */
|
|
nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* 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 import 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;
|
|
int j;
|
|
int 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(MAXSTRPATH + 255))))
|
|
{
|
|
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;
|
|
}
|
|
|
|
|
|
/* polynomial interpolation by Neville's algorithm ---------------------------*/
|
|
double interppol(const double *x, double *y, int n)
|
|
{
|
|
int i;
|
|
int 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];
|
|
double p[3][NMAX + 1];
|
|
double c[2];
|
|
double *pos;
|
|
double std = 0.0;
|
|
double s[3];
|
|
double sinl;
|
|
double cosl;
|
|
int i;
|
|
int j;
|
|
int k;
|
|
int 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(GNSS_OMEGA_EARTH_DOT * t[j]);
|
|
cosl = cos(GNSS_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_M_S - 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_M_S + 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];
|
|
double c[2];
|
|
double std;
|
|
int i;
|
|
int j;
|
|
int k;
|
|
int 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_M_S - 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_M_S + 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_M_S + 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];
|
|
double ey[3];
|
|
double ez[3];
|
|
double es[3];
|
|
double r[3];
|
|
double rsun[3];
|
|
double gmst;
|
|
double erpv[5] = {};
|
|
double gamma;
|
|
double C1;
|
|
double C2;
|
|
double dant1;
|
|
double dant2;
|
|
int i;
|
|
int j = 0;
|
|
int 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];
|
|
double rst[3];
|
|
double dtss[1];
|
|
double dtst[1];
|
|
double dant[3] = {};
|
|
double vare = 0.0;
|
|
double varc = 0.0;
|
|
double 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_M_S / SPEED_OF_LIGHT_M_S;
|
|
dts[1] = (dtst[0] - dtss[0]) / tt;
|
|
}
|
|
else
|
|
{ /* no precise clock */
|
|
dts[0] = dts[1] = 0.0;
|
|
}
|
|
if (var)
|
|
{
|
|
*var = vare + varc;
|
|
}
|
|
|
|
return 1;
|
|
}
|