/* 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[MAXOBS]={{0}};
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 psuedorange */
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/CLIGHT);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
trace(2,"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(2,"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=SQR(STD_BRDCCLK);
}
}
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]);
}
}
/* -- 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);
}
}
}
