/* LEX correlation function ---------------------------------------------------- * compute LEX message based on FFT * args : sdrch_t *sdr I sdr channel struct * char *data I input 4ms IF data * int dtype I sampling data type (1:real,2:complex) * double ti I sampling interval (s) * int n I number of samples * double freq I doppler frequency computed by L1 signal (Hz) * double crate I code chip rate (chip/s) * int m I number of FFT points * cpx_t codex I frequency domain code * double *cn O estimated C/N0 * return : uint8_t LEX message word * note : LEX message uses CSK modulation and it can be solve by FFT correlation * LEX(E6) doppler shift and code phase are estimated by L1 signal *-----------------------------------------------------------------------------*/ uint8_t lexcorr_fft(sdrch_t *sdr, const char *data, int dtype, double ti, int n, double freq, double crate, int m, cpx_t* codex, double *cn) { int codei,codei2,corri,exinds,exinde; cpx_t *datax; short *dataI,*dataQ; char *dataR; double peakr,*P,maxP,maxP2,meanP; /* memory allocation */ if (!(P=(double*)calloc(m,sizeof(double))) || !(dataR=(char *)sdrmalloc(sizeof(char )*m*dtype))|| !(dataI=(short *)sdrmalloc(sizeof(short)*m))|| !(dataQ=(short *)sdrmalloc(sizeof(short)*m))|| !(datax=cpxmalloc(m))) return -1; /* zero padding */ memset(dataR,0,m*dtype); memcpy(dataR,data,n*dtype); /* mix local carrier */ mixcarr(dataR,dtype,ti,m,freq,0.0,dataI,dataQ); /* to complex */ cpxcpx(dataI,dataQ,(1.0/32)/m,m,datax); /* convolution */ if (plan==NULL||iplan==NULL) { fftwf_plan_with_nthreads(NFFTTHREAD); /* fft execute in multi threads */ plan=fftwf_plan_dft_1d(n,datax,datax,FFTW_FORWARD,FFTW_ESTIMATE); fftwf_plan_with_nthreads(NFFTTHREAD); /* fft execute in multi threads */ iplan=fftwf_plan_dft_1d(n,datax,datax,FFTW_BACKWARD,FFTW_ESTIMATE); } cpxconv(plan,iplan,datax,codex,m,m,0,P); /* maximum index */ maxP=maxvd(P,m,-1,-1,&codei); corri=(int)((double)(n-codei)/n*sdr->clen/2); if (corri==(int)(sdr->clen/2)) corri=0; /* C/N0 calculation */ exinds=codei-sdr->nsampchip; if(exinds<0) exinds+=n; /* excluded index */ exinde=codei+sdr->nsampchip; if(exinde>=n) exinde-=n; meanP=meanvd(P,n,exinds,exinde); /* mean of correlation */ (*cn)=10*log10(maxP/meanP/sdr->ctime); maxP2=maxvd(P,m,exinds,exinde,&codei2); peakr=maxP/maxP2; if (peakr<1.5) SDRPRINTF("error: peakr=%.1f\n",peakr); /* message must be 0-255 */ if (corri>255) SDRPRINTF("error: corri=%05d codei=%06d cn0=%.1f\n",corri,codei,cn); free(P); sdrfree(dataR); sdrfree(dataI); sdrfree(dataQ); cpxfree(datax); return (uint8_t)corri; }
/* rtlsdr configuration function ----------------------------------------------- * load configuration file and setting * args : none * return : int status 0:okay -1:failure *-----------------------------------------------------------------------------*/ extern int rtlsdr_initconf(void) { int ret; /* Set the sample rate */ ret=verbose_set_sample_rate(dev,RTLSDR_SAMPLE_RATE); if (ret<0) { SDRPRINTF("error: failed to set samplerate\n"); return -1; } /* Set the frequency */ ret=verbose_set_frequency(dev,RTLSDR_FREQUENCY); if (ret<0) { SDRPRINTF("error: failed to set frequency\n"); return -1; } /* Enable automatic gain */ ret=verbose_auto_gain(dev); if (ret<0) { SDRPRINTF("error: failed to set automatic gain\n"); return -1; } /* set ppm offset */ ret=verbose_ppm_set(dev,sdrini.rtlsdrppmerr); if (ret<0) { SDRPRINTF("error: failed to set ppm\n"); return -1; } return 0; }
/* get full path from relative path -------------------------------------------- * args : char *relpath I relative path * char *fullpath O full path * return : int 0:success, -1:failure *-----------------------------------------------------------------------------*/ extern int getfullpath(char *relpath, char *abspath) { #ifdef WIN32 if (_fullpath(abspath,relpath,_MAX_PATH)==NULL) { SDRPRINTF("error: getfullpath %s\n",relpath); return -1; } #else if (realpath(relpath,abspath)==NULL) { SDRPRINTF("error: getfullpath %s\n",relpath); return -1; } #endif return 0; }
/* decode GLONASS navigation data ---------------------------------------------- * decode GLONASS navigation data and extract ephemeris * args : sdrnav_t *nav I/O sdr navigation struct * return : int string number (1-5) *-----------------------------------------------------------------------------*/ extern int decode_g1(sdrnav_t *nav) { int i,id=0,bits1[170],bits2[85]; uint8_t bin[11]; /* 85/8 word bits */ /* remove meandr from data */ for (i=0;i<170;i++) { if (i%2==0) bits1[i]= nav->polarity*nav->fbits[i]; else bits1[i]=-nav->polarity*nav->fbits[i]; } for (i=0;i<84;i++) { bits2[i+1]=bits1[2*i]*bits1[2*(i+1)]; } bits2[0] = -1; bits2byte(bits2,85,11,0,bin); /* decode navigation data */ id=decode_flame_g1(bin,&nav->sdreph); if (id<1||id>15) SDRPRINTF("error: GLONASS word number sfn=%d\n",id); return id; }
/* check initial value --------------------------------------------------------- * checking value in sdrini struct * args : sdrini_t *ini I sdrini struct * return : int 0:okay -1:error *-----------------------------------------------------------------------------*/ extern int chk_initvalue(sdrini_t *ini) { int ret; /* checking frequency input */ if ((ini->f_sf[0]<=0||ini->f_sf[0]>100e6) || (ini->f_if[0]<0 ||ini->f_if[0]>100e6)) { SDRPRINTF("error: wrong freq. input sf1: %.0f if1: %.0f\n", ini->f_sf[0],ini->f_if[0]); return -1; } /* checking frequency input */ if(ini->useif2||ini->fend==FEND_STEREO) { if ((ini->f_sf[1]<=0||ini->f_sf[1]>100e6) || (ini->f_if[1]<0 ||ini->f_if[1]>100e6)) { SDRPRINTF("error: wrong freq. input sf2: %.0f if2: %.0f\n", ini->f_sf[1],ini->f_if[1]); return -1; } } /* checking port number input */ if ((ini->rtcmport<0||ini->rtcmport>32767) || (ini->lexport<0||ini->lexport>32767)) { SDRPRINTF("error: wrong rtcm port rtcm:%d lex:%d\n", ini->rtcmport,ini->lexport); return -1; } /* checking filepath */ if (ini->fend==FEND_FILE ||ini->fend==FEND_FSTEREO|| ini->fend==FEND_FGN3SV2||ini->fend==FEND_FGN3SV2|| ini->fend==FEND_FRTLSDR||ini->fend==FEND_FBLADERF) { if (ini->useif1&&((ret=GetFileAttributes(ini->file1))<0)){ SDRPRINTF("error: file1 doesn't exist: %s\n",ini->file1); return -1; } if (ini->useif2&&((ret=GetFileAttributes(ini->file2))<0)){ SDRPRINTF("error: file2 doesn't exist: %s\n",ini->file2); return -1; } if ((!ini->useif1)&&(!ini->useif2)) { SDRPRINTF("error: file1 or file2 are not selected\n"); return -1; } } /* checking rinex directory */ if (ini->rinex) { if ((ret=GetFileAttributes(ini->rinexpath))<0) { SDRPRINTF("error: rinex output directory doesn't exist: %s\n", ini->rinexpath); return -1; } } return 0; }
/* initialize tracking struct -------------------------------------------------- * set value to tracking struct * args : int sat I satellite number * int ctype I code type (CTYPE_L1CA...) * double ctime I code period (s) * sdrtrk_t *trk I/0 tracking struct * return : int 0:okay -1:error *-----------------------------------------------------------------------------*/ extern int inittrkstruct(int sat, int ctype, double ctime, sdrtrk_t *trk) { int i,prn; int ctimems=(int)(ctime*1000); int sys=satsys(sat,&prn); /* set tracking parameter */ inittrkprmstruct(trk); /* correlation point */ trk->corrn=sdrini.trkcorrn; trk->corrp=(int *)malloc(sizeof(int)*trk->corrn); for (i=0;i<trk->corrn;i++) { trk->corrp[i]=sdrini.trkcorrd*(i+1); if (trk->corrp[i]==sdrini.trkcorrp){ trk->ne=2*(i+1)-1; /* Early */ trk->nl=2*(i+1); /* Late */ } } /* correlation point for plot */ (trk->corrx=(double *)calloc(2*trk->corrn+1,sizeof(double))); for (i=1;i<=trk->corrn;i++) { trk->corrx[2*i-1]=-sdrini.trkcorrd*i; trk->corrx[2*i ]= sdrini.trkcorrd*i; } trk->II =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->QQ =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->oldI =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->oldQ =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->sumI =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->sumQ =(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->oldsumI=(double*)calloc(1+2*trk->corrn,sizeof(double)); trk->oldsumQ=(double*)calloc(1+2*trk->corrn,sizeof(double)); if (ctype==CTYPE_L1CA) trk->loop=LOOP_L1CA; if (ctype==CTYPE_G1) trk->loop=LOOP_G1; if (ctype==CTYPE_E1B) trk->loop=LOOP_E1B; if (ctype==CTYPE_L1SAIF) trk->loop=LOOP_SBAS; if (ctype==CTYPE_L1SBAS) trk->loop=LOOP_SBAS; if (ctype==CTYPE_B1I&&prn>5 ) trk->loop=LOOP_B1I; if (ctype==CTYPE_B1I&&prn<=5) trk->loop=LOOP_B1IG; /* for LEX */ if (sys==SYS_QZS&&ctype==CTYPE_L1CA&&sdrini.nchL6) trk->loop=LOOP_LEX; /* loop interval (ms) */ trk->loopms=trk->loop*ctimems; if (!trk->II||!trk->QQ||!trk->oldI||!trk->oldQ||!trk->sumI||!trk->sumQ|| !trk->oldsumI||!trk->oldsumQ) { SDRPRINTF("error: inittrkstruct memory allocation\n"); return -1; } return 0; }
/* rtlsdr initialization ------------------------------------------------------- * search front end and initialization * args : none * return : int status 0:okay -1:failure *-----------------------------------------------------------------------------*/ extern int rtlsdr_init(void) { int ret,dev_index=0;; /* open rtlsdr */ dev_index=verbose_device_search("0"); ret=rtlsdr_open(&dev, (uint32_t)dev_index); if (ret<0) { SDRPRINTF("error: failed to open rtlsdr device #%d.\n",dev_index); return -1; } /* set configuration */ ret=rtlsdr_initconf(); if (ret<0) { SDRPRINTF("error: failed to initialize rtlsdr\n"); return -1; } return 0; }
/* start grabber --------------------------------------------------------------- * start grabber of front end * args : none * return : int status 0:okay -1:failure *-----------------------------------------------------------------------------*/ extern int rtlsdr_start(void) { int ret; /* reset endpoint before we start reading from it (mandatory) */ ret=verbose_reset_buffer(dev); if (ret<0) { SDRPRINTF("error: failed to reset buffers\n"); return -1; } /* start stream and stay there until we kill the stream */ ret=rtlsdr_read_async(dev,stream_callback_rtlsdr, NULL,RTLSDR_ASYNC_BUF_NUMBER,2*RTLSDR_DATABUFF_SIZE); if (ret<0&&!sdrstat.stopflag) { SDRPRINTF("error: failed to read in async mode\n"); return -1; } return 0; }
/* decode GPS/QZS L1CA navigation data ----------------------------------------- * decode GPS/QZS L1CA navigation data and extract ephemeris * args : sdrnav_t *nav I/O sdr navigation struct * return : int subframe ID (1-5) *-----------------------------------------------------------------------------*/ extern int decode_l1ca(sdrnav_t *nav) { int i,j,id=0; uint8_t bin[38]; /* bit inversion */ for (i=0;i<10;i++) { if (nav->fbitsdec[i*30+1]==-1) { for (j=2;j<26;j++) nav->fbitsdec[i*30+j]*=-1; } } bits2byte(&nav->fbitsdec[nav->addflen],nav->flen,38,0,bin); /* decode navigation data */ id=decode_frame_l1ca(bin,&nav->sdreph); if (id<1||id>5) SDRPRINTF("error: GPS subframe number sfn=%d\n",id); return id; }
/* correlator ------------------------------------------------------------------ * multiply sampling data and carrier (I/Q), multiply code (E/P/L), and integrate * args : char *data I sampling data vector (n x 1 or 2n x 1) * int dtype I sampling data type (1:real,2:complex) * double ti I sampling interval (s) * int n I number of samples * double freq I carrier frequency (Hz) * double phi0 I carrier initial phase (rad) * double crate I code chip rate (chip/s) * double coff I code chip offset (chip) * int s I correlator points (sample) * short *I,*Q O correlation power I,Q * I={I_P,I_E1,I_L1,I_E2,I_L2,...,I_Em,I_Lm} * Q={Q_P,Q_E1,Q_L1,Q_E2,Q_L2,...,Q_Em,Q_Lm} * return : none * notes : see above for data *-----------------------------------------------------------------------------*/ extern void correlator(const char *data, int dtype, double ti, int n, double freq, double phi0, double crate, double coff, int* s, int ns, double *II, double *QQ, double *remc, double *remp, short* codein, int coden) { short *dataI=NULL,*dataQ=NULL,*code_e=NULL,*code; int i; int smax=s[ns-1]; /* 8 is treatment of remainder in SSE2 */ if (!(dataI=(short *)sdrmalloc(sizeof(short)*(n+64)))|| !(dataQ=(short *)sdrmalloc(sizeof(short)*(n+64)))|| !(code_e=(short *)sdrmalloc(sizeof(short)*(n+2*smax)))) { SDRPRINTF("error: correlator memory allocation\n"); return; } code=code_e+smax; /* mix local carrier */ *remp=mixcarr(data,dtype,ti,n,freq,phi0,dataI,dataQ); /* resampling code */ *remc=rescode(codein,coden,coff,smax,ti*crate,n,code_e); /* multiply code and integrate */ dot_23(dataI,dataQ,code,code-s[0],code+s[0],n,II,QQ); for (i=1;i<ns;i++) { dot_22(dataI,dataQ,code-s[i],code+s[i],n,II+1+i*2,QQ+1+i*2); } for (i=0;i<1+2*ns;i++) { II[i]*=CSCALE; QQ[i]*=CSCALE; } sdrfree(dataI); sdrfree(dataQ); sdrfree(code_e); dataI=dataQ=code_e=NULL; }
/* parallel correlator --------------------------------------------------------- * fft based parallel correlator * args : char *data I sampling data vector (n x 1 or 2n x 1) * int dtype I sampling data type (1:real,2:complex) * double ti I sampling interval (s) * int n I number of samples * double *freq I doppler search frequencies (Hz) * int nfreq I number of frequencies * double crate I code chip rate (chip/s) * int m I number of resampling data * cpx_t codex I frequency domain code * double *P O normalized correlation power vector * return : none * notes : P=abs(ifft(conj(fft(code)).*fft(data.*e^(2*pi*freq*t*i)))).^2 *-----------------------------------------------------------------------------*/ extern void pcorrelator(const char *data, int dtype, double ti, int n, double *freq, int nfreq, double crate, int m, cpx_t* codex, double *P) { int i; cpx_t *datax; short *dataI,*dataQ; char *dataR; if (!(dataR=(char *)sdrmalloc(sizeof(char )*m*dtype))|| !(dataI=(short *)sdrmalloc(sizeof(short)*(m+64)))|| !(dataQ=(short *)sdrmalloc(sizeof(short)*(m+64)))|| !(datax=cpxmalloc(m))) { SDRPRINTF("error: pcorrelator memory allocation\n"); return; } /* zero padding */ memset(dataR,0,m*dtype); /* zero paddinng */ memcpy(dataR,data,2*n*dtype); /* for zero padding FFT */ for (i=0;i<nfreq;i++) { /* mix local carrier */ mixcarr(dataR,dtype,ti,m,freq[i],0.0,dataI,dataQ); /* to complex */ cpxcpx(dataI,dataQ,CSCALE/m,m,datax); /* convolution */ cpxconv(NULL,NULL,datax,codex,m,n,1,&P[i*n]); } sdrfree(dataR); sdrfree(dataI); sdrfree(dataQ); cpxfree(datax); }
void *lexthread(void * arg) #endif { sdrch_t *sdr=(sdrch_t*)arg; int cnt=0,lexch=0,state,i,nerr,errloc[LENLEXRS],time=0,dt,mid; uint64_t buffloc; double dfreq,cn0; char *data; uint8_t corri,sendbuf[LENLEXRCV],rsmsg[LENLEXRS]; uint8_t lexpre[LENLEXPRE]={0x1A,0xCF,0xFC,0x1D}; /* preamble */ sdrlex_t sdrlex={{0}}; sdrout_t out={0}; unsigned long tick=0; FILE *fplexlog=NULL,*fplexbin=NULL; short *rcode; cpx_t *xcode; if (sdrini.log) { fplexlog=fopen("LEXLOG.csv","w"); fplexbin=fopen("LEXBIN.bin","wb"); fprintf(fplexlog,"Tow,CN0,Time(ms),Error\n"); } /* start tcp server (lex) */ if (sdrini.lex) { out.soc_lex.port=sdrini.lexport; tcpsvrstart(&out.soc_lex); } data=(char*)sdrmalloc(sizeof(char)*sdr->nsamp*sdr->dtype); /* lex channel is last */ lexch=sdrini.nch; /* FFT code generation */ if (!(rcode=(short *)sdrmalloc(sizeof(short)*sdr->nsamp)) || !(xcode=cpxmalloc(sdr->nsamp))) { SDRPRINTF("error: initsdrch memory alocation\n"); return THRETVAL; } rescode(sdr->code,sdr->clen,0,0,sdr->ci,sdr->nsamp,rcode); /* resampled code */ cpxcpx(rcode,NULL,1.0,sdr->nsamp,xcode); /* FFT code */ cpxfft(NULL,xcode,sdr->nsamp); sdrfree(rcode); sleepms(3000*sdrini.nch); SDRPRINTF("**** LEX sdr thread start! ****\n"); do { /* wait event */ mlock(hlexmtx); waitevent(hlexeve,hlexmtx); unmlock(hlexmtx); /* assist from L1CA */ buffloc=sdrch[lexch].trk.codei[1]+sdrch[lexch].currnsamp+DSAMPLEX; dfreq=-sdrch[lexch].trk.D[1]*(FREQ6/FREQ1); /* get current data */ rcvgetbuff(&sdrini,buffloc,sdr->nsamp,sdr->ftype,sdr->dtype,data); tick=tickgetus(); /* LEX correlation */ corri=lexcorr_fft(sdr,data,sdr->dtype,sdr->ti,sdr->nsamp,dfreq,sdr->crate, sdr->nsamp,xcode,&cn0); dt=tickgetus()-tick; time+=dt; if (dt>4000) SDRPRINTF("error: dt=%.1fms(must be < 4ms)\n",(double)dt/1000); /* check computation time */ if (cnt%250==0) { //SDRPRINTF("time=%.2fms doppler=%.1f\n",(double)time/250000,dfreq); time=0; } shiftdata(&sdrlex.msg[0],&sdrlex.msg[1],1,LENLEXMSG-1); /* shift to left */ sdrlex.msg[LENLEXMSG-1]=corri; /* add last */ /* preamble search */ state=0; for (i=0;i<LENLEXPRE;i++) state+=abs(sdrlex.msg[i]-lexpre[i]); if (state==0) { /* reed solomon */ memset(rsmsg,0,LENLEXRS); memcpy(&rsmsg[9],&sdrlex.msg[LENLEXPRE],LENLEXRS-9); /* RS decode */ nerr=decode_rs_ccsds(rsmsg,errloc,0,0); if (nerr!=0) SDRPRINTF("RS correct %d symbols!\n",nerr); if (sdrini.log) { fprintf(fplexlog,"%f,%f,%d,%d\n", sdrch[lexch].trk.tow[0],cn0,time,nerr); fwrite(sdrlex.msg,1,LENLEXMSG,fplexbin); } if (nerr<0) { cnt++; continue; } /* <0 means failed to RS decode */ /* correct lex message */ memcpy(&sdrlex.msg[LENLEXPRE],&rsmsg[9],LENLEXRSK-9); mid=getbitu(sdrlex.msg,5*8,8); SDRPRINTF("LEX Message Type ID=%d\n",mid); /* generate send buffer */ sendbuf[0]=0xAA; /* sync code1 (see rcvlex.c) */ sendbuf[1]=0x55; /* sync code2 (see rcvlex.c) */ /* set tow (LEX message does not contain tow information...) */ setbitu(sendbuf,2*8,4*8,ROUND(sdrch[lexch].trk.tow[0]*1000)); /* set week ()*/ setbitu(sendbuf,6*8,2*8,sdrch[lexch].nav.sdreph.eph.week); memcpy(&sendbuf[8],sdrlex.msg,LENLEXMSG-LENLEXRSP); /* LEX message */ /* send LEX message */ if (sdrini.lex&&out.soc_lex.flag) send(out.soc_lex.c_soc,(char*)sendbuf,LENLEXRCV,0); } cnt++; } while (!sdrstat.stopflag); if (sdrini.log) { fclose(fplexlog); fclose(fplexbin); } if (out.soc_lex.flag) tcpsvrclose(&out.soc_lex); cpxfree(xcode); return THRETVAL; }
/* sdr navigation data function ------------------------------------------------ * decide navigation bit and decode navigation data * args : sdrch_t *sdr I/O sdr channel struct * uint64_t buffloc I buffer location * uint64_t cnt I counter of sdr channel thread * return : none *-----------------------------------------------------------------------------*/ extern void sdrnavigation(sdrch_t *sdr, uint64_t buffloc, uint64_t cnt) { int sfn; sdr->nav.biti=cnt%sdr->nav.rate; /* current bit location for bit sync */ sdr->nav.ocodei=(sdr->nav.biti-sdr->nav.synci-1); /* overlay code index */ if (sdr->nav.ocodei<0) sdr->nav.ocodei+=sdr->nav.rate; /* navigation bit synchronization */ /* if synchronization does not need (NH20 case) */ if (sdr->nav.rate==1&&cnt>2000/(sdr->ctime*1000)) { sdr->nav.synci=0; sdr->nav.flagsync=ON; } /* check bit synchronization */ if (!sdr->nav.flagsync&&cnt>2000/(sdr->ctime*1000)) sdr->nav.flagsync=checksync(sdr->trk.II[0],sdr->trk.oldI[0],&sdr->nav); if (sdr->nav.flagsync) { /* navigation bit determination */ if (checkbit(sdr->trk.II[0],sdr->trk.loopms,&sdr->nav)==OFF) { //SDRPRINTF("%s nav sync error!!\n",sdr->satstr); } /* check navigation frame synchronization */ if (sdr->nav.swsync) { /* FEC (foward error correction) decoding */ if (!sdr->nav.flagtow) predecodefec(&sdr->nav); /* frame synchronization (preamble search) */ if (!sdr->nav.flagtow) sdr->nav.flagsyncf=findpreamble(&sdr->nav); /* preamble is found */ if (sdr->nav.flagsyncf&&!sdr->nav.flagtow) { /* set reference sample data */ sdr->nav.firstsf=buffloc; sdr->nav.firstsfcnt=cnt; SDRPRINTF("*** find preamble! %s %d %d ***\n", sdr->satstr,(int)cnt,sdr->nav.polarity); sdr->nav.flagtow=ON; } } /* decoding navigation data */ if (sdr->nav.flagtow&&sdr->nav.swsync) { /* if frame bits are stored */ if ((int)(cnt-sdr->nav.firstsfcnt)%sdr->nav.update==0) { predecodefec(&sdr->nav); /* FEC decoding */ sfn=decodenav(&sdr->nav); /* navigation message decoding */ SDRPRINTF("%s ID=%d tow:%.1f week=%d cnt=%d\n", sdr->satstr,sfn,sdr->nav.sdreph.tow_gpst, sdr->nav.sdreph.week_gpst,(int)cnt); /* set reference tow data */ if (sdr->nav.sdreph.tow_gpst==0) { /* reset if tow does not decoded */ sdr->nav.flagsyncf=OFF; sdr->nav.flagtow=OFF; } else if (cnt-sdr->nav.firstsfcnt==0) { sdr->nav.flagdec=ON; sdr->nav.sdreph.eph.sat=sdr->sat; /* satellite number */ sdr->nav.firstsftow=sdr->nav.sdreph.tow_gpst; /* tow */ if (sdr->nav.ctype==CTYPE_G1) sdr->prn=sdr->nav.sdreph.prn; } } } } }
/* initialize navigation struct ------------------------------------------------ * set value to navigation struct * args : int sys I system type (SYS_GPS...) * int ctype I code type (CTYPE_L1CA...) * int prn I PRN (or SV) number * sdrnav_t *nav I/0 navigation struct * return : int 0:okay -1:error *-----------------------------------------------------------------------------*/ extern int initnavstruct(int sys, int ctype, int prn, sdrnav_t *nav) { int i; int pre_l1ca[8]= { 1,-1,-1,-1, 1,-1, 1, 1}; /* L1CA preamble*/ int pre_e1b[10]= { 1,-1, 1,-1,-1, 1, 1, 1, 1, 1}; /* E1B preamble */ int pre_g1[30]= {-1,-1,-1,-1,-1, 1, 1, 1,-1,-1, 1,-1,-1,-1, 1,-1, 1,-1, 1, 1, 1, 1,-1, 1, 1,-1, 1,-1,-1, 1}; /* G1 preamble */ int pre_b1i[11]= {-1,-1,-1, 1, 1, 1,-1, 1, 1,-1, 1}; /* B1I preamble */ int pre_sbs[24]= { 1,-1, 1,-1, 1, 1,-1,-1,-1, 1, 1,-1,-1, 1,-1, 1,-1,-1, 1, 1, 1 -1,-1, 1}; /* SBAS L1/QZS L1SAIF preamble */ int poly[2]={V27POLYA,V27POLYB}; nav->ctype=ctype; nav->sdreph.ctype=ctype; nav->sdreph.prn=prn; nav->sdreph.eph.iodc=-1; /* GPS/QZS L1CA */ if (ctype==CTYPE_L1CA) { nav->rate=NAVRATE_L1CA; nav->flen=NAVFLEN_L1CA; nav->addflen=NAVADDFLEN_L1CA; nav->prelen=NAVPRELEN_L1CA; nav->sdreph.cntth=NAVEPHCNT_L1CA; nav->update=(int)(nav->flen*nav->rate); memcpy(nav->prebits,pre_l1ca,sizeof(int)*nav->prelen); /* overlay code (all 1) */ nav->ocode=(short *)calloc(nav->rate,sizeof(short)); for (i=0;i<nav->rate;i++) nav->ocode[i]=1; } /* SBAS/QZS L1SAIF */ if (ctype==CTYPE_L1SAIF||ctype==CTYPE_L1SBAS) { nav->rate=NAVRATE_SBAS; nav->flen=NAVFLEN_SBAS; nav->addflen=NAVADDFLEN_SBAS; nav->prelen=NAVPRELEN_SBAS; nav->sdreph.cntth=NAVEPHCNT_SBAS; nav->update=(int)(nav->flen/3*nav->rate); memcpy(nav->prebits,pre_sbs,sizeof(int)*nav->prelen); /* create fec */ if((nav->fec=create_viterbi27_port(NAVFLEN_SBAS/2))==NULL) { SDRPRINTF("error: create_viterbi27 failed\n"); return -1; } /* set polynomial */ set_viterbi27_polynomial_port(poly); /* overlay code (all 1) */ nav->ocode=(short *)calloc(nav->rate,sizeof(short)); for (i=0;i<nav->rate;i++) nav->ocode[i]=1; } /* GLONASS G1 */ if (ctype==CTYPE_G1) { nav->rate=NAVRATE_G1; nav->flen=NAVFLEN_G1; nav->addflen=NAVADDFLEN_G1; nav->prelen=NAVPRELEN_G1; nav->sdreph.cntth=NAVEPHCNT_G1; nav->update=(int)(nav->flen*nav->rate); memcpy(nav->prebits,pre_g1,sizeof(int)*nav->prelen); nav->sdreph.geph.frq=prn; /* glonass frequency number */ /* overlay code (all 1) */ nav->ocode=(short *)calloc(nav->rate,sizeof(short)); for (i=0;i<nav->rate;i++) nav->ocode[i]=1; } /* Galileo E1B */ if (ctype==CTYPE_E1B) { nav->rate=NAVRATE_E1B; nav->flen=NAVFLEN_E1B; nav->addflen=NAVADDFLEN_E1B; nav->prelen=NAVPRELEN_E1B; nav->sdreph.cntth=NAVEPHCNT_E1B; nav->update=(int)(nav->flen*nav->rate); memcpy(nav->prebits,pre_e1b,sizeof(int)*nav->prelen); /* create fec */ if((nav->fec=create_viterbi27_port(120))==NULL) { SDRPRINTF("error: create_viterbi27 failed\n"); return -1; } /* set polynomial */ set_viterbi27_polynomial_port(poly); /* overlay code (all 1) */ nav->ocode=(short *)calloc(nav->rate,sizeof(short)); for (i=0;i<nav->rate;i++) nav->ocode[i]=1; } /* BeiDou B1I */ if (ctype==CTYPE_B1I) { /* MEO/IGSO (D1 NAV) */ if (prn>5) { nav->rate=NAVRATE_B1I; nav->flen=NAVFLEN_B1I; nav->addflen=NAVADDFLEN_B1I; nav->prelen=NAVPRELEN_B1I; nav->sdreph.cntth=NAVEPHCNT_B1I; nav->update=(int)(nav->flen*nav->rate); memcpy(nav->prebits,pre_b1i,sizeof(int)*nav->prelen); /* secondary code generation */ nav->ocode=gencode(-1,CTYPE_NH20,NULL,NULL); /* GEO (D2 NAV) */ } else { nav->rate=NAVRATE_B1IG; nav->flen=NAVFLEN_B1IG; nav->addflen=NAVADDFLEN_B1IG; nav->prelen=NAVPRELEN_B1IG; nav->sdreph.cntth=NAVEPHCNT_B1IG; nav->update=(int)(nav->flen*nav->rate); memcpy(nav->prebits,pre_b1i,sizeof(int)*nav->prelen); /* overlay code (all 1) */ nav->ocode=(short *)calloc(nav->rate,sizeof(short)); for (i=0;i<nav->rate;i++) nav->ocode[i]=1; } } if (!(nav->bitsync= (int *)calloc(nav->rate,sizeof(int))) || !(nav->fbits= (int *)calloc(nav->flen+nav->addflen,sizeof(int))) || !(nav->fbitsdec=(int *)calloc(nav->flen+nav->addflen,sizeof(int)))) { SDRPRINTF("error: initnavstruct memory alocation\n"); return -1; } return 0; }
/* read ini file --------------------------------------------------------------- * read ini file and set value to sdrini struct * args : sdrini_t *ini I/0 sdrini struct * return : int 0:okay -1:error * note : this function is only used in CLI application *-----------------------------------------------------------------------------*/ extern int readinifile(sdrini_t *ini) { int i,ret; char inifile[]="./gnss-sdrcli.ini"; char fendfile[256],str[256]; /* check ini file */ if ((ret=GetFileAttributes(inifile))<0){ SDRPRINTF("error: gnss-sdrcli.ini doesn't exist\n"); return -1; } /* receiver setting */ readinistr(inifile,"RCV","FENDCONF",fendfile); /* check front-end configuration file */ if ((ret=GetFileAttributes(fendfile))<0){ SDRPRINTF("error: %s doesn't exist\n",fendfile); return -1; } readinistr(fendfile,"FEND","TYPE",str); if (strcmp(str,"STEREO")==0) ini->fend=FEND_STEREO; else if (strcmp(str,"GN3SV2")==0) ini->fend=FEND_GN3SV2; else if (strcmp(str,"GN3SV3")==0) ini->fend=FEND_GN3SV3; else if (strcmp(str,"BLADERF")==0) ini->fend=FEND_BLADERF; else if (strcmp(str,"RTLSDR")==0) ini->fend=FEND_RTLSDR; else if (strcmp(str,"FILESTEREO")==0) ini->fend=FEND_FSTEREO; else if (strcmp(str,"FILEGN3SV2")==0) ini->fend=FEND_FGN3SV2; else if (strcmp(str,"FILEGN3SV3")==0) ini->fend=FEND_FGN3SV3; else if (strcmp(str,"FILEBLADERF")==0) ini->fend=FEND_FBLADERF; else if (strcmp(str,"FILERTLSDR")==0) ini->fend=FEND_FRTLSDR; else if (strcmp(str,"FILE")==0) ini->fend=FEND_FILE; else { SDRPRINTF("error: wrong frontend type: %s\n",str); return -1; } if (ini->fend==FEND_FILE ||ini->fend==FEND_FSTEREO|| ini->fend==FEND_FGN3SV2 ||ini->fend==FEND_FGN3SV3|| ini->fend==FEND_FBLADERF||ini->fend==FEND_FRTLSDR) { readinistr(fendfile,"FEND","FILE1",ini->file1); if (strcmp(ini->file1,"")!=0) ini->useif1=ON; } if (ini->fend==FEND_FILE) { readinistr(fendfile,"FEND","FILE2",ini->file2); if (strcmp(ini->file2,"")!=0) ini->useif2=ON; } ini->f_cf[0]=readinidouble(fendfile,"FEND","CF1"); ini->f_sf[0]=readinidouble(fendfile,"FEND","SF1"); ini->f_if[0]=readinidouble(fendfile,"FEND","IF1"); ini->dtype[0]=readiniint(fendfile,"FEND","DTYPE1"); ini->f_cf[1]=readinidouble(fendfile,"FEND","CF2"); ini->f_sf[1]=readinidouble(fendfile,"FEND","SF2"); ini->f_if[1]=readinidouble(fendfile,"FEND","IF2"); ini->dtype[1]=readiniint(fendfile,"FEND","DTYPE2"); /* RTL-SDR only */ ini->rtlsdrppmerr=readiniint(fendfile,"FEND","PPMERR"); /* tracking parameter setting */ ini->trkcorrn=readiniint(fendfile,"TRACK","CORRN"); ini->trkcorrd=readiniint(fendfile,"TRACK","CORRD"); ini->trkcorrp=readiniint(fendfile,"TRACK","CORRP"); ini->trkdllb[0]=readinidouble(fendfile,"TRACK","DLLB1"); ini->trkpllb[0]=readinidouble(fendfile,"TRACK","PLLB1"); ini->trkfllb[0]=readinidouble(fendfile,"TRACK","FLLB1"); ini->trkdllb[1]=readinidouble(fendfile,"TRACK","DLLB2"); ini->trkpllb[1]=readinidouble(fendfile,"TRACK","PLLB2"); ini->trkfllb[1]=readinidouble(fendfile,"TRACK","FLLB2"); /* channel setting */ ini->nch=readiniint(inifile,"CHANNEL","NCH"); if (ini->nch<1) { SDRPRINTF("error: wrong inifile value NCH=%d\n",ini->nch); return -1; } if ((ret=readiniints(inifile,"CHANNEL","PRN",ini->prn,ini->nch))<0 || (ret=readiniints(inifile,"CHANNEL","SYS",ini->sys,ini->nch))<0 || (ret=readiniints(inifile,"CHANNEL","CTYPE",ini->ctype,ini->nch))<0 || (ret=readiniints(inifile,"CHANNEL","FTYPE",ini->ftype,ini->nch))<0) { SDRPRINTF("error: wrong inifile value NCH=%d\n",ini->nch); return -1; } /* plot setting */ ini->pltacq=readiniint(inifile,"PLOT","ACQ"); ini->plttrk=readiniint(inifile,"PLOT","TRK"); /* output setting */ ini->outms =readiniint(inifile,"OUTPUT","OUTMS"); ini->rinex =readiniint(inifile,"OUTPUT","RINEX"); ini->rtcm =readiniint(inifile,"OUTPUT","RTCM"); ini->lex =readiniint(inifile,"OUTPUT","LEX"); ini->sbas =readiniint(inifile,"OUTPUT","SBAS"); ini->log =readiniint(inifile,"OUTPUT","LOG"); readinistr(inifile,"OUTPUT","RINEXPATH",ini->rinexpath); ini->rtcmport=readiniint(inifile,"OUTPUT","RTCMPORT"); ini->lexport =readiniint(inifile,"OUTPUT","LEXPORT"); ini->sbasport=readiniint(inifile,"OUTPUT","SBASPORT"); /* spectrum setting */ ini->pltspec=readiniint(inifile,"SPECTRUM","SPEC"); /* sdr channel setting */ for (i=0;i<sdrini.nch;i++) { if (sdrini.ctype[i]==CTYPE_L1CA || sdrini.ctype[i]==CTYPE_G1 || sdrini.ctype[i]==CTYPE_E1B || sdrini.ctype[i]==CTYPE_B1I ) { sdrini.nchL1++; } if (sdrini.ctype[i]==CTYPE_LEXS) { sdrini.nchL6++; } } return 0; }
/* initialize sdr channel struct ----------------------------------------------- * set value to sdr channel struct * args : int chno I channel number (1,2,...) * int sys I system type (SYS_***) * int prn I PRN number * int ctype I code type (CTYPE_***) * int dtype I data type (DTYPEI or DTYPEIQ) * int ftype I front end type (FTYPE1 or FTYPE2) * double f_cf I center (carrier) frequency (Hz) * double f_sf I sampling frequency (Hz) * double f_if I intermidiate frequency (Hz) * sdrch_t *sdr I/0 sdr channel struct * return : int 0:okay -1:error *-----------------------------------------------------------------------------*/ extern int initsdrch(int chno, int sys, int prn, int ctype, int dtype, int ftype, double f_cf, double f_sf, double f_if, sdrch_t *sdr) { int i; short *rcode; sdr->no=chno; sdr->sys=sys; sdr->prn=prn; sdr->sat=satno(sys,prn); sdr->ctype=ctype; sdr->dtype=dtype; sdr->ftype=ftype; sdr->f_sf=f_sf; sdr->f_if=f_if; sdr->ti=1/f_sf; /* code generation */ if (!(sdr->code=gencode(prn,ctype,&sdr->clen,&sdr->crate))) { SDRPRINTF("error: gencode\n"); return -1; } sdr->ci=sdr->ti*sdr->crate; sdr->ctime=sdr->clen/sdr->crate; sdr->nsamp=(int)(f_sf*sdr->ctime); sdr->nsampchip=(int)(sdr->nsamp/sdr->clen); satno2id(sdr->sat,sdr->satstr); /* set carrier frequency */ if (ctype==CTYPE_G1) { sprintf(sdr->satstr,"R%d",prn); /* frequency number instead of PRN */ sdr->f_cf=FREQ1_GLO+DFRQ1_GLO*prn; /* FDMA */ sdr->foffset=DFRQ1_GLO*prn; /* FDMA */ } else if (sdrini.fend==FEND_FRTLSDR) { sdr->foffset=f_cf*sdrini.rtlsdrppmerr*1e-6; } else { sdr->f_cf=f_cf; /* carrier frequency */ sdr->foffset=0.0; /* frequency offset */ } /* for BeiDou B1I */ if (ctype==CTYPE_B1I) sdr->nsampchip*=2; /* for BOC code */ /* acqisition struct */ initacqstruct(sys,ctype,prn,&sdr->acq); sdr->acq.nfft=2*sdr->nsamp;//calcfftnum(2*sdr->nsamp,0); /* memory allocation */ if (!(sdr->acq.freq=(double*)malloc(sizeof(double)*sdr->acq.nfreq))) { SDRPRINTF("error: initsdrch memory alocation\n"); return -1; } /* doppler search frequency */ for (i=0;i<sdr->acq.nfreq;i++) sdr->acq.freq[i]=sdr->f_if+((i-(sdr->acq.nfreq-1)/2)*sdr->acq.step) +sdr->foffset; /* tracking struct */ if (inittrkstruct(sdr->sat,ctype,sdr->ctime,&sdr->trk)<0) return -1; /* navigation struct */ if (initnavstruct(sys,ctype,prn,&sdr->nav)<0) { return -1; } /* memory allocation */ if (!(rcode=(short *)sdrmalloc(sizeof(short)*sdr->acq.nfft)) || !(sdr->xcode=cpxmalloc(sdr->acq.nfft))) { SDRPRINTF("error: initsdrch memory alocation\n"); return -1; } /* other code generation */ for (i=0;i<sdr->acq.nfft;i++) rcode[i]=0; /* zero padding */ rescode(sdr->code,sdr->clen,0,0,sdr->ci,sdr->nsamp,rcode); /* resampling */ cpxcpx(rcode,NULL,1.0,sdr->acq.nfft,sdr->xcode); /* FFT for acquisition */ cpxfft(NULL,sdr->xcode,sdr->acq.nfft); sdrfree(rcode); return 0; }
extern void *syncthread(void * arg) #endif { int i,j,nsat,isat[MAXOBS],ind[MAXSAT]={0},refi; uint64_t sampref,sampbase,codei[MAXSAT],diffcnt,mincodei; double codeid[OBSINTERPN],remcode[MAXSAT],samprefd,reftow=0,oldreftow; sdrobs_t obs[MAXSAT]; sdrtrk_t trk[MAXSAT]={{0}}; /* start tcp server (rtcm) */ if (sdrini.rtcm) { sdrout.soc_rtcm.port=sdrini.rtcmport; tcpsvrstart(&sdrout.soc_rtcm); } /* start tcp server (sbas) */ if (sdrini.sbas) { sdrout.soc_sbas.port=sdrini.sbasport; tcpsvrstart(&sdrout.soc_sbas); } /* rinex output setting */ if (sdrini.rinex) { createrinexopt(&sdrout.opt); if ((createrinexobs(sdrout.rinexobs,&sdrout.opt)<0)|| (createrinexnav(sdrout.rinexnav,&sdrout.opt)<0)) { sdrstat.stopflag=ON; } } sdrout.obsd=(obsd_t *)calloc(MAXSAT,sizeof(obsd_t)); while (!sdrstat.stopflag) { mlock(hobsmtx); /* copy all tracking data */ for (i=nsat=0;i<sdrini.nch;i++) { if (sdrch[i].nav.flagdec&&sdrch[i].nav.sdreph.eph.week!=0) { memcpy(&trk[nsat],&sdrch[i].trk,sizeof(sdrch[i].trk)); isat[nsat]=i; nsat++; } } unmlock(hobsmtx); /* find minimum tow channel (most distant satellite) */ oldreftow=reftow; reftow=3600*24*7; for (i=0;i<nsat;i++) { if (trk[i].tow[0]<reftow) reftow=trk[i].tow[0]; } /* output timing check */ if (nsat==0||oldreftow==reftow||((int)(reftow*1000)%sdrini.outms)!=0) { continue; } /* select same timing index */ for (i=0;i<nsat;i++) { for (j=0;j<OBSINTERPN;j++) { if (fabs(trk[i].tow[j]-reftow)<1E-4) { ind[i]=j; break; } } if (j==OBSINTERPN&&ind[i]==0) SDRPRINTF("%s error tow\n",sdrch[isat[i]].satstr); } /* decide reference satellite (nearest satellite) */ mincodei=UINT64_MAX; refi=0; for (i=0;i<nsat;i++) { codei[i]=trk[i].codei[ind[i]]; remcode[i]=trk[i].remcout[ind[i]]; if (trk[i].codei[ind[i]]<mincodei) { refi=i; mincodei=trk[i].codei[ind[i]]; } } /* reference satellite */ diffcnt=trk[refi].cntout[ind[refi]]-sdrch[isat[refi]].nav.firstsfcnt; sampref=sdrch[isat[refi]].nav.firstsf+ (uint64_t)(sdrch[isat[refi]].nsamp* (-PTIMING/(1000*sdrch[isat[refi]].ctime)+diffcnt)); sampbase=trk[refi].codei[OBSINTERPN-1]-10*sdrch[isat[refi]].nsamp; samprefd=(double)(sampref-sampbase); /* computation observation data */ for (i=0;i<nsat;i++) { obs[i].sys=sdrch[isat[i]].sys; obs[i].prn=sdrch[isat[i]].prn; obs[i].week=sdrch[isat[i]].nav.sdreph.week_gpst; obs[i].tow=reftow+(double)(PTIMING)/1000; obs[i].P=CLIGHT*sdrch[isat[i]].ti* ((double)(codei[i]-sampref)-remcode[i]); /* pseudo range */ /* uint64 to double for interp1 */ uint64todouble(trk[i].codei,sampbase,OBSINTERPN,codeid); obs[i].L=interp1(codeid,trk[i].L,OBSINTERPN,samprefd); obs[i].D=interp1(codeid,trk[i].D,OBSINTERPN,samprefd); obs[i].S=trk[i].S[0]; } sdrout.nsat=nsat; sdrobs2obsd(obs,nsat,sdrout.obsd); /* rinex obs output */ if (sdrini.rinex) { if (writerinexobs(sdrout.rinexobs,&sdrout.opt,sdrout.obsd, sdrout.nsat)<0) { sdrstat.stopflag=ON; } } /* rtcm obs output */ if (sdrini.rtcm&&sdrout.soc_rtcm.flag) sendrtcmobs(sdrout.obsd,&sdrout.soc_rtcm,sdrout.nsat); /* navigation data output */ for (i=0;i<sdrini.nch;i++) { if ((sdrch[i].nav.sdreph.update)&& (sdrch[i].nav.sdreph.cnt==sdrch[i].nav.sdreph.cntth)) { sdrch[i].nav.sdreph.cnt=0; sdrch[i].nav.sdreph.update=OFF; /* rtcm nav output */ if (sdrini.rtcm&&sdrout.soc_rtcm.flag) sendrtcmnav(&sdrch[i].nav.sdreph,&sdrout.soc_rtcm); /* rinex nav output */ if (sdrini.rinex) { if (writerinexnav(sdrout.rinexnav, &sdrout.opt,&sdrch[i].nav.sdreph)<0) { sdrstat.stopflag=ON; } } } } } /* thread termination */ free(sdrout.obsd); tcpsvrclose(&sdrout.soc_rtcm); tcpsvrclose(&sdrout.soc_sbas); SDRPRINTF("SDR syncthread finished!\n"); return THRETVAL; }