/* initialize rtcm control -----------------------------------------------------
* initialize rtcm control struct and reallocate memory for observation and
* ephemeris buffer in rtcm control struct
* args : rtcm_t *raw IO rtcm control struct
* return : status (1:ok,0:memory allocation error)
*-----------------------------------------------------------------------------*/
extern int init_rtcm(rtcm_t *rtcm)
{
gtime_t time0={0};
obsd_t data0={{0}};
eph_t eph0 ={0,-1,-1};
geph_t geph0={0,-1};
ssr_t ssr0={{{0}}};
int i,j;
trace(3,"init_rtcm:\n");
rtcm->staid=rtcm->stah=rtcm->seqno=rtcm->outtype=0;
rtcm->time=rtcm->time_s=time0;
rtcm->sta.name[0]=rtcm->sta.marker[0]='\0';
rtcm->sta.antdes[0]=rtcm->sta.antsno[0]='\0';
rtcm->sta.rectype[0]=rtcm->sta.recver[0]=rtcm->sta.recsno[0]='\0';
rtcm->sta.antsetup=rtcm->sta.itrf=rtcm->sta.deltype=0;
for (i=0;i<3;i++) {
rtcm->sta.pos[i]=rtcm->sta.del[i]=0.0;
}
rtcm->sta.hgt=0.0;
rtcm->dgps=NULL;
for (i=0;i<MAXSAT;i++) {
rtcm->ssr[i]=ssr0;
}
rtcm->msg[0]=rtcm->msgtype[0]=rtcm->opt[0]='\0';
for (i=0;i<6;i++) rtcm->msmtype[i][0]='\0';
rtcm->obsflag=rtcm->ephsat=0;
for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ+NEXOBS;j++) {
rtcm->cp[i][j]=0.0;
rtcm->lock[i][j]=rtcm->loss[i][j]=0;
rtcm->lltime[i][j]=time0;
}
rtcm->nbyte=rtcm->nbit=rtcm->len=0;
rtcm->word=0;
for (i=0;i<100;i++) rtcm->nmsg2[i]=0;
for (i=0;i<300;i++) rtcm->nmsg3[i]=0;
free_rtcm(rtcm);
/* reallocate memory for observation and ephemris buffer */
if (!(rtcm->obs.data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
!(rtcm->nav.eph =(eph_t *)malloc(sizeof(eph_t )*MAXSAT))||
!(rtcm->nav.geph=(geph_t *)malloc(sizeof(geph_t)*MAXPRNGLO))) {
free_rtcm(rtcm);
return 0;
}
rtcm->obs.n=0;
rtcm->nav.n=MAXSAT;
rtcm->nav.ng=MAXPRNGLO;
for (i=0;i<MAXOBS ;i++) rtcm->obs.data[i]=data0;
for (i=0;i<MAXSAT ;i++) rtcm->nav.eph [i]=eph0;
for (i=0;i<MAXPRNGLO;i++) rtcm->nav.geph[i]=geph0;
return 1;
}
/* free stream file ----------------------------------------------------------*/
static void free_strfile(strfile_t *str)
{
trace(3,"free_strfile:\n");
if (str->format==STRFMT_RTCM2||str->format==STRFMT_RTCM3) {
free_rtcm(&str->rtcm);
}
else if (str->format<=MAXRCVFMT) {
free_raw(&str->raw);
}
else if (str->format==STRFMT_RINEX) {
free_rnxctr(&str->rnx);
}
free(str);
}
/* free prec ephemeris and sbas data -----------------------------------------*/
static void freepreceph(nav_t *nav, sbs_t *sbs, lex_t *lex)
{
int i;
trace(3,"freepreceph:\n");
free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0;
free(nav->pclk); nav->pclk=NULL; nav->nc=nav->ncmax=0;
free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0;
free(sbs->msgs); sbs->msgs=NULL; sbs->n =sbs->nmax =0;
free(lex->msgs); lex->msgs=NULL; lex->n =lex->nmax =0;
for (i=0;i<nav->nt;i++) {
free(nav->tec[i].data);
free(nav->tec[i].rms );
}
free(nav->tec ); nav->tec =NULL; nav->nt=nav->ntmax=0;
#ifdef EXTSTEC
stec_free(nav);
#endif
if (fp_rtcm) fclose(fp_rtcm);
free_rtcm(&rtcm);
}
static void *rtksvrthread(void *arg)
#endif
{
rtksvr_t *svr=(rtksvr_t *)arg;
obs_t obs;
obsd_t data[MAXOBS*2];
double tt;
unsigned int tick,ticknmea;
unsigned char *p,*q;
int i,j,n,fobs[3],cycle,cputime;
INIT_ZERO(fobs);
tracet(3,"rtksvrthread:\n");
svr->state=1; obs.data=data;
svr->tick=tickget();
ticknmea=svr->tick-1000;
for (cycle=0;svr->state;cycle++) {
tick=tickget();
for (i=0;i<3;i++) {
p=svr->buff[i]+svr->nb[i]; q=svr->buff[i]+svr->buffsize;
/* read receiver raw/rtcm data from input stream */
if ((n=strread(svr->stream+i,p,q-p))<=0) {
continue;
}
/* write receiver raw/rtcm data to log stream */
strwrite(svr->stream+i+5,p,n);
svr->nb[i]+=n;
/* save peek buffer */
rtksvrlock(svr);
n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i];
memcpy(svr->pbuf[i]+svr->npb[i],p,n);
svr->npb[i]+=n;
rtksvrunlock(svr);
}
for (i=0;i<3;i++) {
if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) {
/* decode download file */
decodefile(svr,i);
}
else {
/* decode receiver raw/rtcm data */
fobs[i]=decoderaw(svr,i);
}
}
for (i=0;i<fobs[0];i++) { /* for each rover observation data */
obs.n=0;
for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) {
obs.data[obs.n++]=svr->obs[0][i].data[j];
}
for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;j++) {
obs.data[obs.n++]=svr->obs[1][0].data[j];
}
/* rtk positioning */
rtksvrlock(svr);
rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav);
rtksvrunlock(svr);
if (svr->rtk.sol.stat!=SOLQ_NONE) {
/* adjust current time */
tt=(int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt)));
/* write solution */
writesol(svr,i);
}
/* if cpu overload, inclement obs outage counter and break */
if ((int)(tickget()-tick)>=svr->cycle) {
svr->prcout+=fobs[0]-i-1;
#if 0 /* omitted v.2.4.1 */
break;
#endif
}
}
/* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0) {
writesol(svr,0);
}
/* send nmea request to base/nrtk input stream */
if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) {
if (svr->stream[1].state==1) {
if (svr->nmeareq==1) {
strsendnmea(svr->stream+1,svr->nmeapos);
}
else if (svr->nmeareq==2&&norm(svr->rtk.sol.rr,3)>0.0) {
strsendnmea(svr->stream+1,svr->rtk.sol.rr);
}
}
ticknmea=tick;
}
if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime;
/* sleep until next cycle */
sleepms(svr->cycle-cputime);
}
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
for (i=0;i<3;i++) {
svr->nb[i]=svr->npb[i]=0;
free(svr->buff[i]); svr->buff[i]=NULL;
free(svr->pbuf[i]); svr->pbuf[i]=NULL;
free_raw (svr->raw +i);
free_rtcm(svr->rtcm+i);
}
for (i=0;i<2;i++) {
svr->nsb[i]=0;
free(svr->sbuf[i]); svr->sbuf[i]=NULL;
}
return 0;
}
