/* 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;
}
