From 7b06fdaf5fee7ed19f897f9c12a9c2333fff8a44 Mon Sep 17 00:00:00 2001 From: Zhiqiang_Yang98 <565093967@qq.com> Date: Sun, 2 Apr 2023 22:14:18 +0800 Subject: [PATCH] =?UTF-8?q?1.=20=E8=BF=9B=E8=A1=8C=E5=86=85=E5=AD=98?= =?UTF-8?q?=E7=AE=A1=E7=90=86=EF=BC=8C=E5=88=86=E9=85=8D=E5=A4=AA=E5=A4=9A?= =?UTF-8?q?=E6=97=A0=E7=94=A8=E5=86=85=E5=AD=98=E4=B8=94=E6=B2=A1=E6=9C=89?= =?UTF-8?q?=E9=87=8A=E6=94=BE=E3=80=82=E9=87=8D=E6=96=B0=E6=95=B4=E7=90=86?= =?UTF-8?q?rtkinit=E5=87=BD=E6=95=B0=EF=BC=8C=E5=8F=AA=E6=9C=89opt-mode=3D?= =?UTF-8?q?=3DPMODE=5FINS=5FTGNSS=E6=97=B6=E5=80=99=E6=89=8D=E5=88=86?= =?UTF-8?q?=E9=85=8D=E7=B4=A7=E7=BB=84=E5=90=88=E5=86=85=E5=AD=98=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TC_NAV_Zjut_post/Src/pntpos.c | 30 ++++++++++++----------- TC_NAV_Zjut_post/Src/real_time/rt_tcsvr.c | 2 +- TC_NAV_Zjut_post/Src/rtkpos.c | 2 +- TC_NAV_Zjut_post/config/Tc.conf | 20 +++++++-------- 4 files changed, 28 insertions(+), 26 deletions(-) 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)