diff --git a/TC_NAV_Zjut_post/CMakeLists.txt b/TC_NAV_Zjut_post/CMakeLists.txt index b224ae1..89c105f 100644 --- a/TC_NAV_Zjut_post/CMakeLists.txt +++ b/TC_NAV_Zjut_post/CMakeLists.txt @@ -15,7 +15,7 @@ if(WIN32) link_libraries(winmm.lib) 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 find_package(Threads REQUIRED) set(CMAKE_THREAD_PREFER_PTHREAD) diff --git a/TC_NAV_Zjut_post/Src/real_time/client.c b/TC_NAV_Zjut_post/Src/real_time/client.c new file mode 100644 index 0000000..bdcd943 --- /dev/null +++ b/TC_NAV_Zjut_post/Src/real_time/client.c @@ -0,0 +1,64 @@ +// +// Created by wakashaw on 23-3-17. +// + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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; +} \ No newline at end of file diff --git a/TC_NAV_Zjut_post/Src/real_time/imu.c b/TC_NAV_Zjut_post/Src/real_time/imu.c new file mode 100644 index 0000000..66c0393 --- /dev/null +++ b/TC_NAV_Zjut_post/Src/real_time/imu.c @@ -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; + +} diff --git a/TC_NAV_Zjut_post/Src/real_time/rt_tcfilt.c b/TC_NAV_Zjut_post/Src/real_time/rt_tcfilt.c new file mode 100644 index 0000000..64d8e06 --- /dev/null +++ b/TC_NAV_Zjut_post/Src/real_time/rt_tcfilt.c @@ -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=nbuffsize-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;issr[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->nsolsolbuf[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;jimud[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;iobs[0][i].n&&obs.nobs[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); +// } + +} \ No newline at end of file diff --git a/TC_NAV_Zjut_post/Src/real_time/rt_tcpos.c b/TC_NAV_Zjut_post/Src/real_time/rt_tcpos.c new file mode 100644 index 0000000..487567e --- /dev/null +++ b/TC_NAV_Zjut_post/Src/real_time/rt_tcpos.c @@ -0,0 +1,203 @@ +// +// Created by wakashaw on 23-3-17. +// +#include +#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; +} \ No newline at end of file diff --git a/TC_NAV_Zjut_post/Src/real_time/rt_tcsvr.c b/TC_NAV_Zjut_post/Src/real_time/rt_tcsvr.c new file mode 100644 index 0000000..d663a7c --- /dev/null +++ b/TC_NAV_Zjut_post/Src/real_time/rt_tcsvr.c @@ -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=nbuffsize-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->nsolsolbuf[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;iraw[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 (iobsn;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->nsbssbsmsg[svr->nsbs++]=*sbsmsg; + } + else { + for (i=0;isbsmsg[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;irtcm[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;inb[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 (fobsprcout++; + } + } + 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;issr[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=nbuffsize-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->navertk.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;istream+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;jobs[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;inav.eph [i].ttr=time0; + for (i=0;inav.geph[i].tof=time0; + for (i=0;inav.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;istream+i); + sprintf(errmsg,"thread create error\n"); + return 0; + } + + signal(SIGINT, sigshut); /* keyboard interrupt */ + + while (!intflg){ + } + return 1; +} + diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/ins.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/ins.c.o index 7c2ccae..7a0b461 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/ins.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/ins.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/nhc.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/nhc.c.o index f98e914..ae55f18 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/nhc.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/nhc.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_doppler.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_doppler.c.o index a0c3089..20ee8e7 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_doppler.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_doppler.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_init.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_init.c.o index 9ddf35e..a016fe5 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_init.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_init.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.o index d11f633..706eee7 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_ins-gnss.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zaru.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zaru.c.o index f4b9a6d..b7c3620 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zaru.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zaru.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zvu.c.o b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zvu.c.o index 901ae1f..100783c 100644 Binary files a/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zvu.c.o and b/TC_NAV_Zjut_post/cmake-build-debug/CMakeFiles/TC_NAV_Zjut.dir/Src/real_time/rt_zvu.c.o differ diff --git a/TC_NAV_Zjut_post/cmake-build-debug/Makefile b/TC_NAV_Zjut_post/cmake-build-debug/Makefile index 478b1a9..5f17d7b 100644 --- a/TC_NAV_Zjut_post/cmake-build-debug/Makefile +++ b/TC_NAV_Zjut_post/cmake-build-debug/Makefile @@ -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 .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 .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 .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 .PHONY : Src/real_time/rt_zaru.o @@ -1461,6 +1581,12 @@ help: @echo "... Src/rcvraw.o" @echo "... Src/rcvraw.i" @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.i" @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.i" @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.i" @echo "... Src/real_time/rt_zaru.s" diff --git a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log b/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log index 089c242..ef7c738 100644 --- a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log +++ b/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log @@ -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 diff --git a/TC_NAV_Zjut_post/config/Tc.conf b/TC_NAV_Zjut_post/config/Tc.conf index d68f194..9ed2ca5 100644 --- a/TC_NAV_Zjut_post/config/Tc.conf +++ b/TC_NAV_Zjut_post/config/Tc.conf @@ -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 = 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) -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) -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) +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) inpstr2-nmeareq =single # (0:off,1:latlon,2:single) inpstr2-nmealat =30.23469717 # (deg) inpstr2-nmealon =120.04264567 # (deg) diff --git a/TC_NAV_Zjut_post/include/rtklib.h b/TC_NAV_Zjut_post/include/rtklib.h index b912b9b..bf73f3a 100644 --- a/TC_NAV_Zjut_post/include/rtklib.h +++ b/TC_NAV_Zjut_post/include/rtklib.h @@ -485,6 +485,7 @@ extern "C" { #define STRFMT_RNXCLK 16 /* stream format: RINEX CLK */ #define STRFMT_SBAS 17 /* stream format: SBAS messages */ #define STRFMT_NMEA 18 /* stream format: NMEA 0183 */ +#define STRFMT_IMU 19 /* stream format: IMU */ #define MAXRCVFMT 13 /* max number of receiver format */ #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 IMUBUFF_SIZE 300 /* imu buffer size */ + #define SQR(x) ((x)*(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, 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 void free_raw (raw_t *raw); EXPORT int input_raw (raw_t *raw, int format, uint8_t data);