/* rtlsdr stream callback -----------------------------------------------------
* callback for receiving RF data
*-----------------------------------------------------------------------------*/
void stream_callback_rtlsdr(unsigned char *buf, uint32_t len, void *ctx)
{
/* copy stream data to global buffer */
mlock(hbuffmtx);
memcpy(&sdrstat.buff[(sdrstat.buffcnt%MEMBUFFLEN)*2*RTLSDR_DATABUFF_SIZE],
buf,2*RTLSDR_DATABUFF_SIZE);
unmlock(hbuffmtx);
mlock(hreadmtx);
sdrstat.buffcnt++;
unmlock(hreadmtx);
if (sdrstat.stopflag) rtlsdr_cancel_async(dev);
}
/* complex IFFT ----------------------------------------------------------------
* cpx=ifft(cpx)
* args : fftwf_plan plan I fftw plan (NULL: create new plan)
* cpx_t *cpx I/O input/output complex data
* int n I number of input/output data
* return : none
*-----------------------------------------------------------------------------*/
extern void cpxifft(fftwf_plan plan, cpx_t *cpx, int n)
{
if (plan==NULL) {
mlock(hfftmtx);
fftwf_plan_with_nthreads(NFFTTHREAD); /* fft execute in multi threads */
plan=fftwf_plan_dft_1d(n,cpx,cpx,FFTW_BACKWARD,FFTW_ESTIMATE);
fftwf_execute_dft(plan,cpx,cpx); /* fft */
fftwf_destroy_plan(plan);
unmlock(hfftmtx);
} else {
fftwf_execute_dft(plan,cpx,cpx); /* fft */
}
}
/* get current data buffer -----------------------------------------------------
* get current data buffer from memory buffer
* args : uint64_t buffloc I buffer location
* int n I number of grab data
* char *expbuf O extracted data buffer
* return : none
*-----------------------------------------------------------------------------*/
extern void rtlsdr_getbuff(uint64_t buffloc, int n, char *expbuf)
{
uint64_t membuffloc=2*buffloc%(MEMBUFFLEN*2*RTLSDR_DATABUFF_SIZE);
int nout;
n=2*n;
nout=(int)((membuffloc+n)-(MEMBUFFLEN*2*RTLSDR_DATABUFF_SIZE));
mlock(hbuffmtx);
if (nout>0) {
rtlsdr_exp(&sdrstat.buff[membuffloc],n-nout,expbuf);
rtlsdr_exp(&sdrstat.buff[0],nout,&expbuf[n-nout]);
} else {
rtlsdr_exp(&sdrstat.buff[membuffloc],n,expbuf);
}
unmlock(hbuffmtx);
}
/* complex FFT -----------------------------------------------------------------
* cpx=fft(cpx)
* args : fftwf_plan plan I fftw plan (NULL: create new plan)
* cpx_t *cpx I/O input/output complex data
* int n I number of input/output data
* return : none
*-----------------------------------------------------------------------------*/
extern void cpxfft(fftwf_plan plan, cpx_t *cpx, int n)
{
#ifdef FFTMTX
mlock(hfftmtx);
#endif
if (plan==NULL) {
fftwf_plan_with_nthreads(NFFTTHREAD); /* fft execute in multi threads */
plan=fftwf_plan_dft_1d(n,cpx,cpx,FFTW_FORWARD,FFTW_ESTIMATE);
fftwf_execute_dft(plan,cpx,cpx); /* fft */
fftwf_destroy_plan(plan);
} else {
fftwf_execute_dft(plan,cpx,cpx); /* fft */
}
#ifdef FFTMTX
unmlock(hfftmtx);
#endif
}
void
refxfs(Xfs *xf, int delta)
{
mlock(&xlock);
xf->ref += delta;
if(xf->ref == 0){
chat("free \"%s\", dev=%d...", xf->name, xf->dev);
free(xf->name);
free(xf->ptr);
purgebuf(xf);
if(xf->dev >= 0){
close(xf->dev);
xf->dev = -1;
}
}
unmlock(&xlock);
}
extern void cpxfft(fftwf_plan plan, cpx_t *cpx, int n)
{
#ifdef TEST
if (plan==NULL) {
mlock(hfftmtx);
fftwf_plan_with_nthreads(NFFTTHREAD); /* fft execute in multi threads */
plan=fftwf_plan_dft_1d(n,cpx,cpx,FFTW_FORWARD,FFTW_ESTIMATE);
fftwf_execute_dft(plan,cpx,cpx); /* fft */
fftwf_destroy_plan(plan);
unmlock(hfftmtx);
} else {
fftwf_execute_dft(plan,cpx,cpx); /* fft */
}
#else
sfft_plan *p;
p=sfft_make_plan(n, 50, SFFT_VERSION_3, FFTW_ESTIMATE);
sfft_exec(p, cpx, cpx);
#endif
}
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;
}
Xfs *
getxfs(char *user, char *name)
{
Xfs *xf, *fxf;
Dir *dir;
Qid dqid;
char *p, *q;
int32_t offset;
int fd, omode;
USED(user);
if(name==nil || name[0]==0)
name = deffile;
if(name == nil){
errno = Enofilsys;
return 0;
}
/*
* If the name passed is of the form 'name:offset' then
* offset is used to prime xf->offset. This allows accessing
* a FAT-based filesystem anywhere within a partition.
* Typical use would be to mount a filesystem in the presence
* of a boot manager programme at the beginning of the disc.
*/
offset = 0;
if(p = strrchr(name, ':')){
*p++ = 0;
offset = strtol(p, &q, 0);
chat("name %s, offset %ld\n", p, offset);
if(offset < 0 || p == q){
errno = Enofilsys;
return 0;
}
offset *= Sectorsize;
}
if(readonly)
omode = OREAD;
else
omode = ORDWR;
fd = open(name, omode);
if(fd < 0 && omode==ORDWR){
omode = OREAD;
fd = open(name, omode);
}
if(fd < 0){
chat("can't open %s: %r\n", name);
errno = Eerrstr;
return 0;
}
dir = dirfstat(fd);
if(dir == nil){
errno = Eio;
close(fd);
return 0;
}
dqid = dir->qid;
free(dir);
mlock(&xlock);
for(fxf=0,xf=xhead; xf; xf=xf->next){
if(xf->ref == 0){
if(fxf == 0)
fxf = xf;
continue;
}
if(!eqqid(xf->qid, dqid))
continue;
if(strcmp(xf->name, name) != 0 || xf->dev < 0)
continue;
if(devcheck(xf) < 0) /* look for media change */
continue;
if(offset && xf->offset != offset)
continue;
chat("incref \"%s\", dev=%d...", xf->name, xf->dev);
++xf->ref;
unmlock(&xlock);
close(fd);
return xf;
}
if(fxf == nil){
fxf = malloc(sizeof(Xfs));
if(fxf == nil){
unmlock(&xlock);
close(fd);
errno = Enomem;
return nil;
}
fxf->next = xhead;
xhead = fxf;
}
chat("alloc \"%s\", dev=%d...", name, fd);
fxf->name = strdup(name);
fxf->ref = 1;
fxf->qid = dqid;
fxf->dev = fd;
fxf->fmt = 0;
fxf->offset = offset;
fxf->ptr = nil;
fxf->isfat32 = 0;
fxf->omode = omode;
unmlock(&xlock);
return fxf;
}
Xfile *
xfile(int fid, int flag)
{
Xfile **hp, *f, *pf;
int k;
k = ((uint32_t)fid) % FIDMOD;
hp = &xfiles[k];
mlock(&xlocks[k]);
pf = nil;
for(f=*hp; f; f=f->next){
if(f->fid == fid)
break;
pf = f;
}
if(f && pf){
pf->next = f->next;
f->next = *hp;
*hp = f;
}
switch(flag){
default:
panic("xfile");
case Asis:
unmlock(&xlocks[k]);
return (f && f->xf && f->xf->dev < 0) ? nil : f;
case Clean:
break;
case Clunk:
if(f){
*hp = f->next;
unmlock(&xlocks[k]);
clean(f);
mlock(&freelock);
f->next = freelist;
freelist = f;
unmlock(&freelock);
} else
unmlock(&xlocks[k]);
return nil;
}
unmlock(&xlocks[k]);
if(f)
return clean(f);
mlock(&freelock);
if(f = freelist){ /* assign = */
freelist = f->next;
unmlock(&freelock);
} else {
unmlock(&freelock);
f = malloc(sizeof(Xfile));
if(f == nil){
errno = Enomem;
return nil;
}
}
mlock(&xlocks[k]);
f->next = *hp;
*hp = f;
unmlock(&xlocks[k]);
f->fid = fid;
f->flags = 0;
f->qid = (Qid){0,0,0};
f->xf = nil;
f->ptr = nil;
return f;
}
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;
}
