Wakanda_shaw/ZJUT_Nav
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

添加real time 代码

Browse Source
This commit is contained in:
ZhiQiang_Yang98 2023-03-25 14:08:48 +08:00
parent 649bf45136
commit be4602bc23
17 changed files with 1557 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TC_NAV_Zjut_post/CMakeLists.txt
View File

@ -15,7 +15,7 @@ if(WIN32)
link_libraries(winmm.lib) link_libraries(winmm.lib)
endif() endif()
add_executable(TC_NAV_Zjut main.c ${SRC_FILES} include/rtklib.h Src/real_time/ins.c Src/decode_IMU.c Src/real_time/rt_init.c Src/real_time/rt_ins-gnss.c Src/real_time/rt_doppler.c Src/real_time/nhc.c Src/post/ps_tcpos.c Src/post/insinit.c Src/real_time/rt_zvu.c Src/real_time/rt_zaru.c) add_executable(TC_NAV_Zjut main.c ${SRC_FILES} include/rtklib.h Src/real_time/ins.c Src/decode_IMU.c Src/real_time/rt_init.c Src/real_time/rt_ins-gnss.c Src/real_time/rt_doppler.c Src/real_time/nhc.c Src/post/ps_tcpos.c Src/post/insinit.c Src/real_time/rt_zvu.c Src/real_time/rt_zaru.c Src/real_time/client.c Src/real_time/imu.c Src/real_time/rt_tcfilt.c Src/real_time/rt_tcpos.c Src/real_time/rt_tcsvr.c)
# if in linux # if in linux
find_package(Threads REQUIRED) find_package(Threads REQUIRED)
set(CMAKE_THREAD_PREFER_PTHREAD) set(CMAKE_THREAD_PREFER_PTHREAD)

64
TC_NAV_Zjut_post/Src/real_time/client.c Normal file
View File

@ -0,0 +1,64 @@
//
// Created by wakashaw on 23-3-17.
//
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<errno.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include "rtklib.h"
/* open client and connect to server ------------------------------------------*/
extern int client_open(const rtksvr_t *svr){
struct sockaddr_in servaddr;
int n;
static int sockfd;
static int first=1;
char recvline[4096], sendline[4096];
double llh[3];
// const char * address="127.0.0.1";
const char * address="81.69.233.50";
if(first){
if( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0){
printf("create socket error: %s(errno: %d)\n", strerror(errno),errno);
exit(0);
}
if( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0){
printf("create socket error: %s(errno: %d)\n", strerror(errno),errno);
exit(0);
}
memset(&servaddr, 0 ,sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_port = htons(8001);
if( inet_pton(AF_INET, address, &servaddr.sin_addr) <= 0){
printf("inet_pton error for %s\n",address);
return 0;
}
if( connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) < 0){
printf("connect error: %s(errno: %d)\n",strerror(errno),errno);
return 0;
}
}
first=0;
// printf("send msg to server: \n");
// fgets(sendline, 4096, stdin);
ecef2pos(svr->rtk.sol.rr,llh);
// printf("%3.12lf,%3.12lf\n",llh[0]*R2D,llh[1]*R2D);
sprintf(sendline,"%3.12lf,%3.12lf\n",llh[0]*R2D,llh[1]*R2D);
if( send(sockfd, sendline, strlen(sendline), 0) < 0)
{
printf("send msg error: %s(errno: %d)\n", strerror(errno), errno);
return 0;
}
// close(sockfd);
return 1;
}

50
TC_NAV_Zjut_post/Src/real_time/imu.c Normal file
View File

@ -0,0 +1,50 @@
//
// Created by wakashaw on 23-3-17.
//
#include "rtklib.h"
/* input imu stream------------------------------------------------
* input sample : $GYOACC,STM,,-0.07,-1.28,9.76,0.49,-1.05,0.21\r\n
*
*-----------------------------------------------------------------*/
extern int input_imu(raw_t *raw,imud_t *imud,uint8_t data){
/* synchronize frame */
if(raw->nbyte==0){
if(data!='$') return 0;
raw->buff[raw->nbyte++]=data;
return 0;
}
/* if not symbol '\r' keep reading data */
if(data!='\r') {
raw->buff[raw->nbyte++]=data;
return 0;
}
raw->nbyte=0;
return decode_imu(raw->buff,imud);
}
/* decode buff imu data ------------------------
* args : uint8_t *buff I imu data buffer
* imud_t *imud O output imu struct
* return 11: imu output type
*-----------------------------------------------*/
extern int decode_imu(uint8_t *buff,imud_t *imud){
char *str = (char*)buff;
int week;
/* raw data is right-forward-up need transfer to forward-right-down */
sscanf(str,"$GYOACC,TDK,%lf,%lf,%lf,%lf,%lf,%lf,%lf",&imud->time,&imud->accl[1],&imud->accl[0],&imud->accl[2],&imud->gyro[1],&imud->gyro[0],&imud->gyro[2]);
imud->accl[2]=imud->accl[2]*-1.0;
imud->gyro[2]=imud->gyro[2]*-1.0;
// printf("%lf,%lf,%lf,%lf,%lf,%lf\n",imud->accl[0],imud->accl[1],imud->accl[2],imud->gyro[0],imud->gyro[1],imud->gyro[2]);
/* get time of week */
time2gpst(utc2gpst(timeget()),&week);
/* trans unix time to tow */
imud->time=imud->time-315964800.0 - week*7*24*3600;
imud->syn_time = gpst2time(week,imud->time);
return 11;
}

213
TC_NAV_Zjut_post/Src/real_time/rt_tcfilt.c Normal file
View File

@ -0,0 +1,213 @@
//
// Created by wakashaw on 23-3-17.
//
#include "rtklib.h"
#define MAXDT 60.0 /* max interval to update imu (s) */
/* init flag */
static int init=0;
/* coarse align flag */
static int coarse_init_flag = 0;
/* static detect 3s*/
static imud_t imuz[IMUBUFF_SIZE];
/* numebr of static imu data */
static int i_imu=0;
/* buffer flag */
static int full=0;
/* save imu data to buffer for static check */
static void loadimudata(imud_t *imud)
{
imuz[i_imu++]=*imud;
if(i_imu==IMUBUFF_SIZE){
full = 1;
i_imu = 0;
}
}
/* save output buffer --------------------------------------------------------*/
static void saveoutbuf(rtksvr_t *svr, uint8_t *buff, int n, int index)
{
rtksvrlock(svr);
n=n<svr->buffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index];
memcpy(svr->sbuf[index]+svr->nsb[index],buff,n);
svr->nsb[index]+=n;
rtksvrunlock(svr);
}
/* carrier-phase bias (fcb) correction ---------------------------------------*/
static void corr_phase_bias(obsd_t *obs, int n, const nav_t *nav)
{
double freq;
uint8_t code;
int i,j;
for (i=0;i<n;i++) for (j=0;j<NFREQ;j++) {
code=obs[i].code[j];
if ((freq=sat2freq(obs[i].sat,code,nav))==0.0) continue;
/* correct phase bias (cyc) */
obs[i].L[j]-=nav->ssr[obs[i].sat-1].pbias[code-1]*freq/CLIGHT;
}
}
/* write solution to output stream -------------------------------------------*/
static void writesol(rtksvr_t *svr, int index)
{
solopt_t solopt=solopt_default;
uint8_t buff[MAXSOLMSG+1];
int i,n;
tracet(4,"writesol: index=%d\n",index);
for (i=0;i<2;i++) {
if (svr->solopt[i].posf==SOLF_STAT) {
/* output solution status */
rtksvrlock(svr);
n=rtkoutstat(&svr->rtk,(char *)buff);
rtksvrunlock(svr);
}
else {
/* output solution */
n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i);
}
strwrite(svr->stream+i+3,buff,n);
/* save output buffer */
saveoutbuf(svr,buff,n,i);
/* output extended solution */
n=outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i);
strwrite(svr->stream+i+3,buff,n);
/* save output buffer */
saveoutbuf(svr,buff,n,i);
}
/* output solution to monitor port */
if (svr->moni) {
n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt);
strwrite(svr->moni,buff,n);
}
/* save solution buffer */
if (svr->nsol<MAXSOLBUF) {
rtksvrlock(svr);
svr->solbuf[svr->nsol++]=svr->rtk.sol;
rtksvrunlock(svr);
}
}
/* tightly coupled main func -----------------------------------
* args:
* rtksvr_t *svr
* int fobs obs data flag
* uint32_t tick tick time
* -------------------------------------------------------------*/
extern void rt_tcfilter(rtksvr_t *svr, int fobs, uint32_t tick){
int i,j,zf;
obs_t obs;
obsd_t data[MAXOBS*2];
double tt,pos[3];
obs.data=data;
int n_imu = svr->n_imu; // number of imu data
imud_t imud;
static gtime_t gt;
/* load imu data */
if(n_imu){
for(i=0;i<3;i++){
imud.gyro[i]=0.0;
imud.accl[i]=0.0;
for(j=0;j<n_imu;j++){
imud.gyro[i]+=svr->imud[j].gyro[i];
imud.accl[i]+=svr->imud[j].accl[i];
}
imud.gyro[i]/=n_imu;
imud.accl[i]/=n_imu;
}
imud.time=svr->imud[n_imu-1].time;
imud.syn_time = svr->imud[n_imu-1].syn_time;
// printf("%lf\n", timediff(imud.syn_time,gt));
gt = imud.syn_time;
}
else{
imud.syn_time = timeadd(imud.syn_time,0.01);
}
/* imu buffer for static test */
loadimudata(&imud);
/* if obs data */
if(fobs){
for (i=0;i<fobs;i++) { /* for each rover observation data */
obs.n=0;
for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) {
obs.data[obs.n++]=svr->obs[0][i].data[j];
}
/* carrier phase bias correction */
if (!strstr(svr->rtk.opt.pppopt,"-DIS_FCB")) {
corr_phase_bias(obs.data,obs.n,&svr->nav);
}
/* initial ins state */
if(init==0){
if(rt_init_ins(obs.data,svr,obs.n,imuz,i_imu,full,&svr->rtk.opt)){
init=1;
return;
}
else return;
}
/* tightly coupled */
rtksvrlock(svr);
tcigpos(&svr->rtk.opt,obs.data,obs.n,&svr->nav,&imud,n_imu,&svr->rtk,&svr->rtk.ins,INSUPD_MEAS);
rtksvrunlock(svr);
/* write solution */
if (svr->rtk.sol.stat!=SOLQ_NONE) {
/* adjust current time */
tt=(int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt)));
/* solution. */
ins2sol(&svr->rtk.ins,&svr->rtk.opt.insopt,&svr->rtk.sol);
/* write solution */
writesol(svr,i);
#ifdef CLIENT
/* plot */
if(!client_open(svr)){
printf("connect to server fail\n");
return;
}
#endif
}
/* if cpu overload, inclement obs outage counter and break */
if ((int)(tickget()-tick)>=svr->cycle) {
svr->prcout+=fobs-i-1;
}
}
}
else{
/* ins still initialing */
if(init==0) return;
/* ins mechanization */
rtksvrlock(svr);
tcigpos(&svr->rtk.opt,obs.data,obs.n,&svr->nav,&imud,n_imu,&svr->rtk,&svr->rtk.ins,INSUPD_TIME);
rtksvrunlock(svr);
}
/* non-holonomic constraint */
// nhc(&svr->rtk.ins,&svr->rtk.opt.insopt,&imud);
/* zero velocity/zero angular rate update */
// ecef2pos(&svr->rtk.ins.position,pos);
// zf=detstatic_GLRT(imuz,IMUBUFF_SIZE,&svr->rtk.opt.insopt,pos);
// if(zf){
// trace(3,"detect static\n");
// /* zero velocity update */
// zvu(&svr->rtk.ins,&svr->rtk.opt.insopt,imuz,1);
// /* zero angular rate update */
// zaru(&svr->rtk.ins,&svr->rtk.opt.insopt,imuz,1);
// }
}

203
TC_NAV_Zjut_post/Src/real_time/rt_tcpos.c Normal file
View File

@ -0,0 +1,203 @@
//
// Created by wakashaw on 23-3-17.
//
#include <stdlib.h>
#include "rtklib.h"
#define SQRT(x) ((x) <= 0.0 || (x) != (x) ? 0.0 : sqrt(x))
#define PRGNAME "rtkrcv" /* program name */
#define CMDPROMPT "rtkrcv> " /* command prompt */
#define MAXCON 32 /* max number of consoles */
#define MAXARG 10 /* max number of args in a command */
#define MAXCMD 256 /* max length of a command */
#define MAXSTR 1024 /* max length of a stream */
#define OPTSDIR "." /* default config directory */
#define OPTSFILE "rtkrcv.conf" /* default config file */
#define NAVIFILE "rtkrcv.nav" /* navigation save file */
#define STATFILE "rtkrcv_%Y%m%d%h%M.stat" /* solution status file */
#define TRACEFILE "rtkrcv_%Y%m%d%h%M.trace" /* debug trace file */
#define INTKEEPALIVE 1000 /* keep alive interval (ms) */
/* global variables ----------------------------------------------------------*/
static rtksvr_t svr; /* rtk server struct */
static stream_t moni; /* monitor stream */
static prcopt_t prcopt; /* processing options */
static solopt_t solopt[2] = {{0}}; /* solution options */
static filopt_t filopt = {""}; /* file options */
static int intflg = 0; /* interrupt flag (2:shtdown) */
static int strtype[] = {/* stream types */
STR_SERIAL, STR_NONE, STR_NONE, STR_FILE, STR_FILE, STR_NONE, STR_NONE, STR_NONE};
static char strpath[8][MAXSTR] = {"", "", "", "", "", "", "", ""}; /* stream paths */
static int strfmt[] = { /* stream formats */
STR_SERIAL, STR_NONE, STR_NONE, SOLF_LLH, SOLF_NMEA};
static int svrcycle = 10; /* server cycle (ms) */
static int timeout = 10000; /* timeout time (ms) */
static int reconnect = 10000; /* reconnect interval (ms) */
static int nmeacycle = 5000; /* nmea request cycle (ms) */
static int buffsize = 32768; /* input buffer size (bytes) */
static int navmsgsel = 0; /* navigation mesaage select */
static char proxyaddr[256] = ""; /* http/ntrip proxy */
static int nmeareq = 0; /* nmea request type (0:off,1:lat/lon,2:single) */
static double nmeapos[] = {0, 0, 0}; /* nmea position (lat/lon/height) (deg,m) */
static int modflgr[256] = {0}; /* modified flags of receiver options */
static int modflgs[256] = {0}; /* modified flags of system options */
static int moniport = 52002; /* monitor port */
static int keepalive = 0; /* keep alive flag */
static int start = 0; /* auto start */
static int fswapmargin = 30; /* file swap margin (s) */
static char sta_name[256] = ""; /* station name */
/* receiver options table ----------------------------------------------------*/
#define TIMOPT "0:gpst,1:utc,2:jst,3:tow"
#define CONOPT "0:dms,1:deg,2:xyz,3:enu,4:pyl"
#define FLGOPT "0:off,1:std+2:age/ratio/ns"
#define ISTOPT "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripcli,7:ftp,8:http"
#define OSTOPT "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,5:ntripsvr,9:ntripcas"
#define FMTOPT "0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,14,15:sp3,19:imu"
#define NMEOPT "0:off,1:latlon,2:single"
#define SOLOPT "0:llh,1:xyz,2:enu,3:nmea,4:stat"
#define MSGOPT "0:all,1:rover,2:base,3:corr"
static opt_t rcvopts[] =
{
{"inpstr1-type", 3, (void *)&strtype[0], ISTOPT},
{"inpstr2-type", 3, (void *)&strtype[1], ISTOPT},
{"inpstr3-type", 3, (void *)&strtype[2], ISTOPT},
{"inpstr1-path", 2, (void *)strpath[0], ""},
{"inpstr2-path", 2, (void *)strpath[1], ""},
{"inpstr3-path", 2, (void *)strpath[2], ""},
{"inpstr1-format", 3, (void *)&strfmt[0], FMTOPT},
{"inpstr2-format", 3, (void *)&strfmt[1], FMTOPT},
{"inpstr3-format", 3, (void *)&strfmt[2], FMTOPT},
{"inpstr2-nmeareq", 3, (void *)&nmeareq, NMEOPT},
{"inpstr2-nmealat", 1, (void *)&nmeapos[0], "deg"},
{"inpstr2-nmealon", 1, (void *)&nmeapos[1], "deg"},
{"inpstr2-nmeahgt", 1, (void *)&nmeapos[2], "m"},
{"outstr1-type", 3, (void *)&strtype[3], OSTOPT},
{"outstr2-type", 3, (void *)&strtype[4], OSTOPT},
{"outstr1-path", 2, (void *)strpath[3], ""},
{"outstr2-path", 2, (void *)strpath[4], ""},
{"outstr1-format", 3, (void *)&strfmt[3], SOLOPT},
{"outstr2-format", 3, (void *)&strfmt[4], SOLOPT},
{"logstr1-type", 3, (void *)&strtype[5], OSTOPT},
{"logstr2-type", 3, (void *)&strtype[6], OSTOPT},
{"logstr3-type", 3, (void *)&strtype[7], OSTOPT},
{"logstr1-path", 2, (void *)strpath[5], ""},
{"logstr2-path", 2, (void *)strpath[6], ""},
{"logstr3-path", 2, (void *)strpath[7], ""},
{"misc-svrcycle", 0, (void *)&svrcycle, "ms"},
{"misc-timeout", 0, (void *)&timeout, "ms"},
{"misc-reconnect", 0, (void *)&reconnect, "ms"},
{"misc-nmeacycle", 0, (void *)&nmeacycle, "ms"},
{"misc-buffsize", 0, (void *)&buffsize, "bytes"},
{"misc-navmsgsel", 3, (void *)&navmsgsel, MSGOPT},
{"misc-proxyaddr", 2, (void *)proxyaddr, ""},
{"misc-fswapmargin", 0, (void *)&fswapmargin, "s"},
{"", 0, NULL, ""}};
#define MAXCON 32 /* max number of consoles */
#define MAXARG 10 /* max number of args in a command */
#define MAXCMD 256 /* max length of a command */
#define MAXSTR 1024 /* max length of a stream */
/* ins-gnss tightly coupled func--------------------------------------------
* args:
* prcopt_t *popt I options
* const char *log I input log file
* return: status (1:ok,0:fall)
*/
extern int tcpos(){
int i, outstat = 1, trace = 3, sock = 0;
char *TRACEFILE_my = "../tracefile/nav.trace";
char *opt_file = "../config/Tc.conf";
static sta_t sta[MAXRCV] = {{""}};
double pos[3] = {0.0, 0.0, 0.0}, npos[3];
char s1[3][MAXRCVCMD] = {"", "", ""}, *cmds[] = {NULL, NULL, NULL};
char s2[3][MAXRCVCMD] = {"", "", ""}, *cmds_periodic[] = {NULL, NULL, NULL};
char *ropts[] = {"", "", ""};
char *paths[] = {
strpath[0], strpath[1], strpath[2], strpath[3], strpath[4], strpath[5],
strpath[6], strpath[7]
};
char errmsg[2048] = "";
int stropt[8] = {0};
if (trace > 0)
{
traceopen(TRACEFILE_my);
tracelevel(trace);
}
/* initialize rtk server */
rtksvrinit(&svr);
// strinit(&moni);
/* load options file */
resetsysopts(); /* 清空配置信息 */
if (!loadopts(opt_file, rcvopts) || !loadopts(opt_file, sysopts)) /* 从文件中读取配置信息 */
{
printf("no options file: %s. defaults used\n", opt_file);
}
getsysopts(&prcopt, solopt, &filopt); /* 指针指向新的配置信息 */
/* open solution file */
rtkopenstat(STATFILE, outstat);
/* 设置输出解算数据的格式信息 */
solopt[0].posf = strfmt[3];
solopt[1].posf = strfmt[4];
/* startsvr */
/* confirm overwrite */
if (prcopt.refpos == 4) /* 4:rtcm pos */
{ /* rtcm */
for (i = 0; i < 3; i++)
prcopt.rb[i] = 0.0;
}
/* 设置需要发送GGA的位置信息 初试位置信息 */
pos[0] = nmeapos[0] * D2R;
pos[1] = nmeapos[1] * D2R;
pos[2] = nmeapos[2];
pos2ecef(pos, npos);
/* 接收器 / 卫星选项的修改标志 */
for (i = 0; *rcvopts[i].name; i++)
modflgr[i] = 0;
for (i = 0; *sysopts[i].name; i++)
modflgs[i] = 0;
/* set stream options */
stropt[0] = timeout;
stropt[1] = reconnect;
stropt[2] = 1000;
stropt[3] = buffsize;
stropt[4] = fswapmargin;
strsetopt(stropt);
/* start rtk server */
if (!tcsvrstart(&svr, svrcycle, buffsize, strtype, paths, strfmt, navmsgsel,
cmds, cmds_periodic, ropts, nmeacycle, nmeareq, npos, &prcopt,
solopt, NULL, errmsg))
return 0;
/* 输出当前配置信息 */
printf("\n解算信息: \n");
prcopt.nf==1?printf("L1\n"):prcopt.nf==2?printf("L1+L2\n"):prcopt.nf==3?printf("L1+L2+L5\n"):prcopt.nf==3?printf("L1+L2+L5+L6\n"):printf("Freq err\n");
prcopt.navsys&0x01?printf("GPS "):NULL;
prcopt.navsys&0x04?printf("GLO "):NULL;
prcopt.navsys&0x08?printf("GAL "):NULL;
prcopt.navsys&0x10?printf("QZSS "):NULL;
prcopt.navsys&0x20?printf("BDS "):NULL;
printf("\n");
traceclose();
/* 等待子线程结束 */
return 0;
}

879
TC_NAV_Zjut_post/Src/real_time/rt_tcsvr.c Normal file
View File

@ -0,0 +1,879 @@
//
// Created by wakashaw on 23-3-17.
//
/*------------------------------------------------------------------------------
* rtksvr.c : rtk server functions
*
* Copyright (C) 2007-2020 by T.TAKASU, All rights reserved.
*
* options : -DWIN32 use WIN32 API
*
* version : $Revision:$ $Date:$
* history : 2009/01/07 1.0 new
* 2009/06/02 1.1 support glonass
* 2010/07/25 1.2 support correction input/log stream
* supoort online change of output/log streams
* supoort monitor stream
* added api:
* rtksvropenstr(),rtksvrclosestr()
* changed api:
* rtksvrstart()
* 2010/08/25 1.3 fix problem of ephemeris time inversion (2.4.0_p6)
* 2010/09/08 1.4 fix problem of ephemeris and ssr squence upset
* (2.4.0_p8)
* 2011/01/10 1.5 change api: rtksvrstart(),rtksvrostat()
* 2011/06/21 1.6 fix ephemeris handover problem
* 2012/05/14 1.7 fix bugs
* 2013/03/28 1.8 fix problem on lack of glonass freq number in raw
* fix problem on ephemeris with inverted toe
* add api rtksvrfree()
* 2014/06/28 1.9 fix probram on ephemeris update of beidou
* 2015/04/29 1.10 fix probram on ssr orbit/clock inconsistency
* 2015/07/31 1.11 add phase bias (fcb) correction
* 2015/12/05 1.12 support opt->pppopt=-DIS_FCB
* 2016/07/01 1.13 support averaging single pos as base position
* 2016/07/31 1.14 fix bug on ion/utc parameters input
* 2016/08/20 1.15 support api change of sendnmea()
* 2016/09/18 1.16 fix server-crash with server-cycle > 1000
* 2016/09/20 1.17 change api rtksvrstart()
* 2016/10/01 1.18 change api rtksvrstart()
* 2016/10/04 1.19 fix problem to send nmea of single solution
* 2016/10/09 1.20 add reset-and-single-sol mode for nmea-request
* 2017/04/11 1.21 add rtkfree() in rtksvrfree()
* 2020/11/30 1.22 add initializing svr->nav in rtksvrinit()
* allocate double size ephemeris in rtksvrinit()
* handle multiple ephemeris sets in updatesvr()
* use API sat2freq() to get carrier frequency
* use integer types in stdint.h
*-----------------------------------------------------------------------------*/
#include "rtklib.h"
#include "signal.h"
#define MIN_INT_RESET 30000 /* mininum interval of reset command (ms) */
static int intflg =0; /* interrupt flag (2:shtdown) */
/* external stop signal ------------------------------------------------------*/
static void sigshut(int sig)
{
trace(3,"sigshut: sig=%d\n",sig);
intflg=1;
}
/* write solution header to output stream ------------------------------------*/
static void writesolhead(stream_t *stream, const solopt_t *solopt)
{
uint8_t buff[1024];
int n;
n=outsolheads(buff,solopt);
strwrite(stream,buff,n);
}
/* save output buffer --------------------------------------------------------*/
static void saveoutbuf(rtksvr_t *svr, uint8_t *buff, int n, int index)
{
rtksvrlock(svr);
n=n<svr->buffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index];
memcpy(svr->sbuf[index]+svr->nsb[index],buff,n);
svr->nsb[index]+=n;
rtksvrunlock(svr);
}
/* write solution to output stream -------------------------------------------*/
static void writesol(rtksvr_t *svr, int index)
{
solopt_t solopt=solopt_default;
uint8_t buff[MAXSOLMSG+1];
int i,n;
tracet(4,"writesol: index=%d\n",index);
for (i=0;i<2;i++) {
if (svr->solopt[i].posf==SOLF_STAT) {
/* output solution status */
rtksvrlock(svr);
n=rtkoutstat(&svr->rtk,(char *)buff);
rtksvrunlock(svr);
}
else {
/* output solution */
n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i);
}
strwrite(svr->stream+i+3,buff,n);
/* save output buffer */
saveoutbuf(svr,buff,n,i);
/* output extended solution */
n=outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i);
strwrite(svr->stream+i+3,buff,n);
/* save output buffer */
saveoutbuf(svr,buff,n,i);
}
/* output solution to monitor port */
if (svr->moni) {
n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt);
strwrite(svr->moni,buff,n);
}
/* save solution buffer */
if (svr->nsol<MAXSOLBUF) {
rtksvrlock(svr);
svr->solbuf[svr->nsol++]=svr->rtk.sol;
rtksvrunlock(svr);
}
}
/* update glonass frequency channel number in raw data struct ----------------*/
static void update_glofcn(rtksvr_t *svr)
{
int i,j,sat,frq;
for (i=0;i<MAXPRNGLO;i++) {
sat=satno(SYS_GLO,i+1);
for (j=0,frq=-999;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat!=sat) continue;
frq=svr->raw[j].nav.geph[i].frq;
}
if (frq<-7||frq>6) continue;
for (j=0;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat==sat) continue;
svr->raw[j].nav.geph[i].sat=sat;
svr->raw[j].nav.geph[i].frq=frq;
}
}
}
/* update observation data ---------------------------------------------------*/
static void update_obs(rtksvr_t *svr, obs_t *obs, int index, int iobs)
{
int i,n=0,sat,sys;
if (iobs<MAXOBSBUF) {
for (i=0;i<obs->n;i++) {
sat=obs->data[i].sat;
sys=satsys(sat,NULL);
if (svr->rtk.opt.exsats[sat-1]==1||!(sys&svr->rtk.opt.navsys)) {
continue;
}
svr->obs[index][iobs].data[n]=obs->data[i];
svr->obs[index][iobs].data[n++].rcv=index+1;
}
svr->obs[index][iobs].n=n;
sortobs(&svr->obs[index][iobs]);
}
svr->nmsg[index][0]++;
}
/* update ephemeris ----------------------------------------------------------*/
static void update_eph(rtksvr_t *svr, nav_t *nav, int ephsat, int ephset,
int index)
{
eph_t *eph1,*eph2,*eph3;
geph_t *geph1,*geph2,*geph3;
int prn;
if (satsys(ephsat,&prn)!=SYS_GLO) {
if (!svr->navsel||svr->navsel==index+1) {
/* svr->nav.eph={current_set1,current_set2,prev_set1,prev_set2} */
eph1=nav->eph+ephsat-1+MAXSAT*ephset; /* received */
eph2=svr->nav.eph+ephsat-1+MAXSAT*ephset; /* current */
eph3=svr->nav.eph+ephsat-1+MAXSAT*(2+ephset); /* previous */
if (eph2->ttr.time==0||
(eph1->iode!=eph3->iode&&eph1->iode!=eph2->iode)||
(timediff(eph1->toe,eph3->toe)!=0.0&&
timediff(eph1->toe,eph2->toe)!=0.0)||
(timediff(eph1->toc,eph3->toc)!=0.0&&
timediff(eph1->toc,eph2->toc)!=0.0)) {
*eph3=*eph2; /* current ->previous */
*eph2=*eph1; /* received->current */
}
}
svr->nmsg[index][1]++;
}
else {
if (!svr->navsel||svr->navsel==index+1) {
geph1=nav->geph+prn-1;
geph2=svr->nav.geph+prn-1;
geph3=svr->nav.geph+prn-1+MAXPRNGLO;
if (geph2->tof.time==0||
(geph1->iode!=geph3->iode&&geph1->iode!=geph2->iode)) {
*geph3=*geph2;
*geph2=*geph1;
update_glofcn(svr);
}
}
svr->nmsg[index][6]++;
}
}
/* update sbas message -------------------------------------------------------*/
static void update_sbs(rtksvr_t *svr, sbsmsg_t *sbsmsg, int index)
{
int i,sbssat=svr->rtk.opt.sbassatsel;
if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) {
sbsmsg->rcv=index+1;
if (svr->nsbs<MAXSBSMSG) {
svr->sbsmsg[svr->nsbs++]=*sbsmsg;
}
else {
for (i=0;i<MAXSBSMSG-1;i++) svr->sbsmsg[i]=svr->sbsmsg[i+1];
svr->sbsmsg[i]=*sbsmsg;
}
sbsupdatecorr(sbsmsg,&svr->nav);
}
svr->nmsg[index][3]++;
}
/* update ion/utc parameters -------------------------------------------------*/
static void update_ionutc(rtksvr_t *svr, nav_t *nav, int index)
{
if (svr->navsel==0||svr->navsel==index+1) {
matcpy(svr->nav.utc_gps,nav->utc_gps,8,1);
matcpy(svr->nav.utc_glo,nav->utc_glo,8,1);
matcpy(svr->nav.utc_gal,nav->utc_gal,8,1);
matcpy(svr->nav.utc_qzs,nav->utc_qzs,8,1);
matcpy(svr->nav.utc_cmp,nav->utc_cmp,8,1);
matcpy(svr->nav.utc_irn,nav->utc_irn,9,1);
matcpy(svr->nav.utc_sbs,nav->utc_sbs,4,1);
matcpy(svr->nav.ion_gps,nav->ion_gps,8,1);
matcpy(svr->nav.ion_gal,nav->ion_gal,4,1);
matcpy(svr->nav.ion_qzs,nav->ion_qzs,8,1);
matcpy(svr->nav.ion_cmp,nav->ion_cmp,8,1);
matcpy(svr->nav.ion_irn,nav->ion_irn,8,1);
}
svr->nmsg[index][2]++;
}
/* update antenna position ---------------------------------------------------*/
static void update_antpos(rtksvr_t *svr, int index)
{
sta_t *sta;
double pos[3],del[3]={0},dr[3];
int i;
if (svr->rtk.opt.refpos==POSOPT_RTCM&&index==1) {
if (svr->format[1]==STRFMT_RTCM2||svr->format[1]==STRFMT_RTCM3) {
sta=&svr->rtcm[1].sta;
}
else {
sta=&svr->raw[1].sta;
}
/* update base station position */
for (i=0;i<3;i++) {
svr->rtk.rb[i]=sta->pos[i];
}
/* antenna delta */
ecef2pos(svr->rtk.rb,pos);
if (sta->deltype) { /* xyz */
del[2]=sta->hgt;
enu2ecef(pos,del,dr);
for (i=0;i<3;i++) {
svr->rtk.rb[i]+=sta->del[i]+dr[i];
}
}
else { /* enu */
enu2ecef(pos,sta->del,dr);
for (i=0;i<3;i++) {
svr->rtk.rb[i]+=dr[i];
}
}
}
svr->nmsg[index][4]++;
}
/* update ssr corrections ----------------------------------------------------*/
static void update_ssr(rtksvr_t *svr, int index)
{
int i,sys,prn,iode;
for (i=0;i<MAXSAT;i++) {
if (!svr->rtcm[index].ssr[i].update) continue;
/* check consistency between iods of orbit and clock */
if (svr->rtcm[index].ssr[i].iod[0]!=svr->rtcm[index].ssr[i].iod[1]) {
continue;
}
svr->rtcm[index].ssr[i].update=0;
iode=svr->rtcm[index].ssr[i].iode;
sys=satsys(i+1,&prn);
/* check corresponding ephemeris exists */
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) {
if (svr->nav.eph[i ].iode!=iode&&
svr->nav.eph[i+MAXSAT].iode!=iode) {
continue;
}
}
else if (sys==SYS_GLO) {
if (svr->nav.geph[prn-1 ].iode!=iode&&
svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) {
continue;
}
}
svr->nav.ssr[i]=svr->rtcm[index].ssr[i];
}
svr->nmsg[index][7]++;
}
/* update imu data -----------------------------------------------------------*/
static void updata_imu(rtksvr_t *svr, imud_t imud){
svr->imud[svr->n_imu++] = imud;
// printf("%lf\n",svr->imud[svr->n_imu].time);
// printf("%d\n",svr->n_imu);
}
/* update rtk server struct --------------------------------------------------*/
static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
int ephsat, int ephset, sbsmsg_t *sbsmsg, int index,
int iobs, imud_t imud)
{
tracet(4,"updatesvr: ret=%d ephsat=%d ephset=%d index=%d\n",ret,ephsat,
ephset,index);
if (ret==1) { /* observation data */
update_obs(svr,obs,index,iobs);
}
else if (ret==2) { /* ephemeris */
update_eph(svr,nav,ephsat,ephset,index);
}
else if (ret==3) { /* sbas message */
update_sbs(svr,sbsmsg,index);
}
else if (ret==9) { /* ion/utc parameters */
update_ionutc(svr,nav,index);
}
else if (ret==5) { /* antenna postion */
update_antpos(svr,index);
}
else if (ret==7) { /* dgps correction */
svr->nmsg[index][5]++;
}
else if (ret==10) { /* ssr message */
update_ssr(svr,index);
}
else if(ret==11 && (index==0||index==2)){ /* IMU data */
updata_imu(svr,imud);
}
else if (ret==-1) { /* error */
svr->nmsg[index][9]++;
}
}
/* decode receiver raw/rtcm data ---------------------------------------------*/
static int decoderaw(rtksvr_t *svr, int index)
{
obs_t *obs;
nav_t *nav;
sbsmsg_t *sbsmsg=NULL;
imud_t imud;
int i,ret,ephsat,ephset,fobs=0;
tracet(4,"decoderaw: index=%d\n",index);
rtksvrlock(svr);
for (i=0;i<svr->nb[index];i++) {
/* input rtcm/receiver raw data from stream */
if (svr->format[index]==STRFMT_RTCM2) {
ret=input_rtcm2(svr->rtcm+index,svr->buff[index][i]);
obs=&svr->rtcm[index].obs;
nav=&svr->rtcm[index].nav;
ephsat=svr->rtcm[index].ephsat;
ephset=svr->rtcm[index].ephset;
}
else if (svr->format[index]==STRFMT_RTCM3) {
ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]);
obs=&svr->rtcm[index].obs;
nav=&svr->rtcm[index].nav;
ephsat=svr->rtcm[index].ephsat;
ephset=svr->rtcm[index].ephset;
/* only rover has imu */
if(index==0){
imud = svr->rtcm[index].imud;
}
}
else if(svr->format[index]==STRFMT_IMU){
ret=input_imu(svr->raw+index,&imud,svr->buff[index][i]);
}
else {
ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]);
obs=&svr->raw[index].obs;
nav=&svr->raw[index].nav;
ephsat=svr->raw[index].ephsat;
ephset=svr->raw[index].ephset;
sbsmsg=&svr->raw[index].sbsmsg;
}
#if 0 /* record for receiving tick for debug */
if (ret==1) {
trace(0,"%d %10d T=%s NS=%2d\n",index,tickget(),
time_str(obs->data[0].time,0),obs->n);
}
#endif
/* update rtk server */
if (ret>0) {
update_svr(svr,ret,obs,nav,ephsat,ephset,sbsmsg,index,fobs,imud);
}
/* observation data received */
if (ret==1) {
if (fobs<MAXOBSBUF) fobs++;
else svr->prcout++;
}
}
svr->nb[index]=0;
rtksvrunlock(svr);
return fobs;
}
/* decode download file ------------------------------------------------------*/
static void decodefile(rtksvr_t *svr, int index)
{
nav_t nav={0};
char file[1024];
int nb;
tracet(4,"decodefile: index=%d\n",index);
rtksvrlock(svr);
/* check file path completed */
if ((nb=svr->nb[index])<=2||
svr->buff[index][nb-2]!='\r'||svr->buff[index][nb-1]!='\n') {
rtksvrunlock(svr);
return;
}
strncpy(file,(char *)svr->buff[index],nb-2); file[nb-2]='\0';
svr->nb[index]=0;
rtksvrunlock(svr);
if (svr->format[index]==STRFMT_SP3) { /* precise ephemeris */
/* read sp3 precise ephemeris */
readsp3(file,&nav,0);
if (nav.ne<=0) {
tracet(1,"sp3 file read error: %s\n",file);
return;
}
/* update precise ephemeris */
rtksvrlock(svr);
if (svr->nav.peph) free(svr->nav.peph);
svr->nav.ne=svr->nav.nemax=nav.ne;
svr->nav.peph=nav.peph;
svr->ftime[index]=utc2gpst(timeget());
strcpy(svr->files[index],file);
rtksvrunlock(svr);
}
else if (svr->format[index]==STRFMT_RNXCLK) { /* precise clock */
/* read rinex clock */
if (readrnxc(file,&nav)<=0) {
tracet(1,"rinex clock file read error: %s\n",file);
return;
}
/* update precise clock */
rtksvrlock(svr);
if (svr->nav.pclk) free(svr->nav.pclk);
svr->nav.nc=svr->nav.ncmax=nav.nc;
svr->nav.pclk=nav.pclk;
svr->ftime[index]=utc2gpst(timeget());
strcpy(svr->files[index],file);
rtksvrunlock(svr);
}
}
/* carrier-phase bias (fcb) correction ---------------------------------------*/
static void corr_phase_bias(obsd_t *obs, int n, const nav_t *nav)
{
double freq;
uint8_t code;
int i,j;
for (i=0;i<n;i++) for (j=0;j<NFREQ;j++) {
code=obs[i].code[j];
if ((freq=sat2freq(obs[i].sat,code,nav))==0.0) continue;
/* correct phase bias (cyc) */
obs[i].L[j]-=nav->ssr[obs[i].sat-1].pbias[code-1]*freq/CLIGHT;
}
}
/* periodic command ----------------------------------------------------------*/
static void periodic_cmd(int cycle, const char *cmd, stream_t *stream)
{
const char *p=cmd,*q;
char msg[1024],*r;
int n,period;
for (p=cmd;;p=q+1) {
for (q=p;;q++) if (*q=='\r'||*q=='\n'||*q=='\0') break;
n=(int)(q-p); strncpy(msg,p,n); msg[n]='\0';
period=0;
if ((r=strrchr(msg,'#'))) {
sscanf(r,"# %d",&period);
*r='\0';
while (*--r==' ') *r='\0'; /* delete tail spaces */
}
if (period<=0) period=1000;
if (*msg&&cycle%period==0) {
strsendcmd(stream,msg);
}
if (!*q) break;
}
}
/* baseline length -----------------------------------------------------------*/
static double baseline_len(const rtk_t *rtk)
{
double dr[3];
int i;
if (norm(rtk->sol.rr,3)<=0.0||norm(rtk->rb,3)<=0.0) return 0.0;
for (i=0;i<3;i++) {
dr[i]=rtk->sol.rr[i]-rtk->rb[i];
}
return norm(dr,3)*0.001; /* (km) */
}
/* send nmea request to base/nrtk input stream -------------------------------*/
static void send_nmea(rtksvr_t *svr, uint32_t *tickreset)
{
sol_t sol_nmea={{0}};
double vel,bl;
uint32_t tick=tickget();
int i;
if (svr->stream[1].state!=1) return;
sol_nmea.ns=10; /* Some servers don't like when ns = 0 */
if (svr->nmeareq==1) { /* lat-lon-hgt mode */
sol_nmea.stat=SOLQ_SINGLE;
sol_nmea.time=utc2gpst(timeget());
matcpy(sol_nmea.rr,svr->nmeapos,3,1);
strsendnmea(svr->stream+1,&sol_nmea);
}
else if (svr->nmeareq==2) { /* single-solution mode */
if (norm(svr->rtk.sol.rr,3)<=0.0) return;
sol_nmea.stat=SOLQ_SINGLE;
sol_nmea.time=utc2gpst(timeget());
matcpy(sol_nmea.rr,svr->rtk.sol.rr,3,1);
strsendnmea(svr->stream+1,&sol_nmea);
}
else if (svr->nmeareq==3) { /* reset-and-single-sol mode */
/* send reset command if baseline over threshold */
bl=baseline_len(&svr->rtk);
if (bl>=svr->bl_reset&&(int)(tick-*tickreset)>MIN_INT_RESET) {
strsendcmd(svr->stream+1,svr->cmd_reset);
tracet(2,"send reset: bl=%.3f rr=%.3f %.3f %.3f rb=%.3f %.3f %.3f\n",
bl,svr->rtk.sol.rr[0],svr->rtk.sol.rr[1],svr->rtk.sol.rr[2],
svr->rtk.rb[0],svr->rtk.rb[1],svr->rtk.rb[2]);
*tickreset=tick;
}
if (norm(svr->rtk.sol.rr,3)<=0.0) return;
sol_nmea.stat=SOLQ_SINGLE;
sol_nmea.time=utc2gpst(timeget());
matcpy(sol_nmea.rr,svr->rtk.sol.rr,3,1);
/* set predicted position if velocity > 36km/h */
if ((vel=norm(svr->rtk.sol.rr+3,3))>10.0) {
for (i=0;i<3;i++) {
sol_nmea.rr[i]+=svr->rtk.sol.rr[i+3]/vel*svr->bl_reset*0.8;
}
}
strsendnmea(svr->stream+1,&sol_nmea);
tracet(3,"send nmea: rr=%.3f %.3f %.3f\n",sol_nmea.rr[0],sol_nmea.rr[1],
sol_nmea.rr[2]);
}
}
/* rtk server thread ---------------------------------------------------------*/
#ifdef WIN32
static DWORD WINAPI rtksvrthread(void *arg)
#else
static void *tcsvrthread(void *arg)
#endif
{
rtksvr_t *svr=(rtksvr_t *)arg;
obs_t obs;
obsd_t data[MAXOBS*2];
sol_t sol={{0}};
double tt;
uint32_t tick,ticknmea,tick1hz,tickreset;
uint8_t *p,*q;
char msg[128];
int i,j,n,fobs[3]={0},cycle,cputime;
tracet(3,"rtksvrthread:\n");
svr->state=1;
svr->rtk.ins.init_flag=0;
obs.data=data;
svr->tick=tickget();
ticknmea=tick1hz=svr->tick-1000;
tickreset=svr->tick-MIN_INT_RESET;
for (cycle=0;svr->state;cycle++) {
tick=tickget();
for (i=0;i<3;i++) {
p=svr->buff[i]+svr->nb[i];
q=svr->buff[i]+svr->buffsize;
/* read receiver raw/rtcm data from input stream */
if ((n=strread(svr->stream+i,p,q-p))<=0) {
continue;
}
/* write receiver raw/rtcm data to log stream */
strwrite(svr->stream+i+5,p,n);
svr->nb[i]+=n;
/* save peek buffer */
rtksvrlock(svr);
n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i];
memcpy(svr->pbuf[i]+svr->npb[i],p,n);
svr->npb[i]+=n;
rtksvrunlock(svr);
}
svr->n_imu = 0;
for (i=0;i<3;i++) {
if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) {
/* decode download file */
decodefile(svr,i);
}
else {
/* decode receiver raw/rtcm data */
fobs[i]=decoderaw(svr,i);
}
}
/* averaging single base pos */
if (fobs[1]>0&&svr->rtk.opt.refpos==POSOPT_SINGLE) {
if ((svr->rtk.opt.maxaveep<=0||svr->nave<svr->rtk.opt.maxaveep)&&
pntpos(svr->obs[1][0].data,svr->obs[1][0].n,&svr->nav,
&svr->rtk.opt,&sol,NULL,NULL,msg)) {
svr->nave++;
for (i=0;i<3;i++) {
svr->rb_ave[i]+=(sol.rr[i]-svr->rb_ave[i])/svr->nave;
}
}
for (i=0;i<3;i++) svr->rtk.opt.rb[i]=svr->rb_ave[i];
}
/* TC function */
rt_tcfilter(svr, fobs[0], tick);
/* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat==SOLQ_NONE&&(int)(tick-tick1hz)>=1000) {
writesol(svr,0);
tick1hz=tick;
}
/* write periodic command to input stream */
for (i=0;i<3;i++) {
periodic_cmd(cycle*svr->cycle,svr->cmds_periodic[i],svr->stream+i);
}
/* send nmea request to base/nrtk input stream */
if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) {
send_nmea(svr,&tickreset);
ticknmea=tick;
}
if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime;
/* sleep until next cycle */
sleepms(svr->cycle-cputime);
}
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
for (i=0;i<3;i++) {
svr->nb[i]=svr->npb[i]=0;
free(svr->buff[i]); svr->buff[i]=NULL;
free(svr->pbuf[i]); svr->pbuf[i]=NULL;
free_raw (svr->raw +i);
free_rtcm(svr->rtcm+i);
}
for (i=0;i<2;i++) {
svr->nsb[i]=0;
free(svr->sbuf[i]); svr->sbuf[i]=NULL;
}
return 0;
}
/* start rtk server ------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* int cycle I server cycle (ms)
* int buffsize I input buffer size (bytes)
* int *strs I stream types (STR_???)
* types[0]=input stream rover
* types[1]=input stream base station
* types[2]=input stream correction
* types[3]=output stream solution 1
* types[4]=output stream solution 2
* types[5]=log stream rover
* types[6]=log stream base station
* types[7]=log stream correction
* char *paths I input stream paths
* int *format I input stream formats (STRFMT_???)
* format[0]=input stream rover
* format[1]=input stream base station
* format[2]=input stream correction
* int navsel I navigation message select
* (0:rover,1:base,2:ephem,3:all)
* char **cmds I input stream start commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream corr (NULL: no command)
* char **cmds_periodic I input stream periodic commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream corr (NULL: no command)
* char **rcvopts I receiver options
* rcvopt[0]=receiver option rover
* rcvopt[1]=receiver option base
* rcvopt[2]=receiver option corr
* int nmeacycle I nmea request cycle (ms) (0:no request)
* int nmeareq I nmea request type
* (0:no,1:base pos,2:single sol,3:reset and single)
* double *nmeapos I transmitted nmea position (ecef) (m)
* prcopt_t *prcopt I rtk processing options
* solopt_t *solopt I solution options
* solopt[0]=solution 1 options
* solopt[1]=solution 2 options
* stream_t *moni I monitor stream (NULL: not used)
* char *errmsg O error message
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern int tcsvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **cmds_periodic, char **rcvopts, int nmeacycle,
int nmeareq, const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni, char *errmsg)
{
gtime_t time,time0={0};
int i,j,rw;
tracet(3,"rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n",
cycle,buffsize,navsel,nmeacycle,nmeareq);
if (svr->state) {
sprintf(errmsg,"server already started");
return 0;
}
strinitcom();
svr->cycle=cycle>1?cycle:1;
svr->nmeacycle=nmeacycle>1000?nmeacycle:1000;
svr->nmeareq=nmeareq;
for (i=0;i<3;i++) svr->nmeapos[i]=nmeapos[i];
svr->buffsize=buffsize>4096?buffsize:4096;
for (i=0;i<3;i++) svr->format[i]=formats[i];
svr->navsel=navsel;
svr->nsbs=0;
svr->nsol=0;
svr->prcout=0;
rtkfree(&svr->rtk);
rtkinit(&svr->rtk,prcopt);
if (prcopt->initrst) { /* init averaging pos by restart */
svr->nave=0;
for (i=0;i<3;i++) svr->rb_ave[i]=0.0;
}
for (i=0;i<3;i++) { /* input/log streams */
svr->nb[i]=svr->npb[i]=0;
if (!(svr->buff[i]=(uint8_t *)malloc(buffsize))||
!(svr->pbuf[i]=(uint8_t *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
sprintf(errmsg,"rtk server malloc error");
return 0;
}
for (j=0;j<10;j++) svr->nmsg[i][j]=0;
for (j=0;j<MAXOBSBUF;j++) svr->obs[i][j].n=0;
strcpy(svr->cmds_periodic[i],!cmds_periodic[i]?"":cmds_periodic[i]);
/* initialize receiver raw and rtcm control */
init_raw(svr->raw+i,formats[i]);
init_rtcm(svr->rtcm+i);
/* set receiver and rtcm option */
strcpy(svr->raw [i].opt,rcvopts[i]);
strcpy(svr->rtcm[i].opt,rcvopts[i]);
/* connect dgps corrections */
svr->rtcm[i].dgps=svr->nav.dgps;
}
for (i=0;i<2;i++) { /* output peek buffer */
if (!(svr->sbuf[i]=(uint8_t *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
sprintf(errmsg,"rtk server malloc error");
return 0;
}
}
/* set solution options */
for (i=0;i<2;i++) {
svr->solopt[i]=solopt[i];
}
/* set base station position */
if (prcopt->refpos!=POSOPT_SINGLE) {
for (i=0;i<6;i++) {
svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0;
}
}
/* update navigation data */
for (i=0;i<MAXSAT*4 ;i++) svr->nav.eph [i].ttr=time0;
for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i].tof=time0;
for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i].tof=time0;
/* set monitor stream */
svr->moni=moni;
/* open input streams */
for (i=0;i<8;i++) {
rw=i<3?STR_MODE_R:STR_MODE_W;
if (strs[i]!=STR_FILE) rw|=STR_MODE_W;
if (!stropen(svr->stream+i,strs[i],rw,paths[i])) {
sprintf(errmsg,"str%d open error path=%s",i+1,paths[i]);
for (i--;i>=0;i--) strclose(svr->stream+i);
return 0;
}
/* set initial time for rtcm and raw */
if (i<3) {
time=utc2gpst(timeget());
svr->raw [i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
svr->rtcm[i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
}
}
/* sync input streams */
strsync(svr->stream,svr->stream+1);
strsync(svr->stream,svr->stream+2);
/* write start commands to input streams */
for (i=0;i<3;i++) {
if (!cmds[i]) continue;
strwrite(svr->stream+i,(unsigned char *)"",0); /* for connect */
sleepms(100);
strsendcmd(svr->stream+i,cmds[i]);
}
/* write solution header to solution streams */
for (i=3;i<5;i++) {
writesolhead(svr->stream+i,svr->solopt+i-3);
}
/* create rtk server thread */
#ifdef WIN32
if (!(svr->thread=CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) {
#else
if (pthread_create(&svr->thread,NULL,tcsvrthread,svr)) {
#endif
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
sprintf(errmsg,"thread create error\n");
return 0;
}
signal(SIGINT, sigshut); /* keyboard interrupt */
while (!intflg){
}
return 1;
}

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/ins.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/nhc.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_doppler.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_init.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zaru.c.o
View File

Binary file not shown.

BIN
TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zvu.c.o
View File

Binary file not shown.

135
TC_NAV_Zjut_post/cmake-build-debug/Makefile
View File

@ -729,6 +729,54 @@ Src/rcvraw.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/rcvraw.c.s $(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/rcvraw.c.s
.PHONY : Src/rcvraw.c.s .PHONY : Src/rcvraw.c.s
Src/real_time/client.o: Src/real_time/client.c.o
.PHONY : Src/real_time/client.o
# target to build an object file
Src/real_time/client.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/client.c.o
.PHONY : Src/real_time/client.c.o
Src/real_time/client.i: Src/real_time/client.c.i
.PHONY : Src/real_time/client.i
# target to preprocess a source file
Src/real_time/client.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/client.c.i
.PHONY : Src/real_time/client.c.i
Src/real_time/client.s: Src/real_time/client.c.s
.PHONY : Src/real_time/client.s
# target to generate assembly for a file
Src/real_time/client.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/client.c.s
.PHONY : Src/real_time/client.c.s
Src/real_time/imu.o: Src/real_time/imu.c.o
.PHONY : Src/real_time/imu.o
# target to build an object file
Src/real_time/imu.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/imu.c.o
.PHONY : Src/real_time/imu.c.o
Src/real_time/imu.i: Src/real_time/imu.c.i
.PHONY : Src/real_time/imu.i
# target to preprocess a source file
Src/real_time/imu.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/imu.c.i
.PHONY : Src/real_time/imu.c.i
Src/real_time/imu.s: Src/real_time/imu.c.s
.PHONY : Src/real_time/imu.s
# target to generate assembly for a file
Src/real_time/imu.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/imu.c.s
.PHONY : Src/real_time/imu.c.s
Src/real_time/ins.o: Src/real_time/ins.c.o Src/real_time/ins.o: Src/real_time/ins.c.o
.PHONY : Src/real_time/ins.o .PHONY : Src/real_time/ins.o
@ -849,6 +897,78 @@ Src/real_time/rt_ins-gnss.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.s $(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.s
.PHONY : Src/real_time/rt_ins-gnss.c.s .PHONY : Src/real_time/rt_ins-gnss.c.s
Src/real_time/rt_tcfilt.o: Src/real_time/rt_tcfilt.c.o
.PHONY : Src/real_time/rt_tcfilt.o
# target to build an object file
Src/real_time/rt_tcfilt.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcfilt.c.o
.PHONY : Src/real_time/rt_tcfilt.c.o
Src/real_time/rt_tcfilt.i: Src/real_time/rt_tcfilt.c.i
.PHONY : Src/real_time/rt_tcfilt.i
# target to preprocess a source file
Src/real_time/rt_tcfilt.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcfilt.c.i
.PHONY : Src/real_time/rt_tcfilt.c.i
Src/real_time/rt_tcfilt.s: Src/real_time/rt_tcfilt.c.s
.PHONY : Src/real_time/rt_tcfilt.s
# target to generate assembly for a file
Src/real_time/rt_tcfilt.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcfilt.c.s
.PHONY : Src/real_time/rt_tcfilt.c.s
Src/real_time/rt_tcpos.o: Src/real_time/rt_tcpos.c.o
.PHONY : Src/real_time/rt_tcpos.o
# target to build an object file
Src/real_time/rt_tcpos.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcpos.c.o
.PHONY : Src/real_time/rt_tcpos.c.o
Src/real_time/rt_tcpos.i: Src/real_time/rt_tcpos.c.i
.PHONY : Src/real_time/rt_tcpos.i
# target to preprocess a source file
Src/real_time/rt_tcpos.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcpos.c.i
.PHONY : Src/real_time/rt_tcpos.c.i
Src/real_time/rt_tcpos.s: Src/real_time/rt_tcpos.c.s
.PHONY : Src/real_time/rt_tcpos.s
# target to generate assembly for a file
Src/real_time/rt_tcpos.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcpos.c.s
.PHONY : Src/real_time/rt_tcpos.c.s
Src/real_time/rt_tcsvr.o: Src/real_time/rt_tcsvr.c.o
.PHONY : Src/real_time/rt_tcsvr.o
# target to build an object file
Src/real_time/rt_tcsvr.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcsvr.c.o
.PHONY : Src/real_time/rt_tcsvr.c.o
Src/real_time/rt_tcsvr.i: Src/real_time/rt_tcsvr.c.i
.PHONY : Src/real_time/rt_tcsvr.i
# target to preprocess a source file
Src/real_time/rt_tcsvr.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcsvr.c.i
.PHONY : Src/real_time/rt_tcsvr.c.i
Src/real_time/rt_tcsvr.s: Src/real_time/rt_tcsvr.c.s
.PHONY : Src/real_time/rt_tcsvr.s
# target to generate assembly for a file
Src/real_time/rt_tcsvr.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/TC_NAV_Zjut.dir/build.make CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_tcsvr.c.s
.PHONY : Src/real_time/rt_tcsvr.c.s
Src/real_time/rt_zaru.o: Src/real_time/rt_zaru.c.o Src/real_time/rt_zaru.o: Src/real_time/rt_zaru.c.o
.PHONY : Src/real_time/rt_zaru.o .PHONY : Src/real_time/rt_zaru.o
@ -1461,6 +1581,12 @@ help:
@echo "... Src/rcvraw.o" @echo "... Src/rcvraw.o"
@echo "... Src/rcvraw.i" @echo "... Src/rcvraw.i"
@echo "... Src/rcvraw.s" @echo "... Src/rcvraw.s"
@echo "... Src/real_time/client.o"
@echo "... Src/real_time/client.i"
@echo "... Src/real_time/client.s"
@echo "... Src/real_time/imu.o"
@echo "... Src/real_time/imu.i"
@echo "... Src/real_time/imu.s"
@echo "... Src/real_time/ins.o" @echo "... Src/real_time/ins.o"
@echo "... Src/real_time/ins.i" @echo "... Src/real_time/ins.i"
@echo "... Src/real_time/ins.s" @echo "... Src/real_time/ins.s"
@ -1476,6 +1602,15 @@ help:
@echo "... Src/real_time/rt_ins-gnss.o" @echo "... Src/real_time/rt_ins-gnss.o"
@echo "... Src/real_time/rt_ins-gnss.i" @echo "... Src/real_time/rt_ins-gnss.i"
@echo "... Src/real_time/rt_ins-gnss.s" @echo "... Src/real_time/rt_ins-gnss.s"
@echo "... Src/real_time/rt_tcfilt.o"
@echo "... Src/real_time/rt_tcfilt.i"
@echo "... Src/real_time/rt_tcfilt.s"
@echo "... Src/real_time/rt_tcpos.o"
@echo "... Src/real_time/rt_tcpos.i"
@echo "... Src/real_time/rt_tcpos.s"
@echo "... Src/real_time/rt_tcsvr.o"
@echo "... Src/real_time/rt_tcsvr.i"
@echo "... Src/real_time/rt_tcsvr.s"
@echo "... Src/real_time/rt_zaru.o" @echo "... Src/real_time/rt_zaru.o"
@echo "... Src/real_time/rt_zaru.i" @echo "... Src/real_time/rt_zaru.i"
@echo "... Src/real_time/rt_zaru.s" @echo "... Src/real_time/rt_zaru.s"

4
TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
View File

@ -1,3 +1,3 @@
Start testing: Mar 15 14:54 CST Start testing: Mar 25 14:06 CST
---------------------------------------------------------- ----------------------------------------------------------
End testing: Mar 15 14:54 CST End testing: Mar 25 14:06 CST

6
TC_NAV_Zjut_post/config/Tc.conf
View File

@ -135,9 +135,9 @@ inpstr1-path =ttyUSB0:460800:8:n:1:off
# inpstr2-path =caxm002:m2ywj94n@sdk.pnt.10086.cn:8002/RTCM33_GRCEJ # inpstr2-path =caxm002:m2ywj94n@sdk.pnt.10086.cn:8002/RTCM33_GRCEJ
inpstr2-path = inpstr2-path =
inpstr3-path = inpstr3-path =
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) 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) 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) 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)
inpstr2-nmeareq =single # (0:off,1:latlon,2:single) inpstr2-nmeareq =single # (0:off,1:latlon,2:single)
inpstr2-nmealat =30.23469717 # (deg) inpstr2-nmealat =30.23469717 # (deg)
inpstr2-nmealon =120.04264567 # (deg) inpstr2-nmealon =120.04264567 # (deg)

7
TC_NAV_Zjut_post/include/rtklib.h
View File

@ -485,6 +485,7 @@ extern "C" {
#define STRFMT_RNXCLK 16 /* stream format: RINEX CLK */ #define STRFMT_RNXCLK 16 /* stream format: RINEX CLK */
#define STRFMT_SBAS 17 /* stream format: SBAS messages */ #define STRFMT_SBAS 17 /* stream format: SBAS messages */
#define STRFMT_NMEA 18 /* stream format: NMEA 0183 */ #define STRFMT_NMEA 18 /* stream format: NMEA 0183 */
#define STRFMT_IMU 19 /* stream format: IMU */
#define MAXRCVFMT 13 /* max number of receiver format */ #define MAXRCVFMT 13 /* max number of receiver format */
#define STR_MODE_R 0x1 /* stream mode: read */ #define STR_MODE_R 0x1 /* stream mode: read */
@ -559,6 +560,8 @@ extern "C" {
#define MAXIMUBUF 1000 /* max number of imu measurement data buffer */ #define MAXIMUBUF 1000 /* max number of imu measurement data buffer */
#define IMUBUFF_SIZE 300 /* imu buffer size */
#define SQR(x) ((x)*(x)) #define SQR(x) ((x)*(x))
#define SQRT(x) ((x)<=0.0||(x)!=(x)?0.0:sqrt(x)) #define SQRT(x) ((x)<=0.0||(x)!=(x)?0.0:sqrt(x))
@ -1931,6 +1934,10 @@ EXPORT int decode_gal_fnav(const uint8_t *buff, eph_t *eph, double *ion,
EXPORT int decode_irn_nav(const uint8_t *buff, eph_t *eph, double *ion, EXPORT int decode_irn_nav(const uint8_t *buff, eph_t *eph, double *ion,
double *utc); double *utc);
/* decode imu data in imu.c */
EXPORT int input_imu (raw_t *raw, imud_t *imud ,uint8_t data);
EXPORT int decode_imu (uint8_t *buff,imud_t *imud);
EXPORT int init_raw (raw_t *raw, int format); EXPORT int init_raw (raw_t *raw, int format);
EXPORT void free_raw (raw_t *raw); EXPORT void free_raw (raw_t *raw);
EXPORT int input_raw (raw_t *raw, int format, uint8_t data); EXPORT int input_raw (raw_t *raw, int format, uint8_t data);