/* decode ionosphere correction field (ref [1] 5.7.2.2.1.3) ------------------*/
static int decode_lexion(const unsigned char *buff, int i, gtime_t tof,
nav_t *nav)
{
lexion_t ion={{0}};
int tow,week;
trace(3,"decode_lexion: tof=%s\n",time_str(tof,0));
tow=getbitu(buff,i,20); i+=20;
if (tow==0xFFFFF) { /* correction not available */
return i+192;
}
week=getbitu(buff,i,13); i+=13;
ion.t0=gpst2time(week,tow);
ion.tspan =getbitu(buff,i, 8)*60.0; i+= 8; /* time span (s) */
ion.pos0[0] =getbits(buff,i,19)*1E-5; i+=19; /* latitude (rad) */
ion.pos0[1] =getbits(buff,i,20)*1E-5; i+=20; /* longitude (rad) */
ion.coef[0][0]=getbits(buff,i,22)*1E-3; i+=22;
ion.coef[1][0]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[2][0]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[0][1]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[1][1]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[2][1]=getbits(buff,i,22)*1E-1; i+=22;
nav->lexion=ion;
trace(4,"t0=%s tspan=%.0f pos0=%.1f %.1f coef=%.3f %.3f %.3f %.3f %.3f %.3f\n",
time_str(ion.t0,0),ion.tspan,ion.pos0[0]*R2D,ion.pos0[1]*R2D,
ion.coef[0][0],ion.coef[1][0],ion.coef[2][0],ion.coef[0][1],
ion.coef[1][1],ion.coef[2][1]);
return i;
}
/* 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;
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_CMP: tmax=MAXDTOE_CMP+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(2,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0),
sat,iode);
return NULL;
}
return nav->eph+j;
}
/* long term correction ------------------------------------------------------*/
static int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *drs, double *ddts)
{
const sbssatp_t *p;
double t;
int i;
trace(3,"sbslongcorr: sat=%2d\n",sat);
for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
if (p->sat!=sat||p->lcorr.t0.time==0) continue;
t=timediff(time,p->lcorr.t0);
if (fabs(t)>MAXSBSAGEL) {
trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time,0),sat,t);
return 0;
}
for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t;
*ddts=p->lcorr.daf0+p->lcorr.daf1*t;
trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
sat,drs[0],drs[1],drs[2],*ddts*CLIGHT);
return 1;
}
/* if sbas satellite without correction, no correction applied */
if (satsys(sat,NULL)==SYS_SBS) return 1;
trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
/* 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;
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;
}
/* search stec data ----------------------------------------------------------*/
extern int stec_data(stec_t *stec, gtime_t time, int sat, double *iono,
double *rate, double *rms, int *slip)
{
double tt;
int i,j,k;
trace(4,"search_data: %s sat=%2d\n",time_str(time,0),sat);
if (stec->n<=0) return 0;
/* use index cache for satellite */
k=stec->index[sat-1];
/* binary search by satellite */
if (stec->data[k].sat!=sat) {
for (i=0,j=stec->n-1;i<j-1;) {
k=(i+j)/2;
if (sat==stec->data[k].sat) break;
if (sat<stec->data[k].sat) j=k; else i=k;
}
if (stec->data[k].sat!=sat) {
trace(2,"search_data: no iono %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
}
/* serial search by time */
if (timediff(time,stec->data[k].time)>=0.0) {
for (;k<stec->n&&stec->data[k].sat==sat;k++) {
if (timediff(time,stec->data[k].time)<0.0) break;
}
k--;
}
else {
for (;k>=0&&stec->data[k].sat==sat;k--) {
if (timediff(time,stec->data[k].time)>=0.0) break;
}
if (k<0||stec->data[k].sat!=sat) {
trace(2,"search_data: no iono %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
}
/* save index cache for satellite */
stec->index[sat-1]=k;
tt=timediff(time,stec->data[k].time);
if (fabs(tt)>MAX_AGE) {
trace(2,"search_data: age error %s sat=%2d tt=%.0f\n",time_str(time,0),sat,tt);
return 0;
}
*iono=stec->data[k].iono+stec->data[k].rate*tt;
*rate=stec->data[k].rate;
*rms=stec->data[k].rms;
*slip=stec->data[k].slip;
trace(4,"search_data: pos=%.2f %.2f iono=%6.3f rms=%6.3f slip=%d tt=%3.0f\n",
stec->pos[0],stec->pos[1],*iono,*rms,*slip,tt);
return 1;
}
int
coprintf (const char *format, ...)
{
va_list
argptr; /* Argument list pointer */
int
fmtsize = 0; /* Size of formatted line */
char
*formatted = NULL, /* Formatted line */
*prefixed = NULL; /* Prefixed formatted line */
if (console_active)
{
formatted = mem_alloc (LINE_MAX + 1);
if (!formatted)
return (0);
va_start (argptr, format); /* Start variable args processing */
vsnprintf (formatted, LINE_MAX, format, argptr);
va_end (argptr); /* End variable args processing */
switch (console_mode)
{
case CONSOLE_DATETIME:
xstrcpy_debug ();
prefixed = xstrcpy (NULL, date_str (), " ", time_str (), ": ",
formatted, NULL);
break;
case CONSOLE_TIME:
xstrcpy_debug ();
prefixed = xstrcpy (NULL, time_str (), ": ", formatted, NULL);
break;
}
if (console_file)
{
file_write (console_file, prefixed? prefixed: formatted);
fflush (console_file);
}
if (console_fct)
(console_fct) (prefixed? prefixed: formatted);
if (console_echo)
{
fprintf (stdout, "%s", prefixed? prefixed: formatted);
fprintf (stdout, "\n");
fflush (stdout);
}
if (prefixed)
{
fmtsize = strlen (prefixed);
mem_free (prefixed);
}
else
fmtsize = strlen (formatted);
mem_free (formatted);
}
return (fmtsize);
}
/* decode ephemeris and sv clock field (ref [1] 5.7.2.2.1.2) -----------------*/
static int decode_lexeph(const unsigned char *buff, int i, gtime_t toe,
nav_t *nav)
{
lexeph_t eph={{0}};
gtime_t tof;
unsigned char health;
int j,prn,sat;
trace(3,"decode_lexeph: toe=%s\n",time_str(toe,0));
prn =getbitu(buff,i, 8); i+= 8;
eph.ura =getbitu(buff,i, 4); i+= 4;
eph.pos [0]=getbits_33(buff,i)*P2_6; i+=33;
eph.pos [1]=getbits_33(buff,i)*P2_6; i+=33;
eph.pos [2]=getbits_33(buff,i)*P2_6; i+=33;
eph.vel [0]=getbits(buff,i,28)*P2_15; i+=28;
eph.vel [1]=getbits(buff,i,28)*P2_15; i+=28;
eph.vel [2]=getbits(buff,i,28)*P2_15; i+=28;
eph.acc [0]=getbits(buff,i,24)*P2_24; i+=24;
eph.acc [1]=getbits(buff,i,24)*P2_24; i+=24;
eph.acc [2]=getbits(buff,i,24)*P2_24; i+=24;
eph.jerk[0]=getbits(buff,i,20)*P2_32; i+=20;
eph.jerk[1]=getbits(buff,i,20)*P2_32; i+=20;
eph.jerk[2]=getbits(buff,i,20)*P2_32; i+=20;
eph.af0 =getbits(buff,i,26)*P2_35; i+=26;
eph.af1 =getbits(buff,i,20)*P2_48; i+=20;
eph.tgd =getbits(buff,i,13)*P2_35; i+=13;
for (j=0;j<7;j++) {
eph.isc[j]=getbits(buff,i,13)*P2_35; i+=13;
}
if (prn==255) return i; /* no satellite */
if ( 1<=prn&&prn<= 32) sat=satno(SYS_GPS,prn);
else if (193<=prn&&prn<=195) sat=satno(SYS_QZS,prn);
else {
trace(2,"lex ephemeris prn error prn=%d\n",prn);
return i;
}
eph.toe=toe;
eph.sat=sat;
tof =nav->lexeph[sat-1].tof;
health=nav->lexeph[sat-1].health;
nav->lexeph[sat-1]=eph;
nav->lexeph[sat-1].tof =tof;
nav->lexeph[sat-1].health=health;
trace(4,"sat=%2d toe=%s pos=%.3f %.3f %.3f vel=%.5f %.5f %.5f\n",
sat,time_str(toe,0),eph.pos[0],eph.pos[1],eph.pos[2],
eph.vel[0],eph.vel[1],eph.vel[2]);
trace(4,"clk=%11.3f %8.5f tgd=%7.3f\n",eph.af0*1E9,eph.af1*1E9,
eph.tgd*1E9);
trace(4,"isc=%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f\n",
eph.isc[0]*1E9,eph.isc[1]*1E9,eph.isc[2]*1E9,eph.isc[3]*1E9,
eph.isc[4]*1E9,eph.isc[5]*1E9,eph.isc[6]*1E9);
return i;
}
int main(int argc, char *argv[])
{
printf("%s: START\n", time_str());
for (;;) {
struct timeval timeout = {};
timeout.tv_sec = 5;
select(0, NULL, NULL, NULL, &timeout);
printf("%s: select\n", time_str());
}
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[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]);
}
}
/* ionosphere model by tec grid data -------------------------------------------
* compute ionospheric delay by tec grid data
* args : gtime_t time I time (gpst)
* nav_t *nav I navigation data
* double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* int opt I model option
* bit0: 0:earth-fixed,1:sun-fixed
* bit1: 0:single-layer,1:modified single-layer
* double *delay O ionospheric delay (L1) (m)
* double *var O ionospheric dealy (L1) variance (m^2)
* return : status (1:ok,0:error)
* notes : before calling the function, read tec grid data by calling readtec()
* return ok with delay=0 and var=VAR_NOTEC if el<MIN_EL or h<MIN_HGT
*-----------------------------------------------------------------------------*/
extern int iontec(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int opt, double *delay, double *var)
{
double dels[2],vars[2],a,tt;
int i,stat[2];
trace(3,"iontec : time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0),
pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
if (azel[1]<MIN_EL||pos[2]<MIN_HGT) {
*delay=0.0;
*var=VAR_NOTEC;
return 1;
}
for (i=0;i<nav->nt;i++) {
if (timediff(nav->tec[i].time,time)>0.0) break;
}
if (i==0||i>=nav->nt) {
trace(2,"%s: tec grid out of period\n",time_str(time,0));
return 0;
}
if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) {
trace(2,"tec grid time interval error\n");
return 0;
}
/* ionospheric delay by tec grid data */
stat[0]=iondelay(time,nav->tec+i-1,pos,azel,opt,dels ,vars );
stat[1]=iondelay(time,nav->tec+i ,pos,azel,opt,dels+1,vars+1);
if (!stat[0]&&!stat[1]) {
trace(2,"%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n",
time_str(time,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
return 0;
}
if (stat[0]&&stat[1]) { /* linear interpolation by time */
a=timediff(time,nav->tec[i-1].time)/tt;
*delay=dels[0]*(1.0-a)+dels[1]*a;
*var =vars[0]*(1.0-a)+vars[1]*a;
}
else if (stat[0]) { /* nearest-neighbour extrapolation by time */
*delay=dels[0];
*var =vars[0];
}
else {
*delay=dels[1];
*var =vars[1];
}
trace(3,"iontec : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var));
return 1;
}
/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/
static void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
{
double rsun[3],esun[3],r,ang,erpv[5]={0},cosa;
int i,j;
const char *type;
trace(3,"testeclipse:\n");
/* unit vector of sun direction (ecef) */
sunmoonpos(gpst2utc(obs[0].time),erpv,rsun,NULL,NULL);
normv3(rsun,esun);
for (i=0;i<n;i++) {
type=nav->pcvs[obs[i].sat-1].type;
if ((r=norm(rs+i*6,3))<=0.0) continue;
#if 1
/* only block IIA */
if (*type&&!strstr(type,"BLOCK IIA")) continue;
#endif
/* sun-earth-satellite angle */
cosa=dot(rs+i*6,esun,3)/r;
cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa);
ang=acos(cosa);
/* test eclipse */
if (ang<PI/2.0||r*sin(ang)>RE_WGS84) continue;
trace(2,"eclipsing sat excluded %s sat=%2d\n",time_str(obs[0].time,0),
obs[i].sat);
for (j=0;j<3;j++) rs[j+i*6]=0.0;
}
}
/* decode binex mesaage 0x7f: gnss data prototyping --------------------------*/
static int decode_bnx_7f(raw_t *raw, unsigned char *buff, int len)
{
const static double gpst0[]={1980,1,6,0,0,0};
char *msg;
unsigned char *p=buff;
unsigned int srec,min,msec;
srec=U1(p); p+=1; /* subrecord id */
min =U4(p); p+=4;
msec=U2(p); p+=2;
raw->time=timeadd(epoch2time(gpst0),min*60.0+msec*0.001);
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
sprintf(msg," subrec=%02X time%s",srec,time_str(raw->time,3));
}
switch (srec) {
case 0x00: return decode_bnx_7f_00(raw,buff+7,len-7);
case 0x01: return decode_bnx_7f_01(raw,buff+7,len-7);
case 0x02: return decode_bnx_7f_02(raw,buff+7,len-7);
case 0x03: return decode_bnx_7f_03(raw,buff+7,len-7);
case 0x04: return decode_bnx_7f_04(raw,buff+7,len-7);
case 0x05: return decode_bnx_7f_05(raw,buff+7,len-7);
}
return 0;
}
void
writeCCSReportJson(FILE *prof_file,
CostCentreStack const *stack,
ProfilerTotals totals)
{
fprintf(prof_file, "{\n\"program\": \"%s\",\n", prog_name);
fprintf(prof_file, "\"arguments\": [");
for (int count = 0; prog_argv[count]; count++)
fprintf(prof_file, "%s\"%s\"",
count == 0 ? "" : ", ", prog_argv[count]);
fprintf(prof_file, "],\n\"rts_arguments\": [");
for (int count = 0; rts_argv[count]; count++)
fprintf(prof_file, "%s\"%s\"",
count == 0 ? "" : ", ", rts_argv[count]);
fprintf(prof_file, "],\n");
fprintf(prof_file, "\"end_time\": \"%s\",\n", time_str());
fprintf(prof_file, "\"initial_capabilities\": %d,\n",
RtsFlags.ParFlags.nCapabilities);
fprintf(prof_file, "\"total_time\": %11.2f,\n",
((double) totals.total_prof_ticks *
(double) RtsFlags.MiscFlags.tickInterval) / (TIME_RESOLUTION * n_capabilities));
fprintf(prof_file, "\"total_ticks\": %lu,\n",
(unsigned long) totals.total_prof_ticks);
fprintf(prof_file, "\"tick_interval\": %d,\n",
(int) TimeToUS(RtsFlags.MiscFlags.tickInterval));
fprintf(prof_file, "\"total_alloc\":%" FMT_Word64 ",\n",
totals.total_alloc * sizeof(W_));
fprintf(prof_file, "\"cost_centres\": ");
logCostCentres(prof_file);
fprintf(prof_file, ",\n\"profile\": ");
logCostCentreStack(prof_file, stack);
fprintf(prof_file, "}\n");
}
/* fast correction -----------------------------------------------------------*/
static int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *prc, double *var)
{
const sbssatp_t *p;
double t;
trace(3,"sbsfastcorr: sat=%2d\n",sat);
for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
if (p->sat!=sat) continue;
if (p->fcorr.t0.time==0) break;
t=timediff(time,p->fcorr.t0)+sbssat->tlat;
/* expire age of correction or UDRE==14 (not monitored) */
if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue;
*prc=p->fcorr.prc;
#ifdef RRCENA
if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) {
*prc+=p->fcorr.rrc*t;
}
#endif
*var=varfcorr(p->fcorr.udre)+degfcorr(p->fcorr.ai)*t*t/2.0;
trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat,
*prc,sqrt(*var),t);
return 1;
}
trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
int Manager::feed(const uchar *data, pcap_pkthdr &packet){
id++;
printf("[*]Manager::feed 分析第%d个包... \n",id);
printf("Packet length: %d\n", packet.len);
string time_str(ctime((const time_t *)&packet.ts.tv_sec));
time_str.erase(time_str.length()-1);
printf("Recieved time: %s\n",time_str.c_str() );
Ether *ether = (Ether *)data;
data += sizeof(Ether);
if (ether->type != 8){
printf("[!] not an ip package!\n");
return 1;
}
IPhdr *ip = (IPhdr*)data;
data += ip->getHeadLength();
if (ip->hdr.ip_p != 6){
printf( "[!]IP protocol id %d unknown!", ip->hdr.ip_p);
return 2;
}
TCPhdr *tcp = (TCPhdr*)data;
TCPkey key( ip->hdr.ip_src, ip->hdr.ip_dst, tcp->source(), tcp->dest());
if(buffer.count(key)==0){
SeqTCPStream *tc=new SeqTCPStream(ip->hdr.ip_src, ip->hdr.ip_dst, tcp->source(), tcp->dest(), conf);
buffer.insert({key,tc});
}
buffer[key]->push(data, ip->getTcpLength());
printf("[*]Manager::feed 分析第%d个包结束. \n\n",id);
return 0;
}
/* satellite clock with broadcast ephemeris ----------------------------------*/
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);
sys=satsys(sat,NULL);
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
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;
}
/* 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;
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 (E=M,sinE=Ek=0.0;fabs(E-Ek)>1E-12;) {
Ek=E; sinE=sin(Ek); E=M+alm->e*sinE;
}
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;
}
void func_rnew_mail(USER_DATA *usr) {
MAIL_DATA *pMail;
char buf[STRING];
BUFFER *buffer;
int vnum = 1;
buffer = new_buf();
for (pMail = usr->pMailFirst; pMail; pMail = pMail->next) {
if (pMail->stamp_time <= pMail->read_time)
break;
}
if (pMail) {
time_t *last_read = &pMail->read_time;
*last_read = UMAX(*last_read, pMail->stamp_time);
sprintf(buf, "Reading new message %d.\n\rDate: %s\n\rFrom: %s\n\r"
"Subject: %s\n\r\n\r", vnum, time_str(pMail->stamp_time),
pMail->from, pMail->subject);
add_buf(buffer, buf);
add_buf(buffer, pMail->message);
add_buf(buffer, "#x\n\r");
page_to_user(buf_string(buffer), usr);
free_buf(buffer);
save_mail(usr);
return;
}
send_to_user("No new messages.\n\r", usr);
return;
}
void func_list_mail(USER_DATA *usr) {
MAIL_DATA *pMail;
char buf[STRING];
BUFFER *buffer;
int vnum = 0;
buffer = new_buf();
add_buf(buffer, " Num Date From Subject\n\r");
for (pMail = usr->pMailFirst; pMail; pMail = pMail->next) {
if (pMail) {
sprintf(buf, "%s %3d %s %-15s %s\n\r", pMail->marked ? "M"
: (pMail->stamp_time > pMail->read_time) ? "N" : " ", vnum
+ 1, time_str(pMail->stamp_time), pMail->from,
pMail->subject);
add_buf(buffer, buf);
vnum++;
}
}
if (vnum > 0) {
page_to_user(buf_string(buffer), usr);
free_buf(buffer);
return;
}
send_to_user("You don't have any mail.\n\r", usr);
free_buf(buffer);
return;
}
/* input stream file ---------------------------------------------------------*/
static int input_strfile(strfile_t *str)
{
int type=0;
trace(4,"input_strfile:\n");
if (str->format==STRFMT_RTCM2) {
if ((type=input_rtcm2f(&str->rtcm,str->fp))>=1) {
str->time=str->rtcm.time;
str->sat=str->rtcm.ephsat;
}
}
else if (str->format==STRFMT_RTCM3) {
if ((type=input_rtcm3f(&str->rtcm,str->fp))>=1) {
str->time=str->rtcm.time;
str->sat=str->rtcm.ephsat;
}
}
else if (str->format<=MAXRCVFMT) {
if ((type=input_rawf(&str->raw,str->format,str->fp))>=1) {
str->time=str->raw.time;
str->sat=str->raw.ephsat;
}
}
else if (str->format==STRFMT_RINEX) {
if ((type=input_rnxctr(&str->rnx,str->fp))>=1) {
str->time=str->rnx.time;
str->sat=str->rnx.ephsat;
}
}
trace(4,"input_strfile: time=%s type=%d sat=%2d\n",time_str(str->time,3),
type,str->sat);
return type;
}
/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args : obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* prcopt_t *opt I processing options
* sol_t *sol IO solution
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
* ssat_t *ssat IO satellite status (NULL: no output)
* char *msg O error message for error exit
* return : status(1:ok,0:error)
* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
* receiver bias are negligible (only involving glonass-gps time offset
* and receiver bias)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
{
prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
sol->stat=SOLQ_NONE;
if (n<=0) {strcpy(msg,"no observation data"); return 0;}
trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
sol->time=obs[0].time; msg[0]='\0';
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;
#endif
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}
/* satellite positons, velocities and clocks */
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
/* estimate receiver position with pseudorange */
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
/* raim fde */
if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
}
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
return stat;
}
/* ionospheric correction ------------------------------------------------------
* compute ionospheric correction
* args : gtime_t time I time
* nav_t *nav I navigation data
* int sat I satellite number
* double *pos I receiver position {lat,lon,h} (rad|m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* int ionoopt I ionospheric correction option (IONOOPT_???)
* double *ion O ionospheric delay (L1) (m)
* double *var O ionospheric delay (L1) variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var)
{
trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n",
time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,
azel[1]*R2D);
/* broadcast model */
if (ionoopt==IONOOPT_BRDC) {
*ion=ionmodel(time,nav->ion_gps,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
return 1;
}
/* sbas ionosphere model */
if (ionoopt==IONOOPT_SBAS) {
return sbsioncorr(time,nav,pos,azel,ion,var);
}
/* ionex tec model */
if (ionoopt==IONOOPT_TEC) {
return iontec(time,nav,pos,azel,1,ion,var);
}
/* qzss broadcast model */
if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) {
*ion=ionmodel(time,nav->ion_qzs,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
return 1;
}
/* lex ionosphere model */
if (ionoopt==IONOOPT_LEX) {
return lexioncorr(time,nav,pos,azel,ion,var);
}
*ion=0.0;
*var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0;
return 1;
}
/* satellite clock by precise clock ------------------------------------------*/
static int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
double *varc)
{
double t[2],c[2],std;
int i,j,k,index;
trace(4,"pephclk : time=%s sat=%2d\n",time_str(time,3),sat);
if (nav->nc<2||
timediff(time,nav->pclk[0].time)<-MAXDTE||
timediff(time,nav->pclk[nav->nc-1].time)>MAXDTE) {
trace(3,"no prec clock %s sat=%2d\n",time_str(time,0),sat);
return 1;
}
/* binary search */
for (i=0,j=nav->nc-1;i<j;) {
k=(i+j)/2;
if (timediff(nav->pclk[k].time,time)<0.0) i=k+1; else j=k;
}
index=i<=0?0:i-1;
/* linear interpolation for clock */
t[0]=timediff(time,nav->pclk[index ].time);
t[1]=timediff(time,nav->pclk[index+1].time);
c[0]=nav->pclk[index ].clk[sat-1][0];
c[1]=nav->pclk[index+1].clk[sat-1][0];
if (t[0]<=0.0) {
if ((dts[0]=c[0])==0.0) return 0;
std=nav->pclk[index].std[sat-1][3]*CLIGHT-EXTERR_CLK*t[0];
}
else if (t[1]>=0.0) {
if ((dts[0]=c[1])==0.0) return 0;
std=nav->pclk[index+1].std[sat-1][3]*CLIGHT+EXTERR_CLK*t[1];
}
else if (c[0]!=0.0&&c[1]!=0.0) {
dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]);
i=t[0]<-t[1]?0:1;
std=nav->pclk[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]);
}
else {
trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
if (varc) *varc=SQR(std);
return 1;
}
/* psendorange with code bias correction -------------------------------------*/
static double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
int iter, const prcopt_t *opt, double *var)
{
const double *lam=nav->lam[obs->sat-1];
double PC,P1,P2,P1_P2,P1_C1,P2_C2,gamma;
int i=0,j=1,sys;
*var=0.0;
if (!(sys=satsys(obs->sat,NULL))) return 0.0;
/* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
if (NFREQ>=3&&(sys&(SYS_GAL|SYS_SBS))) j=2;
if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) return 0.0;
/* test snr mask */
if (iter>0) {
if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) {
trace(4,"snr mask: %s sat=%2d el=%.1f snr=%.1f\n",
time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25);
return 0.0;
}
if (opt->ionoopt==IONOOPT_IFLC) {
if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) return 0.0;
}
}
gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */
P1=obs->P[i];
P2=obs->P[j];
P1_P2=nav->cbias[obs->sat-1][0];
P1_C1=nav->cbias[obs->sat-1][1];
P2_C2=nav->cbias[obs->sat-1][2];
/* if no P1-P2 DCB, use TGD instead */
if (P1_P2==0.0&&(sys&(SYS_GPS|SYS_GAL|SYS_QZS))) {
P1_P2=(1.0-gamma)*gettgd(obs->sat,nav);
}
if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */
if (P1==0.0||P2==0.0) return 0.0;
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */
/* iono-free combination */
PC=(gamma*P1-P2)/(gamma-1.0);
}
else { /* single-frequency */
if (P1==0.0) return 0.0;
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
PC=P1-P1_P2/(1.0-gamma);
}
if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */
*var=SQR(ERR_CBIAS);
return PC;
}
inline std::string GetCurrentTime(){
time_t raw_time;
time(&raw_time);
auto* time_info = localtime(&raw_time);
std::string time_str(asctime(time_info));
TrimString(time_str);
return time_str;
}
/* decode binex mesaage 0x00: site/monument/marker/ref point/setup metadata --*/
static int decode_bnx_00(raw_t *raw, unsigned char *buff, int len)
{
const static double gpst0[]={1980,1,6,0,0,0};
char *msg;
unsigned char *p=buff;
unsigned int min,qsec,src,fid;
int n=6;
min =U4(p); p+=4;
qsec=U1(p); p+=1;
src =U1(p); p+=1;
n+=getbnxi(p,&fid);
raw->time=timeadd(epoch2time(gpst0),min*60.0+qsec*0.25);
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
sprintf(msg," fid=%02X time=%s src=%d",fid,time_str(raw->time,0),src);
}
switch (fid) {
case 0x00: return decode_bnx_00_00(raw,buff+n,len-n);
case 0x01: return decode_bnx_00_01(raw,buff+n,len-n);
case 0x02: return decode_bnx_00_02(raw,buff+n,len-n);
case 0x03: return decode_bnx_00_03(raw,buff+n,len-n);
case 0x04: return decode_bnx_00_04(raw,buff+n,len-n);
case 0x05: return decode_bnx_00_05(raw,buff+n,len-n);
case 0x06: return decode_bnx_00_06(raw,buff+n,len-n);
case 0x07: return decode_bnx_00_07(raw,buff+n,len-n);
case 0x08: return decode_bnx_00_08(raw,buff+n,len-n);
case 0x09: return decode_bnx_00_09(raw,buff+n,len-n);
case 0x0A: return decode_bnx_00_0a(raw,buff+n,len-n);
case 0x0B: return decode_bnx_00_0b(raw,buff+n,len-n);
case 0x0C: return decode_bnx_00_0c(raw,buff+n,len-n);
case 0x0D: return decode_bnx_00_0d(raw,buff+n,len-n);
case 0x0E: return decode_bnx_00_0e(raw,buff+n,len-n);
case 0x0F: return decode_bnx_00_0f(raw,buff+n,len-n);
case 0x10: return decode_bnx_00_10(raw,buff+n,len-n);
case 0x11: return decode_bnx_00_11(raw,buff+n,len-n);
case 0x12: return decode_bnx_00_12(raw,buff+n,len-n);
case 0x13: return decode_bnx_00_13(raw,buff+n,len-n);
case 0x14: return decode_bnx_00_14(raw,buff+n,len-n);
case 0x15: return decode_bnx_00_15(raw,buff+n,len-n);
case 0x16: return decode_bnx_00_16(raw,buff+n,len-n);
case 0x17: return decode_bnx_00_17(raw,buff+n,len-n);
case 0x18: return decode_bnx_00_18(raw,buff+n,len-n);
case 0x19: return decode_bnx_00_19(raw,buff+n,len-n);
case 0x1A: return decode_bnx_00_1a(raw,buff+n,len-n);
case 0x1B: return decode_bnx_00_1b(raw,buff+n,len-n);
case 0x1C: return decode_bnx_00_1c(raw,buff+n,len-n);
case 0x1D: return decode_bnx_00_1d(raw,buff+n,len-n);
case 0x1E: return decode_bnx_00_1e(raw,buff+n,len-n);
case 0x1F: return decode_bnx_00_1f(raw,buff+n,len-n);
case 0x20: return decode_bnx_00_20(raw,buff+n,len-n);
case 0x21: return decode_bnx_00_21(raw,buff+n,len-n);
case 0x22: return decode_bnx_00_22(raw,buff+n,len-n);
case 0x7F: return decode_bnx_00_7f(raw,buff+n,len-n);
}
return 0;
}
/* decode nmea gpgga: fix information ----------------------------------------*/
static int decode_nmeagga(char **val, int n, sol_t *sol)
{
gtime_t time;
double tod=0.0,lat=0.0,lon=0.0,hdop=0.0,alt=0.0,msl=0.0,ep[6],tt;
double pos[3]={0};
char ns='N',ew='E',ua=' ',um=' ';
int i,solq=0,nrcv=0;
trace(4,"decode_nmeagga: n=%d\n",n);
for (i=0;i<n;i++) {
switch (i) {
case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */
case 1: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */
case 2: ns =*val[i]; break; /* N=north,S=south */
case 3: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */
case 4: ew =*val[i]; break; /* E=east,W=west */
case 5: solq=atoi(val[i]); break; /* fix quality */
case 6: nrcv=atoi(val[i]); break; /* # of satellite tracked */
case 7: hdop=atof(val[i]); break; /* hdop */
case 8: alt =atof(val[i]); break; /* altitude in msl */
case 9: ua =*val[i]; break; /* unit (M) */
case 10: msl =atof(val[i]); break; /* height of geoid */
case 11: um =*val[i]; break; /* unit (M) */
}
}
if ((ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) {
trace(2,"invalid nmea gpgga format\n");
return 0;
}
if (sol->time.time==0.0) {
trace(2,"no date info for nmea gpgga\n");
return 0;
}
pos[0]=(ns=='N'?1.0:-1.0)*dmm2deg(lat)*D2R;
pos[1]=(ew=='E'?1.0:-1.0)*dmm2deg(lon)*D2R;
pos[2]=alt+msl;
time2epoch(sol->time,ep);
septime(tod,ep+3,ep+4,ep+5);
time=utc2gpst(epoch2time(ep));
tt=timediff(time,sol->time);
if (tt<-43200.0) sol->time=timeadd(time, 86400.0);
else if (tt> 43200.0) sol->time=timeadd(time,-86400.0);
else sol->time=time;
pos2ecef(pos,sol->rr);
sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE;
sol->ns=nrcv;
sol->type=0; /* postion type = xyz */
trace(5,"decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n",
time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns,
hdop,ua,um);
return 1;
}
/* read obs and nav data -----------------------------------------------------*/
static int readobsnav(gtime_t ts, gtime_t te, double ti, char **infile,
const int *index, int n, const prcopt_t *prcopt,
obs_t *obs, nav_t *nav, sta_t *sta)
{
int i,j,ind=0,nobs=0,rcv=1;
trace(3,"readobsnav: ts=%s n=%d\n",time_str(ts,0),n);
obs->data=NULL; obs->n =obs->nmax =0;
nav->eph =NULL; nav->n =nav->nmax =0;
nav->geph=NULL; nav->ng=nav->ngmax=0;
nav->seph=NULL; nav->ns=nav->nsmax=0;
nepoch=0;
for (i=0;i<n;i++) {
if (checkbrk("")) return 0;
if (index[i]!=ind) {
if (obs->n>nobs) rcv++;
ind=index[i]; nobs=obs->n;
}
/* read rinex obs and nav file */
if (readrnxt(infile[i],rcv,ts,te,ti,prcopt->rnxopt[rcv<=1?0:1],obs,nav,
rcv<=2?sta+rcv-1:NULL)<0) {
checkbrk("error : insufficient memory");
trace(1,"insufficient memory\n");
return 0;
}
}
if (obs->n<=0) {
checkbrk("error : no obs data");
trace(1,"no obs data\n");
return 0;
}
if (nav->n<=0&&nav->ng<=0&&nav->ns<=0) {
checkbrk("error : no nav data");
trace(1,"no nav data\n");
return 0;
}
/* sort observation data */
nepoch=sortobs(obs);
/* delete duplicated ephemeris */
uniqnav(nav);
/* set time span for progress display */
if (ts.time==0||te.time==0) {
for (i=0; i<obs->n;i++) if (obs->data[i].rcv==1) break;
for (j=obs->n-1;j>=0;j--) if (obs->data[j].rcv==1) break;
if (i<j) {
if (ts.time==0) ts=obs->data[i].time;
if (te.time==0) te=obs->data[j].time;
settspan(ts,te);
}
}
return 1;
}
/* 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;
}
/* tidal displacement ----------------------------------------------------------
* displacements by earth tides
* args : gtime_t tutc I time in utc
* double *rr I site position (ecef) (m)
* int opt I options (or of the followings)
* 1: solid earth tide
* 2: ocean tide loading
* 4: pole tide
* 8: elimate permanent deformation
* double *erp I earth rotation parameters (NULL: not used)
* double *odisp I ocean loading parameters (NULL: not used)
* odisp[0+i*6]: consituent i amplitude radial(m)
* odisp[1+i*6]: consituent i amplitude west (m)
* odisp[2+i*6]: consituent i amplitude south (m)
* odisp[3+i*6]: consituent i phase radial (deg)
* odisp[4+i*6]: consituent i phase west (deg)
* odisp[5+i*6]: consituent i phase south (deg)
* (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,
* 8:Mf,9:Mm,10:Ssa)
* double *dr O displacement by earth tides (ecef) (m)
* return : none
* notes : see ref [1], [2] chap 7
* see ref [4] 5.2.1, 5.2.2, 5.2.3
* ver.2.4.0 does not use ocean loading and pole tide corrections
*-----------------------------------------------------------------------------*/
extern void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr)
{
gtime_t tut;
double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0};
int i;
#ifdef IERS_MODEL
double ep[6],fhr;
int year,mon,day;
#endif
trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0));
if (erp) geterp(erp,tutc,erpv);
tut=timeadd(tutc,erpv[2]);
dr[0]=dr[1]=dr[2]=0.0;
if (norm(rr,3)<=0.0) return;
pos[0]=asin(rr[2]/norm(rr,3));
pos[1]=atan2(rr[1],rr[0]);
xyz2enu(pos,E);
if (opt&1) { /* solid earth tides */
/* sun and moon position in ecef */
sunmoonpos(tutc,erpv,rs,rm,&gmst);
#ifdef IERS_MODEL
time2epoch(tutc,ep);
year=(int)ep[0];
mon =(int)ep[1];
day =(int)ep[2];
fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0;
/* call DEHANTTIDEINEL */
dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt);
#else
tide_solid(rs,rm,pos,E,gmst,opt,drt);
#endif
for (i=0;i<3;i++) dr[i]+=drt[i];
}
if ((opt&2)&&odisp) { /* ocean tide loading */
tide_oload(tut,odisp,denu);
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
for (i=0;i<3;i++) dr[i]+=drt[i];
}
if ((opt&4)&&erp) { /* pole tide */
tide_pole(pos,erpv,denu);
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
for (i=0;i<3;i++) dr[i]+=drt[i];
}
trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
}
