1072 lines
40 KiB
C
1072 lines
40 KiB
C
|
/*------------------------------------------------------------------------------
|
|||
|
|
* ephemeris.c : satellite ephemeris and clock functions
|
|||
|
|
*
|
|||
|
|
* Copyright (C) 2010-2020 by T.TAKASU, All rights reserved.
|
|||
|
|
*
|
|||
|
|
* references :
|
|||
|
|
* [1] IS-GPS-200K, Navstar GPS Space Segment/Navigation User Interfaces,
|
|||
|
|
* May 6, 2019
|
|||
|
|
* [2] Global Navigation Satellite System GLONASS, Interface Control Document
|
|||
|
|
* Navigational radiosignal In bands L1, L2, (Version 5.1), 2008
|
|||
|
|
* [3] RTCA/DO-229C, Minimum operational performance standards for global
|
|||
|
|
* positioning system/wide area augmentation system airborne equipment,
|
|||
|
|
* RTCA inc, November 28, 2001
|
|||
|
|
* [4] RTCM Paper, April 12, 2010, Proposed SSR Messages for SV Orbit Clock,
|
|||
|
|
* Code Biases, URA
|
|||
|
|
* [5] RTCM Paper 012-2009-SC104-528, January 28, 2009 (previous ver of [4])
|
|||
|
|
* [6] RTCM Paper 012-2009-SC104-582, February 2, 2010 (previous ver of [4])
|
|||
|
|
* [7] European GNSS (Galileo) Open Service Signal In Space Interface Control
|
|||
|
|
* Document, Issue 1.3, December, 2016
|
|||
|
|
* [8] Quasi-Zenith Satellite System Interface Specification Satellite
|
|||
|
|
* Positioning, Navigation and Timing Service (IS-QZSS-PNT-003), Cabinet
|
|||
|
|
* Office, November 5, 2018
|
|||
|
|
* [9] BeiDou navigation satellite system signal in space interface control
|
|||
|
|
* document open service signal B1I (version 3.0), China Satellite
|
|||
|
|
* Navigation office, February, 2019
|
|||
|
|
* [10] RTCM Standard 10403.3, Differential GNSS (Global Navigation
|
|||
|
|
* Satellite Systems) Services - version 3, October 7, 2016
|
|||
|
|
*
|
|||
|
|
* version : $Revision:$ $Date:$
|
|||
|
|
* history : 2010/07/28 1.1 moved from rtkcmn.c
|
|||
|
|
* added api:
|
|||
|
|
* eph2clk(),geph2clk(),seph2clk(),satantoff()
|
|||
|
|
* satposs()
|
|||
|
|
* changed api:
|
|||
|
|
* eph2pos(),geph2pos(),satpos()
|
|||
|
|
* deleted api:
|
|||
|
|
* satposv(),satposiode()
|
|||
|
|
* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX
|
|||
|
|
* 2010/09/09 1.3 fix problem when precise clock outage
|
|||
|
|
* 2011/01/12 1.4 add api alm2pos()
|
|||
|
|
* change api satpos(),satposs()
|
|||
|
|
* enable valid unhealthy satellites and output status
|
|||
|
|
* fix bug on exception by glonass ephem computation
|
|||
|
|
* 2013/01/10 1.5 support beidou (compass)
|
|||
|
|
* use newton's method to solve kepler eq.
|
|||
|
|
* update ssr correction algorithm
|
|||
|
|
* 2013/03/20 1.6 fix problem on ssr clock relativitic correction
|
|||
|
|
* 2013/09/01 1.7 support negative pseudorange
|
|||
|
|
* fix bug on variance in case of ura ssr = 63
|
|||
|
|
* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0
|
|||
|
|
* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15<ura
|
|||
|
|
* 2014/12/07 1.10 modify MAXDTOE for qzss,gal and bds
|
|||
|
|
* test max number of iteration for Kepler
|
|||
|
|
* 2015/08/26 1.11 update RTOL_ELPLER 1E-14 -> 1E-13
|
|||
|
|
* set MAX_ITER_KEPLER for alm2pos()
|
|||
|
|
* 2017/04/11 1.12 fix bug on max number of obs data in satposs()
|
|||
|
|
* 2018/10/10 1.13 update reference [7]
|
|||
|
|
* support ura value in var_uraeph() for galileo
|
|||
|
|
* test eph->flag to recognize beidou geo
|
|||
|
|
* add api satseleph() for ephemeris selection
|
|||
|
|
* 2020/11/30 1.14 update references [1],[2],[8],[9] and [10]
|
|||
|
|
* add API getseleph()
|
|||
|
|
* rename API satseleph() as setseleph()
|
|||
|
|
* support NavIC/IRNSS by API satpos() and satposs()
|
|||
|
|
* support BDS C59-63 as GEO satellites in eph2pos()
|
|||
|
|
* default selection of I/NAV for Galileo ephemeris
|
|||
|
|
* no support EPHOPT_LEX by API satpos() and satposs()
|
|||
|
|
* unselect Galileo ephemeris with AOD<=0 in seleph()
|
|||
|
|
* fix bug on clock iteration in eph2clk(), geph2clk()
|
|||
|
|
* fix bug on clock reference time in satpos_ssr()
|
|||
|
|
* fix bug on wrong value with ura=15 in var_ura()
|
|||
|
|
* use integer types in stdint.h
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
#include "rtklib.h"
|
|||
|
|
|
|||
|
|
/* constants and macros ------------------------------------------------------*/
|
|||
|
|
|
|||
|
|
#define SQR(x) ((x) * (x))
|
|||
|
|
|
|||
|
|
#define RE_GLO 6378136.0 /* radius of earth (m) ref [2] */
|
|||
|
|
#define MU_GPS 3.9860050E14 /* gravitational constant ref [1] */
|
|||
|
|
#define MU_GLO 3.9860044E14 /* gravitational constant ref [2] */
|
|||
|
|
#define MU_GAL 3.986004418E14 /* earth gravitational constant ref [7] */
|
|||
|
|
#define MU_CMP 3.986004418E14 /* earth gravitational constant ref [9] */
|
|||
|
|
#define J2_GLO 1.0826257E-3 /* 2nd zonal harmonic of geopot ref [2] */
|
|||
|
|
|
|||
|
|
#define OMGE_GLO 7.292115E-5 /* earth angular velocity (rad/s) ref [2] */
|
|||
|
|
#define OMGE_GAL 7.2921151467E-5 /* earth angular velocity (rad/s) ref [7] */
|
|||
|
|
#define OMGE_CMP 7.292115E-5 /* earth angular velocity (rad/s) ref [9] */
|
|||
|
|
|
|||
|
|
#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */
|
|||
|
|
#define COS_5 0.9961946980917456 /* cos(-5.0 deg) */
|
|||
|
|
|
|||
|
|
#define ERREPH_GLO 5.0 /* error of glonass ephemeris (m) */
|
|||
|
|
#define TSTEP 60.0 /* integration step glonass ephemeris (s) */
|
|||
|
|
#define RTOL_KEPLER 1E-13 /* relative tolerance for Kepler equation */
|
|||
|
|
|
|||
|
|
#define DEFURASSR 0.15 /* default accurary of ssr corr (m) */
|
|||
|
|
#define MAXECORSSR 10.0 /* max orbit correction of ssr (m) */
|
|||
|
|
#define MAXCCORSSR (1E-6 * CLIGHT) /* max clock correction of ssr (m) */
|
|||
|
|
#define MAXAGESSR 90.0 /* max age of ssr orbit and clock (s) */
|
|||
|
|
#define MAXAGESSR_HRCLK 10.0 /* max age of ssr high-rate clock (s) */
|
|||
|
|
#define STD_BRDCCLK 30.0 /* error of broadcast clock (m) */
|
|||
|
|
#define STD_GAL_NAPA 500.0 /* error of galileo ephemeris for NAPA (m) */
|
|||
|
|
|
|||
|
|
#define MAX_ITER_KEPLER 30 /* max number of iteration of Kelpler */
|
|||
|
|
|
|||
|
|
/* ephemeris selections ------------------------------------------------------*/
|
|||
|
|
static int eph_sel[] = {/* GPS,GLO,GAL,QZS,BDS,IRN,SBS */
|
|||
|
|
0, 0, 0, 0, 0, 0, 0};
|
|||
|
|
|
|||
|
|
/* variance by ura ephemeris -------------------------------------------------*/
|
|||
|
|
static double var_uraeph(int sys, 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};
|
|||
|
|
if (sys == SYS_GAL)
|
|||
|
|
{ /* galileo sisa (ref [7] 5.1.11) */
|
|||
|
|
if (ura <= 49)
|
|||
|
|
return SQR(ura * 0.01);
|
|||
|
|
if (ura <= 74)
|
|||
|
|
return SQR(0.5 + (ura - 50) * 0.02);
|
|||
|
|
if (ura <= 99)
|
|||
|
|
return SQR(1.0 + (ura - 75) * 0.04);
|
|||
|
|
if (ura <= 125)
|
|||
|
|
return SQR(2.0 + (ura - 100) * 0.16);
|
|||
|
|
return SQR(STD_GAL_NAPA);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{ /* gps ura (ref [1] 20.3.3.3.1.1) */
|
|||
|
|
return ura < 0 || 14 < ura ? SQR(6144.0) : SQR(ura_value[ura]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* variance by ura ssr (ref [10] table 3.3-1 DF389) --------------------------*/
|
|||
|
|
static double var_urassr(int ura)
|
|||
|
|
{
|
|||
|
|
double std;
|
|||
|
|
if (ura <= 0)
|
|||
|
|
return SQR(DEFURASSR);
|
|||
|
|
if (ura >= 63)
|
|||
|
|
return SQR(5.4665);
|
|||
|
|
std = (pow(3.0, (ura >> 3) & 7) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1E-3;
|
|||
|
|
return SQR(std);
|
|||
|
|
}
|
|||
|
|
/* 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]
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
// extern 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-OMGE)*tk-OMGE*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,beidou)
|
|||
|
|
* 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
|
|||
|
|
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>
|
|||
|
|
gtime_t time I time by satellite clock (gpst)
|
|||
|
|
eph_t *eph I broadcast ephemeris
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>
|
|||
|
|
double satellite clock bias (s) without relativeity correction
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern double eph2clk(gtime_t time, const eph_t *eph)
|
|||
|
|
{
|
|||
|
|
double t, ts;
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB> dt = t-toc
|
|||
|
|
t = ts = timediff(time, eph->toc);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ö<EFBFBD><C3B6><EFBFBD>ʽУ<CABD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<D3B2><EEA3AC> dt<64>м<EFBFBD>ȥ<EFBFBD>ⲿ<EFBFBD>֣<EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>յ<EFBFBD> dt(<28><>һ<EFBFBD><D2BB><EFBFBD>ֲ<EFBFBD>֪<EFBFBD><D6AA><EFBFBD><EFBFBD>Ϊʲô<CAB2><C3B4>)
|
|||
|
|
for (i = 0; i < 2; i++)
|
|||
|
|
{
|
|||
|
|
t = ts - (eph->f0 + eph->f1 * t + eph->f2 * t * t);
|
|||
|
|
}
|
|||
|
|
//ʹ<>ö<EFBFBD><C3B6><EFBFBD>ʽУ<CABD><D0A3><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>յ<EFBFBD><D5B5>Ӳ<EFBFBD>
|
|||
|
|
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)
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD>ݹ㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>
|
|||
|
|
gtime_t time I transmission time by satellite clock
|
|||
|
|
eph_t *eph I <EFBFBD>㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *var O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern 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);
|
|||
|
|
// 1<><31>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD><EFBFBD>볤<EFBFBD><EBB3A4> A <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>
|
|||
|
|
if (eph->A <= 0.0)
|
|||
|
|
{
|
|||
|
|
rs[0] = rs[1] = rs[2] = *dts = *var = 0.0;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
// 2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滯ʱ<E6BBAF><CAB1> tk
|
|||
|
|
tk = timediff(time, eph->toe);
|
|||
|
|
|
|||
|
|
// 3<><33><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> mu <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ٶ<EFBFBD> omge
|
|||
|
|
switch ((sys = satsys(eph->sat, &prn)))
|
|||
|
|
{
|
|||
|
|
case SYS_GAL:
|
|||
|
|
mu = MU_GAL;
|
|||
|
|
omge = OMGE_GAL;
|
|||
|
|
break;
|
|||
|
|
case SYS_CMP:
|
|||
|
|
mu = MU_CMP;
|
|||
|
|
omge = OMGE_CMP;
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
mu = MU_GPS;
|
|||
|
|
omge = OMGE;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
// 4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> M
|
|||
|
|
M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;
|
|||
|
|
|
|||
|
|
// 5<><35><EFBFBD><EFBFBD>ţ<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD> E<><45><EFBFBD>ο<EFBFBD> RTKLIB manual P145 (E.4.19)
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
// 6<><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD> u
|
|||
|
|
u = atan2(sqrt(1.0 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg;
|
|||
|
|
|
|||
|
|
// 7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶯У<E3B6AF><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD> u<><75><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> r<><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> i<><69>
|
|||
|
|
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 */
|
|||
|
|
if (sys == SYS_CMP && (prn <= 5 || prn >= 59))
|
|||
|
|
{ /* ref [9] table 4-1 */
|
|||
|
|
// 8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ O
|
|||
|
|
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);
|
|||
|
|
// 9<><39><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ô<EFBFBD><C3B4><EFBFBD> rs <20><>
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
// 10<31><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<D3B2>˴<EFBFBD><CBB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ЧӦ<D0A7><D3A6>û<EFBFBD>п<EFBFBD><D0BF><EFBFBD> TGD<47><44>Ҳû<D2B2>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>Ư
|
|||
|
|
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 / SQR(CLIGHT);
|
|||
|
|
|
|||
|
|
// 11<31><31><EFBFBD><EFBFBD> URA ֵ<><D6B5><EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD> ICD-GPS-200H 20.3.3.3.1.3 SV Accuracy <20><><EFBFBD>ҵ<EFBFBD>
|
|||
|
|
/* position and clock error variance */
|
|||
|
|
*var = var_uraeph(sys, eph->sva);
|
|||
|
|
}
|
|||
|
|
/* glonass orbit differential equations --------------------------------------*/
|
|||
|
|
static void deq(const double *x, double *xdot, const double *acc)
|
|||
|
|
{
|
|||
|
|
double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = SQR(OMGE_GLO);
|
|||
|
|
|
|||
|
|
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 * SQR(RE_GLO) / 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 --------------------*/
|
|||
|
|
static 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]
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern double geph2clk(gtime_t time, const geph_t *geph)
|
|||
|
|
{
|
|||
|
|
double t, ts;
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
trace(4, "geph2clk: time=%s sat=%2d\n", time_str(time, 3), geph->sat);
|
|||
|
|
|
|||
|
|
t = ts = timediff(time, geph->toe);
|
|||
|
|
|
|||
|
|
for (i = 0; i < 2; i++)
|
|||
|
|
{
|
|||
|
|
t = ts - (-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]
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern 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 = SQR(ERREPH_GLO);
|
|||
|
|
}
|
|||
|
|
/* 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]
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
// extern 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]
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
// extern 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(SYS_SBS,seph->sva);
|
|||
|
|
// }
|
|||
|
|
/* select ephememeris --------------------------------------------------------*/
|
|||
|
|
static eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
|
|||
|
|
{
|
|||
|
|
double t, tmax, tmin;
|
|||
|
|
int i, j = -1, sys, sel = 0;
|
|||
|
|
|
|||
|
|
trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);
|
|||
|
|
|
|||
|
|
sys = satsys(sat, NULL);
|
|||
|
|
switch (sys)
|
|||
|
|
{
|
|||
|
|
case SYS_GPS:
|
|||
|
|
tmax = MAXDTOE + 1.0;
|
|||
|
|
sel = eph_sel[0];
|
|||
|
|
break;
|
|||
|
|
case SYS_GAL:
|
|||
|
|
tmax = MAXDTOE_GAL;
|
|||
|
|
sel = eph_sel[2];
|
|||
|
|
break;
|
|||
|
|
case SYS_QZS:
|
|||
|
|
tmax = MAXDTOE_QZS + 1.0;
|
|||
|
|
sel = eph_sel[3];
|
|||
|
|
break;
|
|||
|
|
case SYS_CMP:
|
|||
|
|
tmax = MAXDTOE_CMP + 1.0;
|
|||
|
|
sel = eph_sel[4];
|
|||
|
|
break;
|
|||
|
|
case SYS_IRN:
|
|||
|
|
tmax = MAXDTOE_IRN + 1.0;
|
|||
|
|
sel = eph_sel[5];
|
|||
|
|
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 (sys == SYS_GAL)
|
|||
|
|
{
|
|||
|
|
sel = getseleph(SYS_GAL);
|
|||
|
|
/* this code is from 2.4.3 b34 but does not seem to be fully supported,
|
|||
|
|
so for now I have dropped back to the b33 code */
|
|||
|
|
/* if (sel==0&&!(nav->eph[i].code&(1<<9))) continue; */ /* I/NAV */
|
|||
|
|
/*if (sel==1&&!(nav->eph[i].code&(1<<8))) continue; */ /* F/NAV */
|
|||
|
|
if (sel == 1 && !(nav->eph[i].code & (1 << 9)))
|
|||
|
|
continue; /* I/NAV */
|
|||
|
|
if (sel == 2 && !(nav->eph[i].code & (1 << 8)))
|
|||
|
|
continue; /* F/NAV */
|
|||
|
|
if (timediff(nav->eph[i].toe, time) >= 0.0) /* <20><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>Чʹ<D0A7><CAB9>ʱ<EFBFBD><CAB1> */
|
|||
|
|
continue; /* AOD<=0 */
|
|||
|
|
}
|
|||
|
|
if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
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, 3), sat, iode);
|
|||
|
|
return NULL;
|
|||
|
|
}
|
|||
|
|
return nav->eph + j;
|
|||
|
|
}
|
|||
|
|
/* select glonass ephememeris ------------------------------------------------*/
|
|||
|
|
static 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 ---------------------------------------------------*/
|
|||
|
|
// static 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
|
|||
|
|
ͨ<EFBFBD><EFBFBD><EFBFBD>㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
gtime_t time I <EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
|
|||
|
|
gtime_t teph I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD> (gpst)
|
|||
|
|
int sat I <EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD> (1-MAXSAT)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *dts O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
|
|||
|
|
ע<EFBFBD>⣺<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>п<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ЧӦ<EFBFBD><EFBFBD> TGD<EFBFBD>ġ<EFBFBD>
|
|||
|
|
----------------------------------*/
|
|||
|
|
static 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);
|
|||
|
|
|
|||
|
|
/*<2A><><EFBFBD><EFBFBD> satsys <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ϵͳ<CFB5><EFBFBD><CDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڸ<EFBFBD>ϵͳ<CFB5>е<EFBFBD> PRN<52><4E><EFBFBD><EFBFBD>*/
|
|||
|
|
sys = satsys(sat, NULL);
|
|||
|
|
|
|||
|
|
/*<2A><><EFBFBD><EFBFBD> GPS٤<53><D9A4><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> seleph <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD> teph <20><><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_CMP || sys == SYS_IRN)
|
|||
|
|
{
|
|||
|
|
if (!(eph = seleph(teph, sat, -1, nav)))
|
|||
|
|
return 0;
|
|||
|
|
*dts = eph2clk(time, eph); //<2F><><EFBFBD><EFBFBD> eph2clk <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<C5BA><C5B7><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
}
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ݹ㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD> P<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>C
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
gtime_t time I transmission time by satellite clock
|
|||
|
|
gtime_t teph I time to select ephemeris (gpst)
|
|||
|
|
int sat I <EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD> (1-MAXSAT)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int iode I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ں<EFBFBD>
|
|||
|
|
double *rs O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *var O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
int *svh O <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ (-1:correction not available)
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
-----------------------*/
|
|||
|
|
static 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);
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satsys <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ϵͳ
|
|||
|
|
sys = satsys(sat, NULL);
|
|||
|
|
|
|||
|
|
*svh = -1;
|
|||
|
|
// 2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD> seleph <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD><EFBFBD>
|
|||
|
|
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_CMP || sys == SYS_IRN)
|
|||
|
|
{
|
|||
|
|
if (!(eph = seleph(teph, sat, iode, nav)))
|
|||
|
|
return 0;
|
|||
|
|
// 3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD>еĹ㲥<C4B9><E3B2A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> eph2pos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<C5BA><C5B7><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD> λ<>á<EFBFBD><C3A1>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5>õ<EFBFBD> eph2pos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>Ӳ<D3B2><EEBFBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ЧӦ<D0A7><D3A6>û<EFBFBD>п<EFBFBD><D0BF><EFBFBD> TGD
|
|||
|
|
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
|
|||
|
|
4<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>̵Ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϸ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٴμ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>̵<EFBFBD> P<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>C<EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD>ͨ<EFBFBD><EFBFBD><EFBFBD>Ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD>ٶȺ<EFBFBD>ƵƯ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD>Ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD>ٶȺ<EFBFBD>ƵƯ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Щλ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳʽ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
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 -------------------------*/
|
|||
|
|
// static 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=NULL;
|
|||
|
|
// 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, use brdcast: %s sat=%2d\n",time_str(time,0),sat);
|
|||
|
|
// ephpos(time,teph,sat,nav,-1,rs,dts,var,svh);
|
|||
|
|
// /* *svh=-1; */ /* use broadcast if no sbas */
|
|||
|
|
// return 1;
|
|||
|
|
// }
|
|||
|
|
// /* 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 --------------------------*/
|
|||
|
|
// static 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[1]/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(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) {
|
|||
|
|
// trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
|
|||
|
|
// time_str(time,0),norm(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_CMP) {
|
|||
|
|
// 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)/CLIGHT/CLIGHT;
|
|||
|
|
// }
|
|||
|
|
// /* 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) / CLIGHT) (ref [10] eq.3.12-7) */
|
|||
|
|
// dts[0]+=dclk/CLIGHT;
|
|||
|
|
//
|
|||
|
|
// /* 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 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD><EFBFBD>ٶȡ<EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
* 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)
|
|||
|
|
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
gtime_t time I time (gpst)
|
|||
|
|
gtime_t teph I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD> (gpst)
|
|||
|
|
int sat I <EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int ephopt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD> (EPHOPT_???)
|
|||
|
|
double *rs O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *var O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
int *svh O <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ (-1:correction not available)
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern 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;
|
|||
|
|
/*<2A><><EFBFBD>ݲ<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ò<EFBFBD>ͬ<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD> EPHOPT_BRDC<44><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ephpos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ݹ㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD> P<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>C<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> EPHOPT_PREC<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> peph2pos <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>Ӽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD> P<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>C<EFBFBD><EFBFBD>
|
|||
|
|
*/
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
*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
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern 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 = {0};
|
|||
|
|
double dt, pr;
|
|||
|
|
int i, j;
|
|||
|
|
|
|||
|
|
trace(3, "satposs : teph=%s n=%d ephopt=%d\n", time_str(teph, 3), n, ephopt);
|
|||
|
|
|
|||
|
|
/*<2A><>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿһ<C3BF><D2BB><EFBFBD>۲<EFBFBD><DBB2><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ȳ<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>ǰ<EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD> rs<EFBFBD><EFBFBD>dts<EFBFBD><EFBFBD>var<EFBFBD><EFBFBD>svh<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0 */
|
|||
|
|
for (i = 0; i < n && i < 2 * 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
|
|||
|
|
<EFBFBD>ж<EFBFBD>ijһƵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŵ<EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ 0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>Ƶ<EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣬Ƶ<EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><EFBFBD><EFBFBD> NFREQ<EFBFBD><EFBFBD>Ĭ<EFBFBD><EFBFBD>Ϊ 3<EFBFBD><EFBFBD>*/
|
|||
|
|
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
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>̼<EFBFBD>ȥα<EFBFBD><EFBFBD><EFBFBD>źŴ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䣬<EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŵķ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>*/
|
|||
|
|
time = timeadd(obs[i].time, -pr / CLIGHT);
|
|||
|
|
|
|||
|
|
/* satellite clock bias by broadcast ephemeris
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ephclk <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ㲥<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> GPS ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD> dt<EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣬<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>п<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ЧӦ<EFBFBD><EFBFBD> TGD <EFBFBD><EFBFBD>*/
|
|||
|
|
if (!ephclk(time, teph, obs[i].sat, nav, &dt))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>źŷ<C5BA><C5B7><EFBFBD>ʱ<EFBFBD>̼<EFBFBD>ȥ<EFBFBD>Ӳ<EFBFBD> dt<64><74><EFBFBD>õ<EFBFBD> GPS ʱ<><CAB1><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<C5BA><C5B7><EFBFBD>ʱ<EFBFBD><CAB1>
|
|||
|
|
time = timeadd(time, -dt);
|
|||
|
|
|
|||
|
|
/* satellite position and clock at transmission time
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> satpos <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD><EFBFBD>(ecef,m)<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>(ecef,m/s)<EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD>((s|s/s))<EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ЧӦ<EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD>ֻ<EFBFBD>ǻ<EFBFBD>û<EFBFBD>п<EFBFBD><EFBFBD><EFBFBD> TGD<EFBFBD><EFBFBD>*/
|
|||
|
|
if (!satpos(time, teph, obs[i].sat, ephopt, nav, rs + i * 6, dts + i * 2, var + i, svh + i))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* <20><><EFBFBD><EFBFBD>û<EFBFBD>о<EFBFBD><D0BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ù㲥<C3B9><E3B2A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
if (dts[i * 2] == 0.0)
|
|||
|
|
{
|
|||
|
|
if (!ephclk(time, teph, obs[i].sat, nav, dts + i * 2))
|
|||
|
|
continue;
|
|||
|
|
dts[1 + i * 2] = 0.0;
|
|||
|
|
*var = SQR(STD_BRDCCLK);
|
|||
|
|
}
|
|||
|
|
trace(3, "satposs: %s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
|
|||
|
|
time_str(time, 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
|
|||
|
|
dts[i * 2] * 1E9, var[i], svh[i]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* set selected satellite ephemeris --------------------------------------------
|
|||
|
|
* Set selected satellite ephemeris for multiple ones like LNAV - CNAV, I/NAV -
|
|||
|
|
* F/NAV. Call it before calling satpos(),satposs() to use unselected one.
|
|||
|
|
* args : int sys I satellite system (SYS_???)
|
|||
|
|
* int sel I selection of ephemeris
|
|||
|
|
* GPS,QZS : 0:LNAV ,1:CNAV (default: LNAV)
|
|||
|
|
* b33 and demo5 b34: GAL: 0:any,1:I/NAV,2:F/NAV
|
|||
|
|
* 2.4.3 b34 but not functional? GAL : 0:I/NAV,1:F/NAV (default: I/NAV)
|
|||
|
|
* others : undefined
|
|||
|
|
* return : none
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern void setseleph(int sys, int sel)
|
|||
|
|
{
|
|||
|
|
switch (sys)
|
|||
|
|
{
|
|||
|
|
case SYS_GPS:
|
|||
|
|
eph_sel[0] = sel;
|
|||
|
|
break;
|
|||
|
|
case SYS_GLO:
|
|||
|
|
eph_sel[1] = sel;
|
|||
|
|
break;
|
|||
|
|
case SYS_GAL:
|
|||
|
|
eph_sel[2] = sel;
|
|||
|
|
break;
|
|||
|
|
case SYS_QZS:
|
|||
|
|
eph_sel[3] = sel;
|
|||
|
|
break;
|
|||
|
|
case SYS_CMP:
|
|||
|
|
eph_sel[4] = sel;
|
|||
|
|
break;
|
|||
|
|
case SYS_IRN:
|
|||
|
|
eph_sel[5] = sel;
|
|||
|
|
break;
|
|||
|
|
// case SYS_SBS: eph_sel[6]=sel; break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* get selected satellite ephemeris -------------------------------------------
|
|||
|
|
* Get the selected satellite ephemeris.
|
|||
|
|
* args : int sys I satellite system (SYS_???)
|
|||
|
|
* return : selected ephemeris
|
|||
|
|
* refer setseleph()
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern int getseleph(int sys)
|
|||
|
|
{
|
|||
|
|
switch (sys)
|
|||
|
|
{
|
|||
|
|
case SYS_GPS:
|
|||
|
|
return eph_sel[0];
|
|||
|
|
case SYS_GLO:
|
|||
|
|
return eph_sel[1];
|
|||
|
|
case SYS_GAL:
|
|||
|
|
return eph_sel[2];
|
|||
|
|
case SYS_QZS:
|
|||
|
|
return eph_sel[3];
|
|||
|
|
case SYS_CMP:
|
|||
|
|
return eph_sel[4];
|
|||
|
|
case SYS_IRN:
|
|||
|
|
return eph_sel[5];
|
|||
|
|
// case SYS_SBS: return eph_sel[6];
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
}
|