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