/* average of single position ------------------------------------------------*/
static int avepos(double *ra, int rcv, const obs_t *obs, const nav_t *nav,
const prcopt_t *opt)
{
obsd_t data[MAXOBS];
gtime_t ts={0};
sol_t sol={{0}};
int i,j,n=0,m,iobs;
char msg[128];
trace(3,"avepos: rcv=%d obs.n=%d\n",rcv,obs->n);
for (i=0;i<3;i++) ra[i]=0.0;
for (iobs=0;(m=nextobsf(obs,&iobs,rcv))>0;iobs+=m) {
for (i=j=0;i<m&&i<MAXOBS;i++) {
data[j]=obs->data[iobs+i];
if ((satsys(data[j].sat,NULL)&opt->navsys)&&
opt->exsats[data[j].sat-1]!=1) j++;
}
if (j<=0||!screent(data[0].time,ts,ts,1.0)) continue; /* only 1 hz */
if (!pntpos(data,j,nav,opt,&sol,NULL,NULL,msg)) continue;
for (i=0;i<3;i++) ra[i]+=sol.rr[i];
n++;
}
if (n<=0) {
trace(1,"no average of base station position\n");
return 0;
}
for (i=0;i<3;i++) ra[i]/=n;
return 1;
}
/* convert obs message -------------------------------------------------------*/
static void convobs(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n,
unsigned char slips[][NFREQ+NEXOBS])
{
gtime_t time;
trace(3,"convobs :\n");
if (!ofp[0]||str->obs->n<=0) return;
time=str->obs->data[0].time;
/* save slips */
saveslips(slips,str->obs->data,str->obs->n);
if (!screent(time,opt->ts,opt->te,opt->tint)) return;
/* restore slips */
restslips(slips,str->obs->data,str->obs->n);
/* output rinex obs */
outrnxobsb(ofp[0],opt,str->obs->data,str->obs->n,0);
if (opt->tstart.time==0) opt->tstart=time;
opt->tend=time;
n[0]++;
}
/* input solution data from stream ---------------------------------------------
* input solution data from stream
* args : unsigned char data I stream data
* gtime_t ts I start time (ts.time==0: from start)
* gtime_t te I end time (te.time==0: to end)
* double tint I time interval (0: all)
* int qflag I quality flag (0: all)
* solbuf_t *solbuf IO solution buffer
* return : status (1:solution received,0:no solution,-1:disconnect received)
*-----------------------------------------------------------------------------*/
extern int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint,
int qflag, const solopt_t *opt, solbuf_t *solbuf)
{
sol_t sol={{0}};
int stat;
trace(4,"inputsol: data=0x%02x\n",data);
sol.time=solbuf->time;
if (data=='$'||(!isprint(data)&&data!='\r'&&data!='\n')) { /* sync header */
solbuf->nb=0;
}
solbuf->buff[solbuf->nb++]=data;
if (data!='\n'&&solbuf->nb<MAXSOLMSG) return 0; /* sync trailer */
solbuf->buff[solbuf->nb]='\0';
solbuf->nb=0;
/* check disconnect message */
if (!strcmp((char *)solbuf->buff,MSG_DISCONN)) {
trace(3,"disconnect received\n");
return -1;
}
/* decode solution */
if ((stat=decode_sol((char *)solbuf->buff,opt,&sol,solbuf->rb))>0) {
solbuf->time=sol.time; /* update current time */
}
if (stat!=1||!screent(sol.time,ts,te,tint)||(qflag&&sol.stat!=qflag)) {
return 0;
}
/* add solution to solution buffer */
return addsol(solbuf,&sol);
}
/* convert sbas message ------------------------------------------------------*/
static void convsbs(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t ts1,te1;
int msg,prn,sat,sys;
trace(3,"convsbs :\n");
ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
msg=sbsupdatecorr(&str->raw.sbsmsg,str->nav);
prn=str->raw.sbsmsg.prn;
if (MINPRNSBS<=prn&&prn<=MAXPRNSBS) sys=SYS_SBS;
else if (MINPRNQZS<=prn&&prn<=MAXPRNQZS) sys=SYS_QZS;
else {
trace(2,"sbas message satellite error: prn=%d\n",prn);
return;
}
if (!(sat=satno(sys,prn))||opt->exsats[sat-1]==1) return;
if (ofp[6]) { /* output sbas log */
if (screent(gpst2time(str->raw.sbsmsg.week,str->raw.sbsmsg.tow),opt->ts,
opt->te,0.0)) {
sbsoutmsg(ofp[6],&str->raw.sbsmsg); n[6]++;
}
}
if (!(opt->navsys&SYS_SBS)||msg!=9||
!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS);
n[1]++;
}
if (ofp[3]&&opt->rnxver<=2.99) {
/* output rinex hnav */
outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS);
n[3]++;
}
}
/* convert lex message -------------------------------------------------------*/
static void convlex(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t ts1,te1;
trace(3,"convlex :\n");
ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
if (ofp[6]&&screent(str->time,opt->ts,opt->te,0.0)) {
lexoutmsg(ofp[6],&str->raw.lexmsg); n[6]++;
}
}
// get position, velocity or accel from solutions ---------------------------
TIMEPOS * __fastcall TPlot::SolToPos(solbuf_t *sol, int index, int qflag, int type)
{
TIMEPOS *pos,*vel,*acc;
gtime_t ts={0};
sol_t *data;
double tint,xyz[3],xyzs[4];
int i;
trace(3,"SolToPos: n=%d\n",sol->n);
pos=new TIMEPOS(index<0?sol->n:3,1);
if (index>=0) {
if (type==1&&index>sol->n-2) index=sol->n-2;
if (type==2&&index>sol->n-3) index=sol->n-3;
}
for (i=index<0?0:index;data=getsol(sol,i);i++) {
tint=TimeEna[2]?TimeInt:0.0;
if (index<0&&!screent(data->time,ts,ts,tint)) continue;
if (qflag&&data->stat!=qflag) continue;
PosToXyz(data->time,data->rr,data->type,xyz);
CovToXyz(data->rr,data->qr,data->type,xyzs);
pos->t [pos->n]=data->time;
pos->x [pos->n]=xyz [0];
pos->y [pos->n]=xyz [1];
pos->z [pos->n]=xyz [2];
pos->xs [pos->n]=xyzs[0]; // var x^2
pos->ys [pos->n]=xyzs[1]; // var y^2
pos->zs [pos->n]=xyzs[2]; // var z^2
pos->xys[pos->n]=xyzs[3]; // cov xy
pos->q [pos->n]=data->stat;
pos->n++;
if (index>=0&&pos->n>=3) break;
}
if (type!=1&&type!=2) return pos; // position
vel=pos->tdiff();
delete pos;
if (type==1) return vel; // velocity
acc=vel->tdiff();
delete vel;
return acc; // acceleration
}
/* read solution status data -------------------------------------------------*/
static int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint,
solstatbuf_t *statbuf)
{
solstat_t stat={{0}};
char buff[MAXSOLMSG+1];
trace(3,"readsolstatdata:\n");
while (fgets(buff,sizeof(buff),fp)) {
/* decode solution status */
if (!decode_solstat(buff,&stat)) continue;
/* add solution to solution buffer */
if (screent(stat.time,ts,te,tint)) {
addsolstat(statbuf,&stat);
}
}
return statbuf->n>0;
}
/* convert pvt message -------------------------------------------------------*/
static void convpvt(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t time,current;
static gtime_t firsttime={0,0};
double ep[6];
trace(3,"convpvt :\n");
if (!ofp[7]) return;
time=str->pvt->time;
if (!screent(time,opt->ts,opt->te,opt->tint)) return;
if(n[7]==0)
{
current=timeget();
if(timediff(time,current)>86400) return;
firsttime=time;
time2epoch(time,ep);
fprintf(ofp[7], "%04.0f %02.0f %02.0f %02.0f %02.0f %02.6f\n",
ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
}
fprintf(ofp[7], "%15.7f %14.3lf %14.3lf %14.3lf %11.3lf %11.3lf %11.3lf",
timediff(time,firsttime), str->pvt->pos[0], str->pvt->pos[1], str->pvt->pos[2],
str->pvt->vel[0], str->pvt->vel[1], str->pvt->vel[2]);
fprintf(ofp[7],"\n");
if (opt->tstart.time==0) opt->tstart=time;
opt->tend=time;
n[7]++;
}
/* convert nav message -------------------------------------------------------*/
static void convnav(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t ts1,te1,ts2,te2;
int sys,prn;
trace(3,"convnav :\n");
ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
ts2=opt->ts; if (ts2.time!=0) ts2=timeadd(ts2,-MAXDTOE_GLO);
te2=opt->te; if (te2.time!=0) te2=timeadd(te2, MAXDTOE_GLO);
sys=satsys(str->sat,&prn)&opt->navsys;
if (sys==SYS_GPS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
}
else if (sys==SYS_GLO) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts2,te2,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxgnavb(ofp[1],opt,str->nav->geph+prn-1);
n[1]++;
}
if (ofp[2]&&opt->rnxver<=2.99) {
/* output rinex gnav */
outrnxgnavb(ofp[2],opt,str->nav->geph+prn-1);
n[2]++;
}
}
else if (sys==SYS_SBS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS);
n[1]++;
}
if (ofp[3]&&opt->rnxver<=2.99) {
/* output rinex hnav */
outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS);
n[3]++;
}
}
else if (sys==SYS_QZS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
if (ofp[4]&&opt->rnxver<=2.99) {
/* output rinex qnav */
outrnxnavb(ofp[4],opt,str->nav->eph+str->sat-1);
n[4]++;
}
}
else if (sys==SYS_GAL) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
if (ofp[5]&&opt->rnxver<=2.99) {
/* output rinex lnav */
outrnxnavb(ofp[5],opt,str->nav->eph+str->sat-1);
n[5]++;
}
}
else if (sys==SYS_CMP) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
}
}
/* read sbas log file --------------------------------------------------------*/
static void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs)
{
sbsmsg_t *sbs_msgs;
int i,week,prn,ch,msg;
unsigned int b;
double tow,ep[6]={0};
char buff[256],*p;
gtime_t time;
FILE *fp;
trace(3,"readmsgs: file=%s sel=%d\n",file,sel);
if (!(fp=fopen(file,"r"))) {
trace(2,"sbas message file open error: %s\n",file);
return;
}
while (fgets(buff,sizeof(buff),fp)) {
if (sscanf(buff,"%d %lf %d",&week,&tow,&prn)==3&&(p=strstr(buff,": "))) {
p+=2; /* rtklib form */
}
else if (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d",
&prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) {
/* ems (EGNOS Message Service) form */
ep[0]+=ep[0]<70.0?2000.0:1900.0;
tow=time2gpst(epoch2time(ep),&week);
p=buff+(msg>=10?25:24);
}
else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */
if (!(p=getfield(buff,6))) continue;
if (sscanf(p,"%d,%lf",&week,&tow)<2) continue;
if (!(p=strchr(++p,';'))) continue;
if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue;
if (!(p=getfield(p,4))) continue;
}
else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */
if (!(p=getfield(buff,2))) continue;
if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue;
if (!(p=getfield(p,6))) continue;
if (week<WEEKOFFSET) week+=WEEKOFFSET;
}
else continue;
if (sel!=0&&sel!=prn) continue;
time=gpst2time(week,tow);
if (!screent(time,ts,te,0.0)) continue;
if (sbs->n>=sbs->nmax) {
sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2;
if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) {
trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax);
free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0;
return;
}
sbs->msgs=sbs_msgs;
}
sbs->msgs[sbs->n].week=week;
sbs->msgs[sbs->n].tow=(int)(tow+0.5);
sbs->msgs[sbs->n].prn=prn;
for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0;
for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) {
if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b;
}
sbs->msgs[sbs->n++].msg[28]&=0xC0;
}
fclose(fp);
}
