代码拉取完成，页面将自动刷新
#include <math.h>
#include "rtk.h"
#include "main.h"
#define STD_BRDCCLK 30.0 /* error of broadcast clock (m) */
/* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/
static 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||15<ura?SQR(6144.0):SQR(ura_value[ura]);
}
//satellite clock does not include relativity correction and tdg
/* -- double eph2clk(gtime_t time,const eph_t *eph) --------------------------------------
*
* Description : broadcast ephemeris to sat clock bias
* Parameters : time I time by sat clock
* eph I broadcast ephemeris
* Return : sat clock bias
*/
double eph2clk(gtime_t time,const eph_t *eph)
{
double t;
int i;
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;
}
/* -- void eph2pos(gtime_t time, const eph_t *eph,double *rs,double *dts,double *var)--------------------------------------
* Description : broadcast ephemeris to sat position and clock bias
* Parameters :
* time I transmission time Tsv
* rs O sat positions and velocities
* dts O sat clock correction (bias, drift)
* var O sat positions and clock error variance
* Return : 0 no ephemeris
*/
void eph2pos(gtime_t time, const eph_t *eph,double *rs,double *dts,double *var)
{
double Mk,tk,Ek,E,sinEk,cosEk,phik,sin2p,cos2p;
double uk,rk,ik,e=eph->e,xk,yk;
tk=timediff(time,eph->toe);
Mk=eph->M0+(sqrt(MUY/(eph->A*eph->A*eph->A))+eph->deln)*tk;
for (Ek=Mk,E=0.0;fabs(Ek-E)>1e-14;)//newton method
{
E=Ek;
Ek-=(Ek-e*sin(Ek)-Mk)/(1.0-e*cos(Ek));
}
//start=TIM2->CNT;
sinEk=sin(Ek);
//SendIntStr(TIM2->CNT-start);
cosEk=cos(Ek);
phik=atan2(sqrt(1-e*e)*sinEk,cosEk-e)+eph->omg;
sin2p=sin(2.0*phik);
cos2p=cos(2.0*phik);
uk=phik+eph->cus*sin2p+eph->cuc*cos2p;
rk=eph->A*(1-e*cosEk)+eph->crs*sin2p+eph->crc*cos2p;
ik=eph->i0+eph->cis*sin2p+eph->cic*cos2p+eph->idot*tk;
xk=rk*cos(uk);
yk=rk*sin(uk);
phik=eph->OMG0+(eph->OMgd-OMGE)*tk-OMGE*eph->toes;
sin2p=sin(phik);
cos2p=cos(phik);
cosEk=cos(ik);
rs[0]=xk*cos2p-yk*cosEk*sin2p;
rs[1]=xk*sin2p+yk*cosEk*cos2p;
rs[2]=yk*sin(ik);
tk=timediff(time,eph->toc);
(*dts)=(eph->f0)+(eph->f1)*tk+(eph->f2)*tk*tk;
(*dts)+=F*e*sqrt(eph->A)*sinEk;//hieu ung tuong doi tinh
*var=var_uraeph(eph->sva);
}
eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav, char **msg)
{
double t,tmin,tmax;
int i,j=-1;
tmax=MAXDTOE+1.0;
tmin=tmax+1.0;
for (i=sat-1;i<2*MAX_SAT;i+=MAX_SAT)
{
if (nav->eph[i].sat!=sat) continue;
//*msg += sprintf(*msg,"seleph1");
if (iode>=0&&nav->eph[i].iode!=iode)continue;
//*msg += sprintf(*msg,"seleph2");
if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue;
//*msg += sprintf(*msg,"seleph3");
if (iode>=0) return nav->eph+i;
//*msg += sprintf(*msg,"seleph4");
if (t<=tmin)/* choose eph that has toe closest to time */
{
j=i;
tmin=t;
}
}
if (iode>=0||j<0)
{
//*msg += sprintf(*msg,"seleph5");
return NULL;
}
return nav->eph+j;
}
/* satellite clock with broadcast ephemeris ----------------------------------*/
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts,char **msg)
{
eph_t *eph;
if (!(eph=seleph(teph,sat,-1,nav,msg))) return 0;
//*msg += sprintf(*msg,"ephclk");
*dts=eph2clk(time,eph);
return 1;
}
/* satellite position and clock by broadcast ephemeris -----------------------*/
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,char **msg)
{
eph_t *eph;
double rst[3],dtst[1],tt=1E-3;
int i;
*svh=-1;
if (!(eph=seleph(teph,sat,iode,nav,msg))) return 0;
//start=TIM2->CNT;
eph2pos(time,eph,rs,dts,var);
//SendIntStr(TIM2->CNT-start);
time=timeadd(time,tt);
eph2pos(time,eph,rst,dtst,var);
*svh=eph->svh;
/* 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 ------------------------------------------------
* 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)
*-----------------------------------------------------------------------------*/
extern int satpos(gtime_t time, gtime_t teph, int sat,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh,char **msg)
{
*svh=0;
return ephpos(time,teph,sat,nav,-1,rs,dts,var,svh,msg);
// return 1;
}
/* -- void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
double *rs, double *dts, double *var, int *svh) --------------------------------------
*
* Description :
* Parameters : *rs O satellite positions and velocities (ecef), size n
* *dts O sat clocks
* *var O sat position and clock error variances (m^2)
* *svh O sat health flag
* Return :
*/
void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
double *rs, double *dts, double *var, int *svh,char **msg)
{
gtime_t time[MAX_OBS]={{0}};
double dt;
int i,j;
for (i=0;(i<n)&&(i<MAX_OBS);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.0;
if (obs[i].P==0.0)
{
continue;
}
/* transmission time by satellite clock */
time[i]=timeadd(obs[i].time,-obs[i].P/CLIGHT);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt,msg)) {
continue;
}
//*msg += sprintf(*msg,"clk");
time[i]=timeadd(time[i],-dt);
//start = TIM2->CNT;
/* satellite position and clock at transmission time */
if (!satpos(time[i],teph,obs[i].sat,nav,rs+i*6,dts+i*2,var+i,
svh+i,msg)) {
continue;
}
//if (i==0)
// SendIntStr(TIM2->CNT-start);
//*msg += sprintf(*msg,"satpos");
//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,msg))
continue;
dts[1+i*2]=0.0;
*var=SQR(STD_BRDCCLK);
}
}
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手