/* 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;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg, rtk_t *rtk)
{
double x[NX]={0},dx[NX],Q[NX*NX],sig;
int i,j,k,info,stat,nv,ns;
double rms = 0; //added by yuan;
double RMS = 0;
int nv_ = 0; double temp = 0; int kk = 0;
double v[(MAXOBS_+4)*1];double H[(MAXOBS_+4)*135];double var[MAXOBS_+4*1];
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i = 0; i < MAXITR; i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns,rtk);
if (nv<4) {
//sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
//added by yuan, ̽��ϴ�ֲ���н�ȷ�������������;
if ( (rtk->counter>=1) && (i>=1) ){
for (j = 0; j < nv;j++){
if (fabs(v[j]) != 0.0){
rms += v[j];
nv_++;
}
}
rms = rms / nv_;
for (j = 0; j < nv_; j++){
RMS += (v[j] - rms)*(v[j] - rms);
}
RMS = sqrt(RMS/nv_); //�в�ľ�����;
if (RMS>2.5){
temp = fabs(v[0] - rms);
for (j = 1; j < nv_;j++){
if (temp <= fabs(v[j] - rms)){
temp = fabs(v[j] - rms);
kk = j;
}
else{
continue;
}
}
opt->exsats[rtk->sat_[kk] - 1] = 1;
}
rms = 0; RMS = 0; nv_ = 0; temp = 0; kk = 0;
//nv = nv - 1; //����һ������;
}
if (nv < NX) {
//sprintf(msg, "lack of valid sats ns=%d", nv);
break;
}
/* weight by variance */
for (j = 0; j < nv; j++) {
sig = sqrt(var[j]);
v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j*NX] /= sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
//sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++)
x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
}
return stat;
}
}
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;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
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;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
fprintf(output, " Begin coordinates calculation in single point positioning mode\n");
fprintf(output, " Current coordinates: X = %f, Y = %f, Z = %f\n", x[0], x[1], x[2]);
for (i=0;i<MAXITR;i++) {
fprintf(output, " Begin iteration %i:\n", i);
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
fprintf(output, " Begin least-square estimation:\n");
fprintf(output, " ");
for(j = 0; j < NX; j++)
fprintf(output, " X[%i] = %f,", j, dx[j]);
fprintf(output, "\n");
fprintf(output, " ");
for(j = 0; j < nv; j++)
fprintf(output, " v[%i] = %f,", j, v[j]);
fprintf(output, "\n");
for(j = 0; j < nv; j++)
{
fprintf(output, " ");
for(k = 0; k < NX; k++)
fprintf(output, " H[%i][%i] = %f,", j, k, H[k + j * NX]);
fprintf(output, "\n");
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
fprintf(output, " End least-square estimation\n");
fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f, dx5 = %f, dx6 = %f, dx7 = %f\n", dx[0], dx[1], dx[2], dx[3], dx[4], dx[5], dx[6]);
for (j=0;j<NX;j++) x[j]+=dx[j];
fprintf(output, " New state of vector X: X[0] = %f, X[1] = %f, X[2] = %f, X[3] = %f, X[4] = %f, X[5] = %f, X[6] = %f\n", x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
fprintf(output, " norm(dx) = %f\n", norm(dx, NX));
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
fprintf(output, " End of coordinates calculation\n");
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
