mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Add more work on rtklib files

This commit is contained in:
Carles Fernandez 2017-05-12 12:17:42 +02:00
parent b2531cb926
commit 833192e183
11 changed files with 5608 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/algorithms/libs/rtklib/CMakeLists.txt
View File

@ -30,6 +30,9 @@ set(RTKLIB_LIB_SOURCES
rtklib_lambda.cc
rtklib_rtkpos.cc
rtklib_conversions.cc
rtklib_stream.cc
rtklib_rtksvr.cc
rtklib_solution.cc
)
include_directories(
@ -59,5 +62,5 @@ target_link_libraries(
${GLOG_LIBRARIES}
${BLAS}
${LAPACK}
${MAC_LIBRARIES}
${MAC_LIBRARIES}
)

37
src/algorithms/libs/rtklib/rtklib.h
View File

@ -294,10 +294,26 @@ const int ARMODE_WLNL = 6;
const int ARMODE_TCAR = 7;
const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */
const int POSOPT_RINEX = 3; //!< pos option: rinex header pos
const int MAXSTRPATH = 1024; //!< max length of stream path
const int MAXSTRMSG = 1024; //!< max length of stream message
typedef void fatalfunc_t(const char *); //!< fatal callback function type
#define STR_MODE_R 0x1 /* stream mode: read */
#define STR_MODE_W 0x2 /* stream mode: write */
#define STR_MODE_RW 0x3 /* stream mode: read/write */
#define STR_NONE 0 /* stream type: none */
#define STR_SERIAL 1 /* stream type: serial */
#define STR_FILE 2 /* stream type: file */
#define STR_TCPSVR 3 /* stream type: TCP server */
#define STR_TCPCLI 4 /* stream type: TCP client */
#define STR_UDP 5 /* stream type: UDP stream */
#define STR_NTRIPSVR 6 /* stream type: NTRIP server */
#define STR_NTRIPCLI 7 /* stream type: NTRIP client */
#define STR_FTP 8 /* stream type: ftp */
#define STR_HTTP 9 /* stream type: http */
#define NP_PPP(opt) ((opt)->dynamics?9:3) /* number of pos solution */
#define IC_PPP(s,opt) (NP_PPP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */
@ -306,6 +322,14 @@ typedef void fatalfunc_t(const char *); //!< fatal callback function type
#define IB_PPP(s,opt) (NR_PPP(opt)+(s)-1) /* state index of phase bias */
#define NX_PPP(opt) (IB_PPP(MAXSAT,opt)+1) /* number of estimated states */
#define NF_RTK(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
#define NP_RTK(opt) ((opt)->dynamics==0?3:9)
#define NI_RTK(opt) ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT)
#define NT_RTK(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt<TROPOPT_ESTG?2:6))
#define NL_RTK(opt) ((opt)->glomodear!=2?0:NFREQGLO)
#define NB_RTK(opt) ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF_RTK(opt))
#define NR_RTK(opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+NL_RTK(opt))
#define NX_RTK(opt) (NR_RTK(opt)+NB_RTK(opt))
typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */
@ -1006,4 +1030,15 @@ const double lam_carr[MAXFREQ] = { /* carrier wave length (m) */
SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9
};
const int STRFMT_RTCM2 = 0; /* stream format: RTCM 2 */
const int STRFMT_RTCM3 = 1; /* stream format: RTCM 3 */
const int STRFMT_SP3 = 16; /* stream format: SP3 */
const int STRFMT_RNXCLK = 17; /* stream format: RINEX CLK */
const int STRFMT_SBAS = 18; /* stream format: SBAS messages */
const int STRFMT_NMEA = 19; /* stream format: NMEA 0183 */
//const solopt_t solopt_default; /* default solution output options */
const int MAXSTRRTK = 8; /* max number of stream in RTK server */
#endif

16
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -65,6 +65,8 @@ const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */
const double gst0 [] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */
const double bdt0 [] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */
static double timeoffset_ = 0.0;
double leaps[MAXLEAPS+1][7] = { /* leap seconds (y,m,d,h,m,s,utc-gpst) */
{2017, 1, 1, 0, 0, 0, -18},
{2015, 7, 1, 0, 0, 0, -17},
@ -1381,12 +1383,12 @@ gtime_t timeget(void)
* notes : just set time offset between cpu time and current time
* the time offset is reflected to only timeget()
* not reentrant
*-----------------------------------------------------------------------------
*-----------------------------------------------------------------------------*/
void timeset(gtime_t t)
{
timeoffset_+=timediff(t,timeget());
timeoffset_ += timediff(t, timeget());
}
*/
/* read leap seconds table by text -------------------------------------------*/
int read_leaps_text(FILE *fp)
{
@ -3010,8 +3012,9 @@ void freenav(nav_t *nav, int opt)
// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap);
// fflush(fp_trace);
//}
//extern void tracet(int level, const char *format, ...)
//{
void tracet(int level __attribute__((unused)), const char *format __attribute__((unused)), ...)
{
// va_list ap;
//
// if (!fp_trace||level>level_trace) return;
@ -3019,7 +3022,8 @@ void freenav(nav_t *nav, int opt)
// fprintf(fp_trace,"%d %9.3f: ",level,(tickget()-tick_trace)/1000.0);
// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap);
// fflush(fp_trace);
//}
}
void tracemat(int level __attribute__((unused)), const double *A __attribute__((unused)),
int n __attribute__((unused)), int m __attribute__((unused)), int p __attribute__((unused)),
int q __attribute__((unused)))

4
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@ -157,7 +157,7 @@ double time2bdt(gtime_t t, int *week);
gtime_t timeadd(gtime_t t, double sec);
double timediff(gtime_t t1, gtime_t t2);
gtime_t timeget(void);
//void timeset(gtime_t t);
void timeset(gtime_t t);
int read_leaps_text(FILE *fp);
int read_leaps_usno(FILE *fp);
int read_leaps(const char *file);
@ -218,7 +218,7 @@ void freenav(nav_t *nav, int opt);
//void traceclose(void);
//void tracelevel(int level);
void trace (int level, const char *format, ...);
//void tracet (int level, const char *format, ...);
void tracet (int level, const char *format, ...);
void tracemat(int level, const double *A, int n, int m, int p, int q);
void traceobs(int level, const obsd_t *obs, int n);
//void tracenav(int level, const nav_t *nav);

9
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@ -74,14 +74,7 @@ const double TTOL_MOVEB = (1.0+2*DTTOL);
/* time sync tolerance for moving-baseline (s) */
/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */
#define NF_RTK(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
#define NP_RTK(opt) ((opt)->dynamics==0?3:9)
#define NI_RTK(opt) ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT)
#define NT_RTK(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt<TROPOPT_ESTG?2:6))
#define NL_RTK(opt) ((opt)->glomodear!=2?0:NFREQGLO)
#define NB_RTK(opt) ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF_RTK(opt))
#define NR_RTK(opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+NL_RTK(opt))
#define NX_RTK(opt) (NR_RTK(opt)+NB_RTK(opt))
/* state variable index */
#define II_RTK(s,opt) (NP_RTK(opt)+(s)-1) /* ionos (s:satellite no) */

977
src/algorithms/libs/rtklib/rtklib_rtksvr.cc Normal file
View File

@ -0,0 +1,977 @@
#include "rtklib_rtksvr.h"
#include "rtklib_rtkcmn.h"
#include "rtklib_rtkpos.h"
#include "rtklib_solution.h"
#include "rtklib_sbas.h"
#include "rtklib_preceph.h"
#include "rtklib_stream.h"
/* write solution header to output stream ------------------------------------*/
void writesolhead(stream_t *stream, const solopt_t *solopt)
{
unsigned char buff[1024];
int n;
n=outsolheads(buff,solopt);
strwrite(stream,buff,n);
}
/* save output buffer --------------------------------------------------------*/
void saveoutbuf(rtksvr_t *svr, unsigned char *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 -------------------------------------------*/
void writesol(rtksvr_t *svr, int index)
{
solopt_t solopt=solopt_default;
unsigned char buff[1024];
int i,n;
tracet(4,"writesol: index=%d\n",index);
for (i=0;i<2;i++)
{
/* 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 navigation data ----------------------------------------------------*/
void updatenav(nav_t *nav)
{
int i,j;
for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++)
{
nav->lam[i][j]=satwavelen(i+1,j,nav);
}
}
/* update glonass frequency channel number in raw data struct ----------------*/
void updatefcn(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 rtk server struct --------------------------------------------------*/
void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
sbsmsg_t *sbsmsg, int index, int iobs)
{
eph_t *eph1,*eph2,*eph3;
geph_t *geph1,*geph2,*geph3;
gtime_t tof;
double pos[3],del[3]={0},dr[3];
int i,n=0,prn,sbssat=svr->rtk.opt.sbassatsel,sys,iode;
tracet(4,"updatesvr: ret=%d sat=%2d index=%d\n",ret,sat,index);
if (ret==1)
{ /* observation data */
if (iobs<MAXOBSBUF)
{
for (i=0;i<obs->n;i++)
{
if (svr->rtk.opt.exsats[obs->data[i].sat-1]==1||
!(satsys(obs->data[i].sat,NULL)&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]++;
}
else if (ret==2)
{ /* ephemeris */
if (satsys(sat,&prn)!=SYS_GLO)
{
if (!svr->navsel||svr->navsel==index+1)
{
eph1=nav->eph+sat-1;
eph2=svr->nav.eph+sat-1;
eph3=svr->nav.eph+sat-1+MAXSAT;
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))
{
*eph3=*eph2;
*eph2=*eph1;
updatenav(&svr->nav);
}
}
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;
updatenav(&svr->nav);
updatefcn(svr);
}
}
svr->nmsg[index][6]++;
}
}
else if (ret==3)
{ /* sbas message */
if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) {
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]++;
}
else if (ret==9)
{ /* ion/utc parameters */
if (svr->navsel==index||svr->navsel>=3)
{
for (i=0;i<8;i++) svr->nav.ion_gps[i]=nav->ion_gps[i];
for (i=0;i<4;i++) svr->nav.utc_gps[i]=nav->utc_gps[i];
for (i=0;i<4;i++) svr->nav.ion_gal[i]=nav->ion_gal[i];
for (i=0;i<4;i++) svr->nav.utc_gal[i]=nav->utc_gal[i];
for (i=0;i<8;i++) svr->nav.ion_qzs[i]=nav->ion_qzs[i];
for (i=0;i<4;i++) svr->nav.utc_qzs[i]=nav->utc_qzs[i];
svr->nav.leaps=nav->leaps;
}
svr->nmsg[index][2]++;
}
else if (ret==5)
{ /* antenna postion parameters */
if (svr->rtk.opt.refpos==4&&index==1)
{
for (i=0;i<3;i++)
{
svr->rtk.rb[i]=svr->rtcm[1].sta.pos[i];
}
/* antenna delta */
ecef2pos(svr->rtk.rb,pos);
if (svr->rtcm[1].sta.deltype)
{ /* xyz */
del[2]=svr->rtcm[1].sta.hgt;
enu2ecef(pos,del,dr);
for (i=0;i<3;i++)
{
svr->rtk.rb[i]+=svr->rtcm[1].sta.del[i]+dr[i];
}
}
else
{ /* enu */
enu2ecef(pos,svr->rtcm[1].sta.del,dr);
for (i=0;i<3;i++)
{
svr->rtk.rb[i]+=dr[i];
}
}
}
svr->nmsg[index][4]++;
}
else if (ret==7)
{ /* dgps correction */
svr->nmsg[index][5]++;
}
else if (ret==10)
{ /* ssr message */
for (i=0;i<MAXSAT;i++)
{
if (!svr->rtcm[index].ssr[i].update) 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]++;
}
else if (ret==31)
{ /* lex message */
// lexupdatecorr(&svr->raw[index].lexmsg,&svr->nav,&tof);
svr->nmsg[index][8]++;
}
else if (ret==-1)
{ /* error */
svr->nmsg[index][9]++;
}
}
/* decode receiver raw/rtcm data ---------------------------------------------*/
int decoderaw(rtksvr_t *svr, int index)
{
obs_t *obs;
nav_t *nav;
sbsmsg_t *sbsmsg=NULL;
int i,ret,sat,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;
sat=svr->rtcm[index].ephsat;
}
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;
sat=svr->rtcm[index].ephsat;
}
else
{
// Disabled !!
//ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]);
obs=&svr->raw[index].obs;
nav=&svr->raw[index].nav;
sat=svr->raw[index].ephsat;
sbsmsg=&svr->raw[index].sbsmsg;
}
#if 0 /* record for receiving tick */
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) updatesvr(svr,ret,obs,nav,sat,sbsmsg,index,fobs);
/* observation data received */
if (ret==1) {
if (fobs<MAXOBSBUF) fobs++; else svr->prcout++;
}
}
svr->nb[index]=0;
rtksvrunlock(svr);
return fobs;
}
/* decode download file ------------------------------------------------------*/
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 */ // Disabled!!
if ( 1 /*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);
}
}
/* rtk server thread ---------------------------------------------------------*/
void *rtksvrthread(void *arg)
{
rtksvr_t *svr=(rtksvr_t *)arg;
obs_t obs;
obsd_t data[MAXOBS*2];
double tt;
unsigned int tick,ticknmea;
unsigned char *p,*q;
int i,j,n,fobs[3]={0},cycle,cputime;
tracet(3,"rtksvrthread:\n");
svr->state=1; obs.data=data;
svr->tick=tickget();
ticknmea=svr->tick-1000;
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);
}
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);
}
}
for (i=0;i<fobs[0];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];
}
for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;j++)
{
obs.data[obs.n++]=svr->obs[1][0].data[j];
}
/* rtk positioning */
rtksvrlock(svr);
rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav);
rtksvrunlock(svr);
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)));
/* write solution */
writesol(svr,i);
}
/* if cpu overload, inclement obs outage counter and break */
if ((int)(tickget()-tick)>=svr->cycle)
{
svr->prcout+=fobs[0]-i-1;
#if 0 /* omitted v.2.4.1 */
break;
#endif
}
}
/* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0)
{
writesol(svr,0);
}
/* send nmea request to base/nrtk input stream */
if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle)
{
if (svr->stream[1].state==1)
{
if (svr->nmeareq==1)
{
strsendnmea(svr->stream+1,svr->nmeapos);
}
else if (svr->nmeareq==2&&norm_rtk(svr->rtk.sol.rr,3)>0.0)
{
strsendnmea(svr->stream+1,svr->rtk.sol.rr);
}
}
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;
}
/* initialize rtk server -------------------------------------------------------
* initialize rtk server
* args : rtksvr_t *svr IO rtk server
* return : status (0:error,1:ok)
*-----------------------------------------------------------------------------*/
int rtksvrinit(rtksvr_t *svr)
{
gtime_t time0={0};
sol_t sol0 ={{0}};
eph_t eph0 ={0,-1,-1};
geph_t geph0={0,-1};
seph_t seph0={0};
int i,j;
tracet(3,"rtksvrinit:\n");
svr->state=svr->cycle=svr->nmeacycle=svr->nmeareq=0;
for (i=0;i<3;i++) svr->nmeapos[i]=0.0;
svr->buffsize=0;
for (i=0;i<3;i++) svr->format[i]=0;
for (i=0;i<2;i++) svr->solopt[i]=solopt_default;
svr->navsel=svr->nsbs=svr->nsol=0;
rtkinit(&svr->rtk,&prcopt_default);
for (i=0;i<3;i++) svr->nb[i]=0;
for (i=0;i<2;i++) svr->nsb[i]=0;
for (i=0;i<3;i++) svr->npb[i]=0;
for (i=0;i<3;i++) svr->buff[i]=NULL;
for (i=0;i<2;i++) svr->sbuf[i]=NULL;
for (i=0;i<3;i++) svr->pbuf[i]=NULL;
for (i=0;i<MAXSOLBUF;i++) svr->solbuf[i]=sol0;
for (i=0;i<3;i++) for (j=0;j<10;j++) svr->nmsg[i][j]=0;
for (i=0;i<3;i++) svr->ftime[i]=time0;
for (i=0;i<3;i++) svr->files[i][0]='\0';
svr->moni=NULL;
svr->tick=0;
svr->thread=0;
svr->cputime=svr->prcout=0;
if (!(svr->nav.eph =(eph_t *)malloc(sizeof(eph_t )*MAXSAT *2))||
!(svr->nav.geph=(geph_t *)malloc(sizeof(geph_t)*NSATGLO*2))||
!(svr->nav.seph=(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2)))
{
tracet(1,"rtksvrinit: malloc error\n");
return 0;
}
for (i=0;i<MAXSAT *2;i++) svr->nav.eph [i]=eph0;
for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i]=geph0;
for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i]=seph0;
svr->nav.n =MAXSAT *2;
svr->nav.ng=NSATGLO*2;
svr->nav.ns=NSATSBS*2;
for (i=0;i<3;i++) for (j=0;j<MAXOBSBUF;j++)
{
if (!(svr->obs[i][j].data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS)))
{
tracet(1,"rtksvrinit: malloc error\n");
return 0;
}
}
for (i=0;i<3;i++)
{
memset(svr->raw +i,0,sizeof(raw_t ));
memset(svr->rtcm+i,0,sizeof(rtcm_t));
}
for (i=0;i<MAXSTRRTK;i++) strinit(svr->stream+i);
initlock(&svr->lock);
return 1;
}
/* free rtk server -------------------------------------------------------------
* free rtk server
* args : rtksvr_t *svr IO rtk server
* return : none
*-----------------------------------------------------------------------------*/
void rtksvrfree(rtksvr_t *svr)
{
int i,j;
free(svr->nav.eph );
free(svr->nav.geph);
free(svr->nav.seph);
for (i=0;i<3;i++) for (j=0;j<MAXOBSBUF;j++)
{
free(svr->obs[i][j].data);
}
}
/* lock/unlock rtk server ------------------------------------------------------
* lock/unlock rtk server
* args : rtksvr_t *svr IO rtk server
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
void rtksvrlock (rtksvr_t *svr) {rtk_lock (&svr->lock);}
void rtksvrunlock(rtksvr_t *svr) {rtk_unlock(&svr->lock);}
/* 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 **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)
* 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)
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni)
{
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) 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);
for (i=0;i<3;i++)
{ /* input/log streams */
svr->nb[i]=svr->npb[i]=0;
if (!(svr->buff[i]=(unsigned char *)malloc(buffsize))||
!(svr->pbuf[i]=(unsigned char *)malloc(buffsize)))
{
tracet(1,"rtksvrstart: malloc error\n");
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;
/* initialize receiver raw and rtcm control */
//init_raw (svr->raw +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]=(unsigned char *)malloc(buffsize)))
{
tracet(1,"rtksvrstart: malloc error\n");
return 0;
}
}
/* set solution options */
for (i=0;i<2;i++)
{
svr->solopt[i]=solopt[i];
}
/* set base station position */
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 *2;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;
updatenav(&svr->nav);
/* 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]))
{
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]) 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,rtksvrthread,svr))
{
#endif
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
return 0;
}
return 1;
}
/* stop rtk server -------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* char **cmds I input stream stop commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream ephem (NULL: no command)
* return : none
*-----------------------------------------------------------------------------*/
void rtksvrstop(rtksvr_t *svr, char **cmds)
{
int i;
tracet(3,"rtksvrstop:\n");
/* write stop commands to input streams */
rtksvrlock(svr);
for (i=0;i<3;i++)
{
if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]);
}
rtksvrunlock(svr);
/* stop rtk server */
svr->state=0;
/* free rtk server thread */
#ifdef WIN32
WaitForSingleObject(svr->thread,10000);
CloseHandle(svr->thread);
#else
pthread_join(svr->thread,NULL);
#endif
}
/* open output/log stream ------------------------------------------------------
* open output/log stream
* args : rtksvr_t *svr IO rtk server
* int index I output/log stream index
* (3:solution 1,4:solution 2,5:log rover,
* 6:log base station,7:log correction)
* int str I output/log stream types (STR_???)
* char *path I output/log stream path
* solopt_t *solopt I solution options
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
const solopt_t *solopt)
{
tracet(3,"rtksvropenstr: index=%d str=%d path=%s\n",index,str,path);
if (index<3||index>7||!svr->state) return 0;
rtksvrlock(svr);
if (svr->stream[index].state>0)
{
rtksvrunlock(svr);
return 0;
}
if (!stropen(svr->stream+index,str,STR_MODE_W,path))
{
tracet(2,"stream open error: index=%d\n",index);
rtksvrunlock(svr);
return 0;
}
if (index<=4)
{
svr->solopt[index-3]=*solopt;
/* write solution header to solution stream */
writesolhead(svr->stream+index,svr->solopt+index-3);
}
rtksvrunlock(svr);
return 1;
}
/* close output/log stream -----------------------------------------------------
* close output/log stream
* args : rtksvr_t *svr IO rtk server
* int index I output/log stream index
* (3:solution 1,4:solution 2,5:log rover,
* 6:log base station,7:log correction)
* return : none
*-----------------------------------------------------------------------------*/
void rtksvrclosestr(rtksvr_t *svr, int index)
{
tracet(3,"rtksvrclosestr: index=%d\n",index);
if (index<3||index>7||!svr->state) return;
rtksvrlock(svr);
strclose(svr->stream+index);
rtksvrunlock(svr);
}
/* get observation data status -------------------------------------------------
* get current observation data status
* args : rtksvr_t *svr I rtk server
* int rcv I receiver (0:rover,1:base,2:ephem)
* gtime_t *time O time of observation data
* int *sat O satellite prn numbers
* double *az O satellite azimuth angles (rad)
* double *el O satellite elevation angles (rad)
* int **snr O satellite snr for each freq (dBHz)
* snr[i][j] = sat i freq j snr
* int *vsat O valid satellite flag
* return : number of satellites
*-----------------------------------------------------------------------------*/
int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
double *az, double *el, int **snr, int *vsat)
{
int i,j,ns;
tracet(4,"rtksvrostat: rcv=%d\n",rcv);
if (!svr->state) return 0;
rtksvrlock(svr);
ns=svr->obs[rcv][0].n;
if (ns>0)
{
*time=svr->obs[rcv][0].data[0].time;
}
for (i=0;i<ns;i++)
{
sat [i]=svr->obs[rcv][0].data[i].sat;
az [i]=svr->rtk.ssat[sat[i]-1].azel[0];
el [i]=svr->rtk.ssat[sat[i]-1].azel[1];
for (j=0;j<NFREQ;j++)
{
snr[i][j]=(int)(svr->obs[rcv][0].data[i].SNR[j]*0.25);
}
if (svr->rtk.sol.stat==SOLQ_NONE||svr->rtk.sol.stat==SOLQ_SINGLE)
{
vsat[i]=svr->rtk.ssat[sat[i]-1].vs;
}
else
{
vsat[i]=svr->rtk.ssat[sat[i]-1].vsat[0];
}
}
rtksvrunlock(svr);
return ns;
}
/* get stream status -----------------------------------------------------------
* get current stream status
* args : rtksvr_t *svr I rtk server
* int *sstat O status of streams
* char *msg O status messages
* return : none
*-----------------------------------------------------------------------------*/
void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
{
int i;
char s[MAXSTRMSG],*p=msg;
tracet(4,"rtksvrsstat:\n");
rtksvrlock(svr);
for (i=0;i<MAXSTRRTK;i++)
{
sstat[i]=strstat(svr->stream+i,s);
if (*s) p+=sprintf(p,"(%d) %s ",i+1,s);
}
rtksvrunlock(svr);
}

198
src/algorithms/libs/rtklib/rtklib_rtksvr.h Normal file
View File

@ -0,0 +1,198 @@
/*!
* \file rtklib_rtksvr.h
* \brief rtk server functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* This is a derived work from RTKLIB http://www.rtklib.com/
* The original source code at https://github.com/tomojitakasu/RTKLIB is
* released under the BSD 2-clause license with an additional exclusive clause
* that does not apply here. This additional clause is reproduced below:
*
* " The software package includes some companion executive binaries or shared
* libraries necessary to execute APs on Windows. These licenses succeed to the
* original ones of these software. "
*
* Neither the executive binaries nor the shared libraries are required by, used
* or included in GNSS-SDR.
*
* -------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
*----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_RKTSVR_H_
#define GNSS_SDR_RTKLIB_RKTSVR_H_
#include "rtklib.h"
#include "rtklib_stream.h"
#define MAXRAWLEN 4096 /* max length of receiver raw message */
#define MAXSOLBUF 256 /* max number of solution buffer */
#define MAXSBSMSG 32 /* max number of SBAS msg in RTK server */
#define MAXOBSBUF 128 /* max number of observation data buffer */
const solopt_t solopt_default = { /* defaults solution output options */
SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */
0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */
0, 0, 0, /* solstatic, sstat, trace */
{0.0, 0.0}, /* nmeaintv */
" ", "", 0 /* separator/program name */
};
const prcopt_t prcopt_default = { /* defaults processing options */
PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */
15.0*D2R, { {}, {{},{}} }, /* elmin, snrmask */
0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */
5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */
0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */
1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */
0, 0, /* rovpos, refpos */
{100.0, 100.0, 100.0}, /* eratio[] */
{100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */
{30.0, 0.03, 0.3}, /* std[] */
{1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */
5E-12, /* sclkstab */
{3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */
0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */
30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */
{}, {}, {}, /* baseline, ru, rb */
{"",""}, /* anttype */
{} , {}, {}, /* antdel, pcv, exsats */
0, 0, 0, {"",""}, {}, 0, {{},{}}, { {}, {{},{}}, {{},{}}, {}, {} }, 0, {}
};
typedef struct { /* receiver raw data control type */
gtime_t time; /* message time */
gtime_t tobs; /* observation data time */
obs_t obs; /* observation data */
obs_t obuf; /* observation data buffer */
nav_t nav; /* satellite ephemerides */
sta_t sta; /* station parameters */
int ephsat; /* sat number of update ephemeris (0:no satellite) */
sbsmsg_t sbsmsg; /* SBAS message */
char msgtype[256]; /* last message type */
unsigned char subfrm[MAXSAT][380]; /* subframe buffer */
lexmsg_t lexmsg; /* LEX message */
double lockt[MAXSAT][NFREQ+NEXOBS]; /* lock time (s) */
double icpp[MAXSAT],off[MAXSAT],icpc; /* carrier params for ss2 */
double prCA[MAXSAT],dpCA[MAXSAT]; /* L1/CA pseudrange/doppler for javad */
unsigned char halfc[MAXSAT][NFREQ+NEXOBS]; /* half-cycle add flag */
char freqn[MAXOBS]; /* frequency number for javad */
int nbyte; /* number of bytes in message buffer */
int len; /* message length (bytes) */
int iod; /* issue of data */
int tod; /* time of day (ms) */
int tbase; /* time base (0:gpst,1:utc(usno),2:glonass,3:utc(su) */
int flag; /* general purpose flag */
int outtype; /* output message type */
unsigned char buff[MAXRAWLEN]; /* message buffer */
char opt[256]; /* receiver dependent options */
double receive_time;/* RT17: Reiceve time of week for week rollover detection */
unsigned int plen; /* RT17: Total size of packet to be read */
unsigned int pbyte; /* RT17: How many packet bytes have been read so far */
unsigned int page; /* RT17: Last page number */
unsigned int reply; /* RT17: Current reply number */
int week; /* RT17: week number */
unsigned char pbuff[255+4+2]; /* RT17: Packet buffer */
} raw_t;
typedef struct { /* RTK server type */
int state; /* server state (0:stop,1:running) */
int cycle; /* processing cycle (ms) */
int nmeacycle; /* NMEA request cycle (ms) (0:no req) */
int nmeareq; /* NMEA request (0:no,1:nmeapos,2:single sol) */
double nmeapos[3]; /* NMEA request position (ecef) (m) */
int buffsize; /* input buffer size (bytes) */
int format[3]; /* input format {rov,base,corr} */
solopt_t solopt[2]; /* output solution options {sol1,sol2} */
int navsel; /* ephemeris select (0:all,1:rover,2:base,3:corr) */
int nsbs; /* number of sbas message */
int nsol; /* number of solution buffer */
rtk_t rtk; /* RTK control/result struct */
int nb [3]; /* bytes in input buffers {rov,base} */
int nsb[2]; /* bytes in soulution buffers */
int npb[3]; /* bytes in input peek buffers */
unsigned char *buff[3]; /* input buffers {rov,base,corr} */
unsigned char *sbuf[2]; /* output buffers {sol1,sol2} */
unsigned char *pbuf[3]; /* peek buffers {rov,base,corr} */
sol_t solbuf[MAXSOLBUF]; /* solution buffer */
unsigned int nmsg[3][10]; /* input message counts */
raw_t raw [3]; /* receiver raw control {rov,base,corr} */
rtcm_t rtcm[3]; /* RTCM control {rov,base,corr} */
gtime_t ftime[3]; /* download time {rov,base,corr} */
char files[3][MAXSTRPATH]; /* download paths {rov,base,corr} */
obs_t obs[3][MAXOBSBUF]; /* observation data {rov,base,corr} */
nav_t nav; /* navigation data */
sbsmsg_t sbsmsg[MAXSBSMSG]; /* SBAS message buffer */
stream_t stream[8]; /* streams {rov,base,corr,sol1,sol2,logr,logb,logc} */
stream_t *moni; /* monitor stream */
unsigned int tick; /* start tick */
thread_t thread; /* server thread */
int cputime; /* CPU time (ms) for a processing cycle */
int prcout; /* missing observation data count */
lock_t lock; /* lock flag */
} rtksvr_t;
void writesolhead(stream_t *stream, const solopt_t *solopt);
void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index);
void writesol(rtksvr_t *svr, int index);
void updatenav(nav_t *nav);
void updatefcn(rtksvr_t *svr);
void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
sbsmsg_t *sbsmsg, int index, int iobs);
int decoderaw(rtksvr_t *svr, int index);
void decodefile(rtksvr_t *svr, int index);
void *rtksvrthread(void *arg);
int rtksvrinit(rtksvr_t *svr);
void rtksvrfree(rtksvr_t *svr);
void rtksvrlock (rtksvr_t *svr);
void rtksvrunlock(rtksvr_t *svr);
int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni);
void rtksvrstop(rtksvr_t *svr, char **cmds);
int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
const solopt_t *solopt);
void rtksvrclosestr(rtksvr_t *svr, int index);
int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
double *az, double *el, int **snr, int *vsat);
void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg);
#endif

1784
src/algorithms/libs/rtklib/rtklib_solution.cc Normal file
View File

File diff suppressed because it is too large Load Diff

127
src/algorithms/libs/rtklib/rtklib_solution.h Normal file
View File

@ -0,0 +1,127 @@
/*!
* \file rtklib_solution.h
* \brief solution functions headers
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* This is a derived work from RTKLIB http://www.rtklib.com/
* The original source code at https://github.com/tomojitakasu/RTKLIB is
* released under the BSD 2-clause license with an additional exclusive clause
* that does not apply here. This additional clause is reproduced below:
*
* " The software package includes some companion executive binaries or shared
* libraries necessary to execute APs on Windows. These licenses succeed to the
* original ones of these software. "
*
* Neither the executive binaries nor the shared libraries are required by, used
* or included in GNSS-SDR.
*
* -------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*-----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_SOLUTION_H_
#define GNSS_SDR_RTKLIB_SOLUTION_H_
#include "rtklib.h"
const char *opt2sep(const solopt_t *opt);
int tonum(char *buff, const char *sep, double *v);
double sqvar(double covar);
double dmm2deg(double dmm);
void septime(double t, double *t1, double *t2, double *t3);
void soltocov(const sol_t *sol, double *P);
void covtosol(const double *P, sol_t *sol);
int decode_nmearmc(char **val, int n, sol_t *sol);
int decode_nmeagga(char **val, int n, sol_t *sol);
int decode_nmea(char *buff, sol_t *sol);
char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time);
int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol);
int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol);
int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol);
int decode_solgsi(char *buff, const solopt_t *opt, sol_t *sol);
int decode_solpos(char *buff, const solopt_t *opt, sol_t *sol);
void decode_refpos(char *buff, const solopt_t *opt, double *rb);
int decode_sol(char *buff, const solopt_t *opt, sol_t *sol, double *rb);
void decode_solopt(char *buff, solopt_t *opt);
void readsolopt(FILE *fp, solopt_t *opt);
int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint,
int qflag, const solopt_t *opt, solbuf_t *solbuf);
int readsoldata(FILE *fp, gtime_t ts, gtime_t te, double tint, int qflag,
const solopt_t *opt, solbuf_t *solbuf);
int cmpsol(const void *p1, const void *p2);
int sort_solbuf(solbuf_t *solbuf);
int readsolt(char *files[], int nfile, gtime_t ts, gtime_t te,
double tint, int qflag, solbuf_t *solbuf);
int readsol(char *files[], int nfile, solbuf_t *sol);
int addsol(solbuf_t *solbuf, const sol_t *sol);
sol_t *getsol(solbuf_t *solbuf, int index);
void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax);
void freesolbuf(solbuf_t *solbuf);
void freesolstatbuf(solstatbuf_t *solstatbuf);
int cmpsolstat(const void *p1, const void *p2);
int sort_solstat(solstatbuf_t *statbuf);
int decode_solstat(char *buff, solstat_t *stat);
void addsolstat(solstatbuf_t *statbuf, const solstat_t *stat);
int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint,
solstatbuf_t *statbuf);
int readsolstatt(char *files[], int nfile, gtime_t ts, gtime_t te,
double tint, solstatbuf_t *statbuf);
int readsolstat(char *files[], int nfile, solstatbuf_t *statbuf);
int outecef(unsigned char *buff, const char *s, const sol_t *sol,
const solopt_t *opt);
int outpos(unsigned char *buff, const char *s, const sol_t *sol, const solopt_t *opt);
int outenu(unsigned char *buff, const char *s, const sol_t *sol,
const double *rb, const solopt_t *opt);
int outnmea_rmc(unsigned char *buff, const sol_t *sol);
int outnmea_gga(unsigned char *buff, const sol_t *sol);
int outnmea_gsa(unsigned char *buff, const sol_t *sol,
const ssat_t *ssat);
int outnmea_gsv(unsigned char *buff, const sol_t *sol,
const ssat_t *ssat);
int outprcopts(unsigned char *buff, const prcopt_t *opt);
int outsolheads(unsigned char *buff, const solopt_t *opt);
int outsols(unsigned char *buff, const sol_t *sol, const double *rb,
const solopt_t *opt);
int outsolexs(unsigned char *buff, const sol_t *sol, const ssat_t *ssat,
const solopt_t *opt);
void outprcopt(FILE *fp, const prcopt_t *opt);
void outsolhead(FILE *fp, const solopt_t *opt);
void outsol(FILE *fp, const sol_t *sol, const double *rb,
const solopt_t *opt);
void outsolex(FILE *fp, const sol_t *sol, const ssat_t *ssat,
const solopt_t *opt);
#endif

2199
src/algorithms/libs/rtklib/rtklib_stream.cc Normal file
View File

File diff suppressed because it is too large Load Diff

270
src/algorithms/libs/rtklib/rtklib_stream.h Normal file
View File

@ -0,0 +1,270 @@
/*!
* \file rtklib_sbas.h
* \brief sbas functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* This is a derived work from RTKLIB http://www.rtklib.com/
* The original source code at https://github.com/tomojitakasu/RTKLIB is
* released under the BSD 2-clause license with an additional exclusive clause
* that does not apply here. This additional clause is reproduced below:
*
* " The software package includes some companion executive binaries or shared
* libraries necessary to execute APs on Windows. These licenses succeed to the
* original ones of these software. "
*
* Neither the executive binaries nor the shared libraries are required by, used
* or included in GNSS-SDR.
*
* -------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*-----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_STREAM_H_
#define GNSS_SDR_RTKLIB_STREAM_H_
#include "rtklib.h"
#include <pthread.h>
#include <netinet/in.h>
/* constants -----------------------------------------------------------------*/
#define TINTACT 200 /* period for stream active (ms) */
#define SERIBUFFSIZE 4096 /* serial buffer size (bytes) */
#define TIMETAGH_LEN 64 /* time tag file header length */
#define MAXCLI 32 /* max client connection for tcp svr */
#define MAXSTATMSG 32 /* max length of status message */
#define VER_RTKLIB "2.4.2"
#define NTRIP_AGENT "RTKLIB/" VER_RTKLIB
#define NTRIP_CLI_PORT 2101 /* default ntrip-client connection port */
#define NTRIP_SVR_PORT 80 /* default ntrip-server connection port */
#define NTRIP_MAXRSP 32768 /* max size of ntrip response */
#define NTRIP_MAXSTR 256 /* max length of mountpoint string */
#define NTRIP_RSP_OK_CLI "ICY 200 OK\r\n" /* ntrip response: client */
#define NTRIP_RSP_OK_SVR "OK\r\n" /* ntrip response: server */
#define NTRIP_RSP_SRCTBL "SOURCETABLE 200 OK\r\n" /* ntrip response: source table */
#define NTRIP_RSP_TBLEND "ENDSOURCETABLE"
#define NTRIP_RSP_HTTP "HTTP/" /* ntrip response: http */
#define NTRIP_RSP_ERROR "ERROR" /* ntrip response: error */
#define FTP_CMD "wget" /* ftp/http command */
#define FTP_TIMEOUT 30 /* ftp/http timeout (s) */
/* macros --------------------------------------------------------------------*/
#define dev_t int
#define socket_t int
#define closesocket close
#define lock_t pthread_mutex_t
#define thread_t pthread_t
#define initlock(f) pthread_mutex_init(f,NULL)
#define rtk_lock(f) pthread_mutex_lock(f)
#define rtk_unlock(f) pthread_mutex_unlock(f)
typedef struct { /* stream type */
int type; /* type (STR_???) */
int mode; /* mode (STR_MODE_?) */
int state; /* state (-1:error,0:close,1:open) */
unsigned int inb,inr; /* input bytes/rate */
unsigned int outb,outr; /* output bytes/rate */
unsigned int tick,tact; /* tick/active tick */
unsigned int inbt,outbt; /* input/output bytes at tick */
lock_t lock; /* lock flag */
void *port; /* type dependent port control struct */
char path[MAXSTRPATH]; /* stream path */
char msg [MAXSTRMSG]; /* stream message */
} stream_t;
/* type definition -----------------------------------------------------------*/
typedef struct { /* serial control type */
dev_t dev; /* serial device */
int error; /* error state */
} serial_t;
typedef struct { /* file control type */
FILE *fp; /* file pointer */
FILE *fp_tag; /* file pointer of tag file */
FILE *fp_tmp; /* temporary file pointer for swap */
FILE *fp_tag_tmp; /* temporary file pointer of tag file for swap */
char path[MAXSTRPATH]; /* file path */
char openpath[MAXSTRPATH]; /* open file path */
int mode; /* file mode */
int timetag; /* time tag flag (0:off,1:on) */
int repmode; /* replay mode (0:master,1:slave) */
int offset; /* time offset (ms) for slave */
gtime_t time; /* start time */
gtime_t wtime; /* write time */
unsigned int tick; /* start tick */
unsigned int tick_f; /* start tick in file */
unsigned int fpos; /* current file position */
double start; /* start offset (s) */
double speed; /* replay speed (time factor) */
double swapintv; /* swap interval (hr) (0: no swap) */
lock_t lock; /* lock flag */
} file_t;
typedef struct { /* tcp control type */
int state; /* state (0:close,1:wait,2:connect) */
char saddr[256]; /* address string */
int port; /* port */
struct sockaddr_in addr; /* address resolved */
socket_t sock; /* socket descriptor */
int tcon; /* reconnect time (ms) (-1:never,0:now) */
unsigned int tact; /* data active tick */
unsigned int tdis; /* disconnect tick */
} tcp_t;
typedef struct { /* tcp server type */
tcp_t svr; /* tcp server control */
tcp_t cli[MAXCLI]; /* tcp client controls */
} tcpsvr_t;
typedef struct { /* tcp cilent type */
tcp_t svr; /* tcp server control */
int toinact; /* inactive timeout (ms) (0:no timeout) */
int tirecon; /* reconnect interval (ms) (0:no reconnect) */
} tcpcli_t;
typedef struct { /* ntrip control type */
int state; /* state (0:close,1:wait,2:connect) */
int type; /* type (0:server,1:client) */
int nb; /* response buffer size */
char url[256]; /* url for proxy */
char mntpnt[256]; /* mountpoint */
char user[256]; /* user */
char passwd[256]; /* password */
char str[NTRIP_MAXSTR]; /* mountpoint string for server */
unsigned char buff[NTRIP_MAXRSP]; /* response buffer */
tcpcli_t *tcp; /* tcp client */
} ntrip_t;
typedef struct { /* ftp download control type */
int state; /* state (0:close,1:download,2:complete,3:error) */
int proto; /* protocol (0:ftp,1:http) */
int error; /* error code (0:no error,1-10:wget error, */
/* 11:no temp dir,12:uncompact error) */
char addr[1024]; /* download address */
char file[1024]; /* download file path */
char user[256]; /* user for ftp */
char passwd[256]; /* password for ftp */
char local[1024]; /* local file path */
int topts[4]; /* time options {poff,tint,toff,tretry} (s) */
gtime_t tnext; /* next retry time (gpst) */
thread_t thread; /* download thread */
} ftp_t;
serial_t *openserial(const char *path, int mode, char *msg);
void closeserial(serial_t *serial);
int readserial(serial_t *serial, unsigned char *buff, int n, char *msg);
int writeserial(serial_t *serial, unsigned char *buff, int n, char *msg);
int stateserial(serial_t *serial);
int openfile_(file_t *file, gtime_t time, char *msg);
void closefile_(file_t *file);
file_t *openfile(const char *path, int mode, char *msg);
void closefile(file_t *file);
void swapfile(file_t *file, gtime_t time, char *msg);
void swapclose(file_t *file);
int statefile(file_t *file);
int readfile(file_t *file, unsigned char *buff, int nmax, char *msg);
int writefile(file_t *file, unsigned char *buff, int n, char *msg);
void syncfile(file_t *file1, file_t *file2);
void decodetcppath(const char *path, char *addr, char *port, char *user,
char *passwd, char *mntpnt, char *str);
int errsock(void);
int setsock(socket_t sock, char *msg);
socket_t accept_nb(socket_t sock, struct sockaddr *addr, socklen_t *len);
int connect_nb(socket_t sock, struct sockaddr *addr, socklen_t len);
int recv_nb(socket_t sock, unsigned char *buff, int n);
int send_nb(socket_t sock, unsigned char *buff, int n);
int gentcp(tcp_t *tcp, int type, char *msg);
void discontcp(tcp_t *tcp, int tcon);
tcpsvr_t *opentcpsvr(const char *path, char *msg);
void closetcpsvr(tcpsvr_t *tcpsvr);
void updatetcpsvr(tcpsvr_t *tcpsvr, char *msg);
int accsock(tcpsvr_t *tcpsvr, char *msg);
int waittcpsvr(tcpsvr_t *tcpsvr, char *msg);
int readtcpsvr(tcpsvr_t *tcpsvr, unsigned char *buff, int n, char *msg);
int writetcpsvr(tcpsvr_t *tcpsvr, unsigned char *buff, int n, char *msg);
int statetcpsvr(tcpsvr_t *tcpsvr);
int consock(tcpcli_t *tcpcli, char *msg);
tcpcli_t *opentcpcli(const char *path, char *msg);
void closetcpcli(tcpcli_t *tcpcli);
int waittcpcli(tcpcli_t *tcpcli, char *msg);
int readtcpcli(tcpcli_t *tcpcli, unsigned char *buff, int n, char *msg);
int writetcpcli(tcpcli_t *tcpcli, unsigned char *buff, int n, char *msg);
int statetcpcli(tcpcli_t *tcpcli);
int encbase64(char *str, const unsigned char *byte, int n);
int reqntrip_s(ntrip_t *ntrip, char *msg);
int reqntrip_c(ntrip_t *ntrip, char *msg);
int rspntrip_s(ntrip_t *ntrip, char *msg);
int rspntrip_c(ntrip_t *ntrip, char *msg);
int waitntrip(ntrip_t *ntrip, char *msg);
ntrip_t *openntrip(const char *path, int type, char *msg);
void closentrip(ntrip_t *ntrip);
int readntrip(ntrip_t *ntrip, unsigned char *buff, int n, char *msg);
int writentrip(ntrip_t *ntrip, unsigned char *buff, int n, char *msg);
int statentrip(ntrip_t *ntrip);
void decodeftppath(const char *path, char *addr, char *file, char *user,
char *passwd, int *topts);
gtime_t nextdltime(const int *topts, int stat);
void *ftpthread(void *arg);
ftp_t *openftp(const char *path, int type, char *msg);
void closeftp(ftp_t *ftp);
int readftp(ftp_t *ftp, unsigned char *buff, int n, char *msg);
int stateftp(ftp_t *ftp);
void strinitcom(void);
void strinit(stream_t *stream);
int stropen(stream_t *stream, int type, int mode, const char *path);
void strclose(stream_t *stream);
void strsync(stream_t *stream1, stream_t *stream2);
void strlock(stream_t *stream);
void strunlock(stream_t *stream);
int strread(stream_t *stream, unsigned char *buff, int n);
int strwrite(stream_t *stream, unsigned char *buff, int n);
int strstat(stream_t *stream, char *msg);
void strsum(stream_t *stream, int *inb, int *inr, int *outb, int *outr);
void strsetopt(const int *opt);
void strsettimeout(stream_t *stream, int toinact, int tirecon);
void strsetdir(const char *dir);
void strsetproxy(const char *addr);
gtime_t strgettime(stream_t *stream);
void strsendnmea(stream_t *stream, const double *pos);
int gen_hex(const char *msg, unsigned char *buff);
void strsendcmd(stream_t *str, const char *cmd);
#endif