diff --git a/TC_NAV_Zjut_post/Src/pntpos.c b/TC_NAV_Zjut_post/Src/pntpos.c index 11f7e0a..772346e 100644 --- a/TC_NAV_Zjut_post/Src/pntpos.c +++ b/TC_NAV_Zjut_post/Src/pntpos.c @@ -850,20 +850,21 @@ static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts double x[4]={0},dx[4],Q[16],*v,*H; double err=opt->err[4]; /* Doppler error (Hz) */ int i,j,nv; - - v=mat(n,1); H=mat(4,n); - + + v=mat(n,1); + H=mat(4,n); + for (i=0;irr,x,azel,vsat,err,v,H))<4) { break; } /* least square estimation */ if (lsq(H,v,4,nv,dx,Q)) break; - + for (j=0;j<4;j++) x[j]+=dx[j]; - + if (norm(dx,4)<1E-6) { trace(3,"estvel : vx=%.3f vy=%.3f vz=%.3f, n=%d\n",x[0],x[1],x[2],n); matcpy(sol->rr+3,x,3,1); /* velocity */ @@ -877,7 +878,8 @@ static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts break; } } - free(v); free(H); + free(v); + free(H); } /* single-point positioning ---------------------------------------------------- * compute receiver position, velocity, clock bias by single-point positioning @@ -976,13 +978,13 @@ static int valins(const double *azel, const int *vsat, int n,const prcopt_t *opt nbg=3; ibg=12; /* check estimated states */ - if (norm(x,3)>15.0*D2R||(nba?norm(x+iba,3)>1E4*Mg2M:false) - ||(nbg?norm(x+ibg,3)>5.0*D2R:false)) { - /* 输出 状态 */ - trace(2,"%lf,%lf,%lf\n",norm(x,3),norm(x+iba,3),norm(x+ibg,3)); - trace(2,"too large estimated state error\n"); - return 0; - } +// if (norm(x,3)>15.0*D2R||(nba?norm(x+iba,3)>1E4*Mg2M:false) +// ||(nbg?norm(x+ibg,3)>5.0*D2R:false)) { +// /* 输出 状态 */ +// trace(2,"%lf,%lf,%lf\n",norm(x,3),norm(x+iba,3),norm(x+ibg,3)); +// trace(2,"too large estimated state error\n"); +// return 0; +// } /* post-fit residual test */ for (i=0;iopt.insopt.gopt =&rtk->opt; /* ins */ - if(opt->mode<=PMODE_INS_TGNSS){ + if(opt->mode==PMODE_INS_TGNSS){ init_tc(&rtk->opt.insopt,&rtk->ins,opt); rtk->ins.rtkp=rtk; } diff --git a/TC_NAV_Zjut_post/config/Tc.conf b/TC_NAV_Zjut_post/config/Tc.conf index 9ed2ca5..a4fc07c 100644 --- a/TC_NAV_Zjut_post/config/Tc.conf +++ b/TC_NAV_Zjut_post/config/Tc.conf @@ -1,13 +1,13 @@ # RTKNAVI options (2022/06/21 05:24:33, v.demo5) pos1-posmode =tc # (0:single,1:dgps,2:kinematic,3:static,4:static-start,5:movingbase,6:fixed,7:ppp-kine,8:ppp-static,9:ppp-fixed,10:ins-mech,11:tc) -pos1-frequency =l1+l2+l5 # (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6) +pos1-frequency =l1 # (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6) pos1-soltype =forward # (0:forward,1:backward,2:combined,3:combined-nophasereset) pos1-elmask =5 # (deg) pos1-snrmask_r =on # (0:off,1:on) pos1-snrmask_b =off # (0:off,1:on) -pos1-snrmask_L1 =37,37,37,35,35,35,35,35,35 -pos1-snrmask_L2 =37,37,37,35,35,35,35,35,35 -pos1-snrmask_L5 =37,37,37,35,35,35,35,35,35 +pos1-snrmask_L1 =30,30,30,30,30,30,30,30,30 +pos1-snrmask_L2 =30,30,30,30,30,30,30,30,30 +pos1-snrmask_L5 =30,30,30,30,30,30,30,30,30 pos1-dynamics =off # (0:off,1:on) pos1-tidecorr =off # (0:off,1:on,2:otl) pos1-ionoopt =brdc # (0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec,5:ionex-tec,6:qzs-brdc) @@ -130,21 +130,21 @@ file-tracefile = inpstr1-type =serial # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http) # inpstr2-type =ntripcli # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http) inpstr2-type =off # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http) -inpstr3-type =off # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http) -inpstr1-path =ttyUSB0:460800:8:n:1:off +inpstr3-type =serial # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http) +inpstr1-path =ttyACM2:460800:8:n:1:off # inpstr2-path =caxm002:m2ywj94n@sdk.pnt.10086.cn:8002/RTCM33_GRCEJ inpstr2-path = -inpstr3-path = +inpstr3-path =ttyACM0:921600:8:n:1:off inpstr1-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) inpstr2-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) -inpstr3-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) +inpstr3-format =imu # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) inpstr2-nmeareq =single # (0:off,1:latlon,2:single) inpstr2-nmealat =30.23469717 # (deg) inpstr2-nmealon =120.04264567 # (deg) outstr1-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,5:ntripsvr,9:ntripcas) outstr2-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,5:ntripsvr,9:ntripcas) -outstr1-path =/Users/wakanda_shaw/Desktop/TC_Nav_Zjut/TC_NAV_Zjut_post/data/630_3/llh.pos -outstr2-path =/Users/wakanda_shaw/Desktop/TC_Nav_Zjut/TC_NAV_Zjut_post/data/630_3/nmea.log +outstr1-path =../data/630_3/llh.pos +outstr2-path =../data/630_3/nmea.log outstr1-format =llh # (0:llh,1:xyz,2:enu,3:nmea) outstr2-format =nmea # (0:llh,1:xyz,2:enu,3:nmea) logstr1-type =off # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,5:ntripsvr,9:ntripcas)