/* process positioning -------------------------------------------------------*/ static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt, int mode) { gtime_t time={0}; sol_t sol={{0}}; rtk_t rtk; obsd_t obs[MAXOBS]; double rb[3]={0}; int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6}; trace(3,"procpos : mode=%d\n",mode); solstatic=sopt->solstatic&& (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC); rtkinit(&rtk,popt); rtcm_path[0]='\0'; while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) { /* exclude satellites */ for (i=n=0;i<nobs;i++) { if ((satsys(obs[i].sat,NULL)&popt->navsys)&& popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i]; } if (n<=0) continue; if (!rtkpos(&rtk,obs,n,&navs)) continue; if (mode==0) { /* forward/backward */ if (!solstatic) { outsol(fp,&rtk.sol,rtk.rb,sopt); } else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) { sol=rtk.sol; for (i=0;i<3;i++) rb[i]=rtk.rb[i]; if (time.time==0||timediff(rtk.sol.time,time)<0.0) { time=rtk.sol.time; } } } else if (!revs) { /* combined-forward */ if (isolf>=nepoch) return; solf[isolf]=rtk.sol; for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i]; isolf++; } else { /* combined-backward */ if (isolb>=nepoch) return; solb[isolb]=rtk.sol; for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i]; isolb++; } } if (mode==0&&solstatic&&time.time!=0.0) { sol.time=time; outsol(fp,&sol,rb,sopt); } rtkfree(&rtk); }
/* combine forward/backward solutions and output results ---------------------*/ static void combres(FILE *fp, const prcopt_t *popt, const solopt_t *sopt) { gtime_t time={0}; sol_t sols={{0}},sol={{0}}; double tt,Qf[9],Qb[9],Qs[9],rbs[3]={0},rb[3]={0}; int i,j,k,solstatic,pri[]={0,1,2,3,4,5,1,6}; trace(3,"combres : isolf=%d isolb=%d\n",isolf,isolb); solstatic=sopt->solstatic&& (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC); for (i=0,j=isolb-1;i<isolf&&j>=0;i++,j--) { if ((tt=timediff(solf[i].time,solb[j].time))<-DTTOL) { sols=solf[i]; for (k=0;k<3;k++) rbs[k]=rbf[k+i*3]; j++; } else if (tt>DTTOL) { sols=solb[j]; for (k=0;k<3;k++) rbs[k]=rbb[k+j*3]; i--; } else if (solf[i].stat<solb[j].stat) { sols=solf[i]; for (k=0;k<3;k++) rbs[k]=rbf[k+i*3]; } else if (solf[i].stat>solb[j].stat) { sols=solb[j]; for (k=0;k<3;k++) rbs[k]=rbb[k+j*3]; } else { sols=solf[i]; sols.time=timeadd(sols.time,-tt/2.0); if ((popt->mode==PMODE_KINEMA||popt->mode==PMODE_MOVEB)&& sols.stat==SOLQ_FIX) { /* degrade fix to float if validation failed */ if (!valcomb(solf+i,solb+j)) sols.stat=SOLQ_FLOAT; } for (k=0;k<3;k++) { Qf[k+k*3]=solf[i].qr[k]; Qb[k+k*3]=solb[j].qr[k]; } Qf[1]=Qf[3]=solf[i].qr[3]; Qf[5]=Qf[7]=solf[i].qr[4]; Qf[2]=Qf[6]=solf[i].qr[5]; Qb[1]=Qb[3]=solb[j].qr[3]; Qb[5]=Qb[7]=solb[j].qr[4]; Qb[2]=Qb[6]=solb[j].qr[5]; if (smoother(solf[i].rr,Qf,solb[j].rr,Qb,3,sols.rr,Qs)) continue; sols.qr[0]=(float)Qs[0]; sols.qr[1]=(float)Qs[4]; sols.qr[2]=(float)Qs[8]; sols.qr[3]=(float)Qs[1]; sols.qr[4]=(float)Qs[5]; sols.qr[5]=(float)Qs[2]; } if (!solstatic) { outsol(fp,&sols,rbs,sopt); } else if (time.time==0||pri[sols.stat]<=pri[sol.stat]) { sol=sols; for (k=0;k<3;k++) rb[k]=rbs[k]; if (time.time==0||timediff(sols.time,time)<0.0) { time=sols.time; } } } if (solstatic&&time.time!=0.0) { sol.time=time; outsol(fp,&sol,rb,sopt); } }
/* input obs data, navigation messages and sbas correction -------------------*/ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) { gtime_t time={0}; int i,nu,nr,n=0; char str[32]; trace(3,"infunc : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs); if (0<=iobsu&&iobsu<obss.n) { settime((time=obss.data[iobsu].time)); if (checkbrk("processing : %s Q=%d",time_str(time,0),solq)) { aborts=1; showmsg("aborted"); return -1; } } if (!revs) { /* input forward data */ if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break; } else { for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break; } nr=nextobsf(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr+i]; iobsu+=nu; /* update sbas corrections */ while (isbs<sbss.n) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)>-1.0-DTTOL) break; isbs++; } /* update lex corrections */ while (ilex<lexs.n) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)>-1.0-DTTOL) break; } ilex++; } /* update rtcm corrections */ if (fp_rtcm) { if (rtcm.time.time==0) rtcm.time=obs[0].time; #if 1 while (timediff(rtcm.time,obs[0].time)<-1.0) { #else while (timediff(rtcm.time,obs[0].time)<5.0) { #endif if (input_rtcm3f(&rtcm,fp_rtcm)==-2) break; } for (i=0;i<MAXSAT;i++) navs.ssr[i]=rtcm.ssr[i]; } } else { /* input backward data */ if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)<DTTOL) break; } else { for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break; } nr=nextobsb(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu-nu+1+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr-nr+1+i]; iobsu-=nu; /* update sbas corrections */ while (isbs>=0) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)<1.0+DTTOL) break; isbs--; } /* update lex corrections */ while (ilex>=0) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)<1.0+DTTOL) break; } ilex--; } } return n; } /* process positioning -------------------------------------------------------*/ static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt, int mode) { gtime_t time={0}; sol_t sol={{0}}; rtk_t rtk; obsd_t obs[MAXOBS]; double rb[3]={0}; int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6}; trace(3,"procpos : mode=%d\n",mode); solstatic=sopt->solstatic&& (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC); rtkinit(&rtk,popt); while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) { /* exclude satellites */ for (i=n=0;i<nobs;i++) { if ((satsys(obs[i].sat,NULL)&popt->navsys)&& popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i]; } if (n<=0) continue; if (!rtkpos(&rtk,obs,n,&navs)) continue; if (mode==0) { /* forward/backward */ if (!solstatic) { outsol(fp,&rtk.sol,rtk.rb,sopt); } else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) { sol=rtk.sol; for (i=0;i<3;i++) rb[i]=rtk.rb[i]; if (time.time==0||timediff(rtk.sol.time,time)<0.0) { time=rtk.sol.time; } } } else if (!revs) { /* combined-forward */ if (isolf>=nepoch) return; solf[isolf]=rtk.sol; for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i]; isolf++; } else { /* combined-backward */ if (isolb>=nepoch) return; solb[isolb]=rtk.sol; for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i]; isolb++; } } if (mode==0&&solstatic&&time.time!=0.0) { sol.time=time; outsol(fp,&sol,rb,sopt); } rtkfree(&rtk); }
int main()//OPTIMIZATION LEVEL = 0 { HAL_Init(); SystemClockConfig(); ConfigLED(); ConfigTimer(); rtksvrstart(&svr); ConfigUART(svr.format[0]); fobs[0]=fobs[1]=0; //svr.raw[1].time.time = 1429540822;//test SS2 data //svr.raw[1].time.time = 1429539852;//test SS2 data while (HAL_UART_Receive_DMA(&UartGPSHandle,svr.buff[0],MAX_RAW_LEN) != HAL_OK); while (HAL_UART_Receive_DMA(&UartRFHandle,svr.buff[1],MAX_RAW_LEN) != HAL_OK); HAL_Delay(3000); sendRequest(svr.format[0]); // test(); while(1) { #ifndef _TEST_RESULT if (flagTimeout) { int index,temp; flagTimeout=0; //SendIntStr(UartGPSHandle.Instance->SR); //SendIntStr(UartRFHandle.Instance->SR); for (index=0;index<2;index++) { if (index==0) temp = UartGPSHandle.hdmarx->Instance->NDTR & 0xffff; else temp = UartRFHandle.hdmarx->Instance->NDTR & 0xffff; if (temp + svr.buffPtr[index] <= MAX_RAW_LEN) svr.nb[index] = MAX_RAW_LEN - svr.buffPtr[index] - temp; else svr.nb[index] = 2*MAX_RAW_LEN - temp - svr.buffPtr[index]; fobs[index] = decode_raw(&svr,index); svr.buffPtr[index] = MAX_RAW_LEN - temp; } // temp = UartGPSHandle.hdmarx->Instance->NDTR & 0xffff; // if (temp + svr.buffPtr[0] <= MAX_RAW_LEN) // svr.nb[0] = MAX_RAW_LEN - svr.buffPtr[0] - temp; // else // svr.nb[0] = 2*MAX_RAW_LEN - temp - svr.buffPtr[0]; // if (svr.buffPtr[0] + svr.nb[0] <= MAX_RAW_LEN) // { // for (i = svr.buff[0] + svr.buffPtr[0] ; // i < svr.buff[0] + svr.buffPtr[0] + svr.nb[0]; i++) // { // HAL_UART_Transmit(&UartResultHandle,i,1,1); // } // } // else // { // for (i = svr.buff[0] + svr.buffPtr[0] ; // i < svr.buff[0] + MAX_RAW_LEN; i++) // { // HAL_UART_Transmit(&UartResultHandle,i,1,1); // } // for (i = svr.buff[0] ; // i < svr.buff[0] + svr.nb[0] + svr.buffPtr[0] - MAX_RAW_LEN ; i++) // { // HAL_UART_Transmit(&UartResultHandle,i,1,1); // } // } // svr.buffPtr[0] = MAX_RAW_LEN - temp; //rtk positioning********************************************************************** // if (0) if (fobs[1]) { fobs[1]=0; LED4_TOGGLE; } if (fobs[0]) { int i; fobs[0]=0; LED3_TOGGLE; #ifdef TIME_MEASURE start=HAL_GetTick(); #endif temp=svr.obs[0].n; for (i=0;i<temp;i++) { obsd[i]=svr.obs[0].data[i]; } for (i=0;(i<svr.obs[1].n)&&(i+temp<MAX_OBS);i++) { obsd[i+temp]=svr.obs[1].data[i]; } if (!rtkpos(&svr.rtk,obsd,i+temp,&svr.nav)) // if (1) { LED5_TOGGLE; #ifdef TIME_MEASURE t=HAL_GetTick()-start; svr.rtk.sol.processTime = t; #endif if (svr.rtk.sol.stat==SOLQ_FIX) LED6_TOGGLE; outsol(&svr.rtk.sol,svr.rtk.rb); SendStr(svr.rtk.sol.result); } else { HAL_UART_Transmit_DMA(&UartResultHandle,(unsigned char*)svr.rtk.errbuf,svr.rtk.errLen); } } } #else if (flagTimeout) { static int i; char* res = svr.rtk.sol.result; flagTimeout = 0; res+=sprintf(res, "%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%06.3f %14.4f %14.4f %14.4f %3d %3d %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %6.2f %6.1f", 2015.0,10.0,12.0,3.0,45.0,18.0,//time yy/mm/dd hh:mm:ss.ssss 1.0,2.0,1.0, 1,1, 1.0,1.0,1.0, 1.0,1.0,1.0, 1.0,1.0); res+=sprintf(res," %4d",i++); res[0]='\n'; SendStr(svr.rtk.sol.result); } #endif } }