/* navigation data bit decision ------------------------------------------------
* navigation data bit is determined using accumulated IP data
* args : double IP I correlation output (IP data)
* int loopms I interval of loop filter (ms)
* sdrnav_t *nav I/O navigation struct
* return : int synchronization status 1:sync 0: not sync
*-----------------------------------------------------------------------------*/
extern int checkbit(double IP, int loopms, sdrnav_t *nav)
{
int diffi=nav->biti-nav->synci;
int syncflag=ON;
static int polarity=1;
static int cnt=0;
nav->swreset=OFF;
nav->swsync=OFF;
/* if synchronization is started */
if (diffi==1||diffi==-nav->rate+1) {
nav->bitIP=IP; /* reset */
nav->swreset=ON;
cnt=1;
}
/* after synchronization */
else {
nav->bitIP+=IP; /* cumsum */
if (nav->bitIP*IP<0) syncflag=OFF;
}
/* genetaing loop filter timing */
if (cnt%loopms==0) nav->swloop=ON;
else nav->swloop=OFF;
/* if synchronization is finished */
if (diffi==0) {
if (nav->flagpol) {
polarity=-1;
}
// ????
nav->bit=(nav->bitIP<0)?-polarity:polarity;
/* set bit*/
shiftdata(&nav->fbits[0],&nav->fbits[1],sizeof(int),
nav->flen+nav->addflen-1); /* shift to left */
nav->fbits[nav->flen+nav->addflen-1]=nav->bit; /* add last */
nav->swsync=ON;
}
cnt++;
return syncflag;
}
/* navigation bit synchronization ----------------------------------------------
* check synchronization of navigation bit
* args : double IP I correlation output (IP data)
* double oldIP I previous correlation output
* sdrnav_t *nav I/O navigation struct
* return : int 1:synchronization 0: not synchronization
*-----------------------------------------------------------------------------*/
extern int checksync(double IP, double IPold, sdrnav_t *nav)
{
int i,corr=0,maxi;
/* BeiDou MEO/IGSO satellite (secondary code is NH20) */
if (nav->ctype==CTYPE_B1I&&nav->sdreph.prn>5) {
shiftdata(&nav->bitsync[0],&nav->bitsync[1],sizeof(int),nav->rate-1);
nav->bitsync[nav->rate-1]=(IP<0?-1:1);
/* correlation between NH20 */
for (i=0;i<nav->rate;i++)
corr+=nav->ocode[i]*nav->bitsync[i];
/* if synchronization success */
if (abs(corr)==nav->rate) {
nav->synci=nav->biti; /* synchronization bit index */
return 1;
}
/* other satellite */
} else {
if (IPold*IP<0) {
nav->bitsync[nav->biti]+=1; /* voting bit position */
/* check vote count */
maxi=maxvi(nav->bitsync,nav->rate,-1,-1,&nav->synci);
/* if synchronization success */
if (maxi>NAVSYNCTH) {
/* synchronization bit index */
nav->synci--; /* minus 1 index*/
if (nav->synci<0) nav->synci=nav->rate-1;
return 1;
}
}
}
return 0;
}
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;
}
