mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-19 00:04:58 +00:00
Code Issues Releases Wiki Activity
7ae4a794f6
gnss-sdr/src/algorithms/libs/rtklib/rtklib_ephemeris.cc
Carles Fernandez 0047595373 Fix defects detected by coverity scan
2017-06-06 18:27:54 +02:00

873 lines
32 KiB
C++
Raw Blame History

/*!
* \file rtklib_ephemeris.cc
* \brief satellite 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.
*
*----------------------------------------------------------------------------*/
#include "rtklib_ephemeris.h"
#include "rtklib_rtkcmn.h"
#include "rtklib_sbas.h"
#include "rtklib_preceph.h"
/* constants ------------------------------------------------------*/
const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */
const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */
const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */
const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */
const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */
const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */
const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */
const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */
const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */
const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */
const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */
const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */
const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */
const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */
const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */
const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */
const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */
const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */
const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */
/* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/
double var_uraeph(int ura)
{
const double ura_value[] = {
2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0,
3072.0, 6144.0
};
return ura < 0 || 14 < ura ? std::pow(6144.0, 2.0) : std::pow(ura_value[ura], 2.0);
}
/* variance by ura ssr (ref [4]) ---------------------------------------------*/
double var_urassr(int ura)
{
double std_;
if (ura <= 0) return std::pow(DEFURASSR, 2.0);
if (ura >= 63) return std::pow(5.4665, 2.0);
std_ = (std::pow((ura >> 3) & 7, 2.0) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3;
return std::pow(std_, 2.0);
}
/* almanac to satellite position and clock bias --------------------------------
* compute satellite position and clock bias with almanac (gps, galileo, qzss)
* args : gtime_t time I time (gpst)
* alm_t *alm I almanac
* double *rs O satellite position (ecef) {x,y,z} (m)
* double *dts O satellite clock bias (s)
* return : none
* notes : see ref [1],[7],[8]
*-----------------------------------------------------------------------------*/
void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts)
{
double tk, M, E, Ek, sinE, cosE, u, r, i, O, x, y, sinO, cosO, cosi, mu;
int n;
trace(4, "alm2pos : time=%s sat=%2d\n", time_str(time, 3), alm->sat);
tk = timediff(time, alm->toa);
if (alm->A <= 0.0)
{
rs[0] = rs[1] = rs[2] = *dts = 0.0;
return;
}
mu = satsys(alm->sat, NULL) == SYS_GAL ? MU_GAL : MU_GPS;
M = alm->M0 + sqrt(mu / (alm->A * alm->A * alm->A)) * tk;
for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++)
{
Ek = E;
E -= (E - alm->e * sin(E) - M) / (1.0 - alm->e * cos(E));
}
if (n >= MAX_ITER_KEPLER)
{
trace(2, "alm2pos: kepler iteration overflow sat=%2d\n", alm->sat);
return;
}
sinE = sin(E);
cosE = cos(E);
u = atan2(sqrt(1.0 - alm->e * alm->e) * sinE, cosE - alm->e) + alm->omg;
r = alm->A * (1.0 - alm->e*cosE);
i = alm->i0;
O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas;
x = r * cos(u);
y = r * sin(u);
sinO = sin(O);
cosO = cos(O);
cosi = cos(i);
rs[0] = x * cosO - y * cosi * sinO;
rs[1] = x * sinO + y * cosi * cosO;
rs[2] = y * sin(i);
*dts = alm->f0 + alm->f1 * tk;
}
/* broadcast ephemeris to satellite clock bias ---------------------------------
* compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss)
* args : gtime_t time I time by satellite clock (gpst)
* eph_t *eph I broadcast ephemeris
* return : satellite clock bias (s) without relativeity correction
* notes : see ref [1],[7],[8]
* satellite clock does not include relativity correction and tdg
*-----------------------------------------------------------------------------*/
double eph2clk(gtime_t time, const eph_t *eph)
{
double t;
int i;
trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat);
t = timediff(time, eph->toc);
for (i = 0; i < 2; i++)
{
t -= eph->f0 + eph->f1 * t + eph->f2 * t * t;
}
return eph->f0 + eph->f1 * t + eph->f2 * t *t;
}
/* broadcast ephemeris to satellite position and clock bias --------------------
* compute satellite position and clock bias with broadcast ephemeris (gps,
* galileo, qzss)
* args : gtime_t time I time (gpst)
* eph_t *eph I broadcast ephemeris
* double *rs O satellite position (ecef) {x,y,z} (m)
* double *dts O satellite clock bias (s)
* double *var O satellite position and clock variance (m^2)
* return : none
* notes : see ref [1],[7],[8]
* satellite clock includes relativity correction without code bias
* (tgd or bgd)
*-----------------------------------------------------------------------------*/
void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
double *var)
{
double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge;
double xg, yg, zg, sino, coso;
int n, sys, prn;
trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat);
if (eph->A <= 0.0)
{
rs[0] = rs[1] = rs[2] = *dts = *var = 0.0;
return;
}
tk = timediff(time , eph->toe);
switch ((sys = satsys(eph->sat, &prn)))
{
case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break;
case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break;
default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break;
}
M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;
for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++)
{
Ek = E;
E -= (E - eph->e * sin(E) - M) / (1.0 - eph->e * cos(E));
}
if (n >= MAX_ITER_KEPLER)
{
trace(2, "eph2pos: kepler iteration overflow sat=%2d\n", eph->sat);
return;
}
sinE = sin(E);
cosE = cos(E);
trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek);
u = atan2(sqrt(1.0 - eph->e*eph->e) * sinE, cosE-eph->e) + eph->omg;
r = eph->A * (1.0 - eph->e * cosE);
i = eph->i0 + eph->idot * tk;
sin2u = sin(2.0 * u); cos2u = cos(2.0 * u);
u += eph->cus * sin2u + eph->cuc * cos2u;
r += eph->crs * sin2u + eph->crc * cos2u;
i += eph->cis * sin2u + eph->cic * cos2u;
x = r * cos(u);
y = r * sin(u);
cosi = cos(i);
/* beidou geo satellite (ref [9]) */
if (sys == SYS_BDS && prn <= 5)
{
O = eph->OMG0 + eph->OMGd * tk - omge * eph->toes;
sinO = sin(O);
cosO = cos(O);
xg = x * cosO - y * cosi * sinO;
yg = x * sinO + y * cosi * cosO;
zg = y * sin(i);
sino = sin(omge * tk);
coso = cos(omge * tk);
rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
rs[1] = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
rs[2] = -yg * SIN_5 + zg * COS_5;
}
else
{
O = eph->OMG0 + (eph->OMGd - omge) * tk - omge * eph->toes;
sinO = sin(O);
cosO = cos(O);
rs[0] = x * cosO - y * cosi * sinO;
rs[1] = x * sinO + y *cosi * cosO;
rs[2] = y * sin(i);
}
tk = timediff(time, eph->toc);
*dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
/* relativity correction */
*dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(SPEED_OF_LIGHT, 2.0);
/* position and clock error variance */
*var = var_uraeph(eph->sva);
}
/* glonass orbit differential equations --------------------------------------*/
void deq(const double *x, double *xdot, const double *acc)
{
double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = std::pow(OMGE_GLO, 2.0);
if (r2 <= 0.0)
{
xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0;
return;
}
/* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */
a = 1.5 * J2_GLO * MU_GLO * std::pow(RE_GLO, 2.0) / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */
b = 5.0 * x[2] * x[2] / r2; /* 5*z^2/r^2 */
c = -MU_GLO / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */
xdot[0] = x[3];
xdot[1] = x[4]; xdot[2] = x[5];
xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0];
xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1];
xdot[5] = (c - 2.0 * a) * x[2] + acc[2];
}
/* glonass position and velocity by numerical integration --------------------*/
void glorbit(double t, double *x, const double *acc)
{
double k1[6], k2[6], k3[6], k4[6], w[6];
int i;
deq(x, k1, acc); for (i = 0; i< 6; i++) w[i] = x[i] + k1[i] * t / 2.0;
deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0;
deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
deq(w, k4, acc);
for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0;
}
/* glonass ephemeris to satellite clock bias -----------------------------------
* compute satellite clock bias with glonass ephemeris
* args : gtime_t time I time by satellite clock (gpst)
* geph_t *geph I glonass ephemeris
* return : satellite clock bias (s)
* notes : see ref [2]
*-----------------------------------------------------------------------------*/
double geph2clk(gtime_t time, const geph_t *geph)
{
double t;
int i;
trace(4, "geph2clk: time=%s sat=%2d\n", time_str(time, 3), geph->sat);
t = timediff(time, geph->toe);
for (i = 0; i < 2; i++)
{
t -= -geph->taun + geph->gamn * t;
}
return -geph->taun + geph->gamn * t;
}
/* glonass ephemeris to satellite position and clock bias ----------------------
* compute satellite position and clock bias with glonass ephemeris
* args : gtime_t time I time (gpst)
* geph_t *geph I glonass ephemeris
* double *rs O satellite position {x,y,z} (ecef) (m)
* double *dts O satellite clock bias (s)
* double *var O satellite position and clock variance (m^2)
* return : none
* notes : see ref [2]
*-----------------------------------------------------------------------------*/
void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
double *var)
{
double t, tt, x[6];
int i;
trace(4, "geph2pos: time=%s sat=%2d\n", time_str(time, 3), geph->sat);
t = timediff(time, geph->toe);
*dts = -geph->taun + geph->gamn * t;
for (i = 0; i < 3; i++)
{
x[i ] = geph->pos[i];
x[i+3] = geph->vel[i];
}
for (tt = t < 0.0 ? - TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt)
{
if (fabs(t) < TSTEP) tt = t;
glorbit(tt, x, geph->acc);
}
for (i = 0; i < 3; i++) rs[i] = x[i];
*var = std::pow(ERREPH_GLO, 2.0);
}
/* sbas ephemeris to satellite clock bias --------------------------------------
* compute satellite clock bias with sbas ephemeris
* args : gtime_t time I time by satellite clock (gpst)
* seph_t *seph I sbas ephemeris
* return : satellite clock bias (s)
* notes : see ref [3]
*-----------------------------------------------------------------------------*/
double seph2clk(gtime_t time, const seph_t *seph)
{
double t;
int i;
trace(4, "seph2clk: time=%s sat=%2d\n", time_str(time, 3), seph->sat);
t = timediff(time, seph->t0);
for (i = 0; i < 2; i++)
{
t-=seph->af0 + seph->af1 * t;
}
return seph->af0 + seph->af1 * t;
}
/* sbas ephemeris to satellite position and clock bias -------------------------
* compute satellite position and clock bias with sbas ephemeris
* args : gtime_t time I time (gpst)
* seph_t *seph I sbas ephemeris
* double *rs O satellite position {x,y,z} (ecef) (m)
* double *dts O satellite clock bias (s)
* double *var O satellite position and clock variance (m^2)
* return : none
* notes : see ref [3]
*-----------------------------------------------------------------------------*/
void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts,
double *var)
{
double t;
int i;
trace(4, "seph2pos: time=%s sat=%2d\n", time_str(time, 3), seph->sat);
t = timediff(time, seph->t0);
for (i = 0; i < 3; i++)
{
rs[i] = seph->pos[i] + seph->vel[i] * t + seph->acc[i] * t * t / 2.0;
}
*dts = seph->af0 + seph->af1 * t;
*var = var_uraeph(seph->sva);
}
/* select ephememeris --------------------------------------------------------*/
eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
{
double t, tmax, tmin;
int i, j = -1;
trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);
switch (satsys(sat, NULL))
{
case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break;
case SYS_GAL: tmax = MAXDTOE_GAL + 1.0; break;
case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; break;
default: tmax = MAXDTOE + 1.0; break;
}
tmin = tmax + 1.0;
for (i = 0; i < nav->n; i++)
{
if (nav->eph[i].sat != sat) continue;
if (iode >= 0 && nav->eph[i].iode != iode) continue;
if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue;
if (iode >= 0) return nav->eph + i;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
}
if (iode >= 0 || j<0)
{
trace(3, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0),
sat, iode);
return NULL;
}
return nav->eph + j;
}
/* select glonass ephememeris ------------------------------------------------*/
geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav)
{
double t, tmax = MAXDTOE_GLO, tmin = tmax + 1.0;
int i, j = -1;
trace(4, "selgeph : time=%s sat=%2d iode=%2d\n", time_str(time, 3), sat, iode);
for (i = 0; i < nav->ng; i++)
{
if (nav->geph[i].sat != sat) continue;
if (iode >= 0 && nav->geph[i].iode != iode) continue;
if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax) continue;
if (iode >= 0) return nav->geph + i;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
}
if (iode >= 0 || j < 0)
{
trace(3, "no glonass ephemeris : %s sat=%2d iode=%2d\n", time_str(time, 0),
sat, iode);
return NULL;
}
return nav->geph + j;
}
/* select sbas ephememeris ---------------------------------------------------*/
seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
{
double t, tmax = MAXDTOE_SBS, tmin = tmax + 1.0;
int i, j = -1;
trace(4, "selseph : time=%s sat=%2d\n", time_str(time, 3), sat);
for (i = 0; i < nav->ns; i++)
{
if (nav->seph[i].sat != sat) continue;
if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) continue;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
}
if (j < 0)
{
trace(3, "no sbas ephemeris : %s sat=%2d\n", time_str(time, 0), sat);
return NULL;
}
return nav->seph + j;
}
/* satellite clock with broadcast ephemeris ----------------------------------*/
int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
int sys;
trace(4, "ephclk : time=%s sat=%2d\n", time_str(time, 3), sat);
sys = satsys(sat, NULL);
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{
if (!(eph = seleph(teph, sat, -1, nav))) return 0;
*dts = eph2clk(time, eph);
}
else if (sys == SYS_GLO)
{
if (!(geph = selgeph(teph, sat, -1, nav))) return 0;
*dts = geph2clk(time, geph);
}
else if (sys == SYS_SBS)
{
if (!(seph = selseph(teph, sat, nav))) return 0;
*dts = seph2clk(time, seph);
}
else return 0;
return 1;
}
/* satellite position and clock by broadcast ephemeris -----------------------*/
int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
double rst[3], dtst[1], tt = 1e-3;
int i, sys;
trace(4, "ephpos : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);
sys = satsys(sat, NULL);
*svh = -1;
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{
if (!(eph = seleph(teph, sat, iode, nav))) return 0;
eph2pos(time, eph, rs, dts, var);
time = timeadd(time, tt);
eph2pos(time, eph, rst, dtst, var);
*svh = eph->svh;
}
else if (sys == SYS_GLO)
{
if (!(geph = selgeph(teph, sat, iode, nav))) return 0;
geph2pos(time, geph, rs, dts, var);
time = timeadd(time, tt);
geph2pos(time, geph, rst, dtst, var);
*svh = geph->svh;
}
else if (sys == SYS_SBS)
{
if (!(seph = selseph(teph, sat, nav))) return 0;
seph2pos(time, seph, rs, dts, var);
time = timeadd(time, tt);
seph2pos(time, seph, rst, dtst, var);
*svh = seph->svh;
}
else return 0;
/* satellite velocity and clock drift by differential approx */
for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt;
dts[1] = (dtst[0] - dts[0]) / tt;
return 1;
}
/* satellite position and clock with sbas correction -------------------------*/
int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *rs, double *dts, double *var, int *svh)
{
const sbssatp_t *sbs;
int i;
trace(4, "satpos_sbas: time=%s sat=%2d\n", time_str(time, 3), sat);
/* search sbas satellite correciton */
for (i = 0; i < nav->sbssat.nsat; i++)
{
sbs = nav->sbssat.sat + i;
if (sbs->sat == sat) break;
}
if (i >= nav->sbssat.nsat)
{
trace(2, "no sbas correction for orbit: %s sat=%2d\n", time_str(time, 0), sat);
ephpos(time, teph, sat, nav, -1, rs, dts, var, svh);
*svh = -1;
return 0;
}
/* satellite postion and clock by broadcast ephemeris */
if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh)) return 0;
/* sbas satellite correction (long term and fast) */
if (sbssatcorr(time, sat, nav, rs, dts, var)) return 1;
*svh = -1;
return 0;
}
/* satellite position and clock with ssr correction --------------------------*/
int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int opt, double *rs, double *dts, double *var, int *svh)
{
const ssr_t *ssr;
eph_t *eph;
double t1, t2, t3, er[3], ea[3], ec[3], rc[3], deph[3], dclk, dant[3] = {0}, tk;
int i, sys;
trace(4, "satpos_ssr: time=%s sat=%2d\n", time_str(time, 3), sat);
ssr = nav->ssr + sat - 1;
if (!ssr->t0[0].time)
{
trace(2, "no ssr orbit correction: %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
if (!ssr->t0[1].time)
{
trace(2, "no ssr clock correction: %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
/* inconsistency between orbit and clock correction */
if (ssr->iod[0] != ssr->iod[1])
{
trace(2, "inconsist ssr correction: %s sat=%2d iod=%d %d\n",
time_str(time, 0), sat, ssr->iod[0], ssr->iod[1]);
*svh = -1;
return 0;
}
t1 = timediff(time, ssr->t0[0]);
t2 = timediff(time, ssr->t0[1]);
t3 = timediff(time, ssr->t0[2]);
/* ssr orbit and clock correction (ref [4]) */
if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR)
{
trace(2, "age of ssr error: %s sat=%2d t=%.0f %.0f\n", time_str(time, 0),
sat, t1, t2);
*svh = -1;
return 0;
}
if (ssr->udi[0] >= 1.0) t1 -= ssr->udi[0] / 2.0;
if (ssr->udi[1] >= 1.0) t2 -= ssr->udi[0] / 2.0;
for (i = 0; i < 3; i++) deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1;
dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2;
/* ssr highrate clock correction (ref [4]) */
if (ssr->iod[0] == ssr->iod[2] && ssr->t0[2].time && fabs(t3) < MAXAGESSR_HRCLK)
{
dclk += ssr->hrclk;
}
if (norm_rtk(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR)
{
trace(3, "invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
time_str(time, 0), norm_rtk(deph, 3), dclk);
*svh = -1;
return 0;
}
/* satellite postion and clock by broadcast ephemeris */
if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh)) return 0;
/* satellite clock for gps, galileo and qzss */
sys = satsys(sat, NULL);
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{
if (!(eph = seleph(teph, sat, ssr->iode, nav))) return 0;
/* satellite clock by clock parameters */
tk=timediff(time, eph->toc);
dts[0] = eph->f0 + eph->f1*tk + eph->f2 * tk * tk;
dts[1] = eph->f1 + 2.0*eph->f2 * tk;
/* relativity correction */
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
}
/* radial-along-cross directions in ecef */
if (!normv3(rs + 3, ea)) return 0;
cross3(rs, rs + 3, rc);
if (!normv3(rc, ec))
{
*svh = -1;
return 0;
}
cross3(ea, ec, er);
/* satellite antenna offset correction */
if (opt)
{
satantoff(time, rs, sat, nav, dant);
}
for (i = 0; i < 3; i++)
{
rs[i] += -(er[i] * deph[0] + ea[i] * deph[1] + ec[i] * deph[2]) + dant[i];
}
/* t_corr = t_sv - (dts(brdc) + dclk(ssr) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */
dts[0] += dclk / SPEED_OF_LIGHT;
/* variance by ssr ura */
*var = var_urassr(ssr->ura);
trace(5, "satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n",
time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var);
return 1;
}
/* satellite position and clock ------------------------------------------------
* compute satellite position, velocity and clock
* args : gtime_t time I time (gpst)
* gtime_t teph I time to select ephemeris (gpst)
* int sat I satellite number
* nav_t *nav I navigation data
* int ephopt I ephemeris option (EPHOPT_???)
* 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 O sat position and clock error variance (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : status (1:ok,0:error)
* notes : satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
*-----------------------------------------------------------------------------*/
int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh)
{
trace(4, "satpos : time=%s sat=%2d ephopt=%d\n", time_str(time, 3), sat, ephopt);
*svh = 0;
switch (ephopt)
{
case EPHOPT_BRDC : return ephpos (time, teph, sat, nav, -1, rs, dts, var, svh);
case EPHOPT_SBAS : return satpos_sbas(time, teph, sat, nav, rs, dts, var, svh);
case EPHOPT_SSRAPC: return satpos_ssr (time, teph, sat, nav, 0, rs, dts, var, svh);
case EPHOPT_SSRCOM: return satpos_ssr (time, teph, sat, nav, 1, rs, dts, var, svh);
case EPHOPT_PREC :
if (!peph2pos(time, sat, nav, 1, rs, dts, var)) break; else return 1;
//TODO: enable lex
//case EPHOPT_LEX :
// if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1;
}
*svh = -1;
return 0;
}
/* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks
* args : gtime_t teph I time to select ephemeris (gpst)
* obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* int ephopt I ephemeris option (EPHOPT_???)
* double *rs O satellite positions and velocities (ecef)
* double *dts O satellite clocks
* double *var O sat position and clock error variances (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : none
* notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m)
* rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s)
* dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s)
* var[i] = obs[i] sat position and clock error variance (m^2)
* svh[i] = obs[i] sat health flag
* if no navigation data, set 0 to rs[], dts[], var[] and svh[]
* satellite position and clock are values at signal transmission time
* satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
* any pseudorange and broadcast ephemeris are always needed to get
* signal transmission time
*-----------------------------------------------------------------------------*/
void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh)
{
gtime_t time[MAXOBS] = {};
double dt, pr;
int i, j;
trace(3, "satposs : teph=%s n=%d ephopt=%d\n", time_str(teph, 3), n, ephopt);
for (i = 0; i < n && i < MAXOBS; i++)
{
for (j = 0; j < 6; j++) rs [j + i * 6] = 0.0;
for (j = 0; j < 2; j++) dts[j + i * 2] = 0.0;
var[i] = 0.0;
svh[i] = 0;
/* search any pseudorange */
for (j = 0, pr = 0.0; j < NFREQ; j++) if ((pr = obs[i].P[j]) != 0.0) break;
if (j >= NFREQ)
{
trace(2, "no pseudorange %s sat=%2d\n", time_str(obs[i].time, 3), obs[i].sat);
continue;
}
/* transmission time by satellite clock */
time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i], teph, obs[i].sat, nav, &dt))
{
trace(3, "no broadcast clock %s sat=%2d\n", time_str(time[i], 3), obs[i].sat);
continue;
}
time[i] = timeadd(time[i], -dt);
/* satellite position and clock at transmission time */
if (!satpos(time[i], teph, obs[i].sat, ephopt, nav, rs + i * 6, dts + i * 2, var + i,
svh + i))
{
trace(3, "no ephemeris %s sat=%2d\n", time_str(time[i], 3), obs[i].sat);
continue;
}
/* if no precise clock available, use broadcast clock instead */
if (dts[i * 2] == 0.0)
{
if (!ephclk(time[i], teph, obs[i].sat, nav, dts + i * 2)) continue;
dts[1 + i * 2] = 0.0;
*var = std::pow(STD_BRDCCLK, 2.0);
}
}
for (i = 0; i < n && i < MAXOBS; i++)
{
trace(4, "%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i], 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
dts[i * 2] * 1e9, var[i], svh[i]);
}
}
Reference in New Issue View Git Blame Copy Permalink