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