mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-13 16:56:52 +00:00
Code Issues Releases Wiki Activity

Merge branch 'rtklib_rtcm' into next

Browse Source 
Add more RTKLIB functions so we can connect to base stations or receive crrections (still not implemented), as well as the decoding of RTCM v2 and v3 messages
This commit is contained in:
Carles Fernandez 2017-05-13 21:09:33 +02:00
commit 0606aa81f2
19 changed files with 10091 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -60,13 +60,52 @@
#include <cmath>
#include <time.h>
#include <ctype.h>
#include <pthread.h>
#include <netinet/in.h>
#include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
#include "gnss_obs_codes.h"
const int FILEPATHSEP = '/';
/* 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)
#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 */
const int TINTACT = 200; //!< period for stream active (ms)
const int SERIBUFFSIZE = 4096; //!< serial buffer size (bytes)
const int TIMETAGH_LEN = 64; //!< time tag file header length
const int MAXCLI = 32; //!< max client connection for tcp svr
const int MAXSTATMSG = 32; //!< max length of status message
const int FTP_TIMEOUT = 30; //!< ftp/http timeout (s)
const int MAXRAWLEN = 4096; //!< max length of receiver raw message
const int MAXSOLBUF = 256; //!< max number of solution buffer
const int MAXSBSMSG = 32; //!< max number of SBAS msg in RTK server
const int MAXOBSBUF = 128; //!< max number of observation data buffer
const int FILEPATHSEP = '/';
const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m)
const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84)
@ -74,6 +113,7 @@ const double HION = 350000.0; //!< ionosphere height (m)
const double PRN_HWBIAS = 1e-6; //!< process noise of h/w bias (m/MHz/sqrt(s))
const double INT_SWAP_STAT = 86400.0; //!< swap interval of solution status file (s)
const double INT_SWAP_TRAC = 86400.0; //!< swap interval of trace file (s)
const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial
const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial
@ -110,11 +150,11 @@ const int TIMES_UTC = 1; //!< time system: utc
const int TIMES_JST = 2; //!< time system: jst
const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m)
const double ERR_BRDCI = 0.5; //!< broadcast iono model error factor
const double ERR_CBIAS = 0.3; //!< code bias error std (m)
const double REL_HUMI = 0.7; //!< relative humidity for saastamoinen model
const double GAP_RESION = 120; //!< default gap to reset ionos parameters (ep)
const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m)
const double ERR_BRDCI = 0.5; //!< broadcast iono model error factor
const double ERR_CBIAS = 0.3; //!< code bias error std (m)
const double REL_HUMI = 0.7; //!< relative humidity for saastamoinen model
const double GAP_RESION = 120; //!< default gap to reset ionos parameters (ep)
const int MAXFREQ = 7; //!< max NFREQ
@ -131,16 +171,16 @@ const int MAXPRNGPS = 32; //!< max satellite PRN number of GPS
const int NSATGPS = (MAXPRNGPS - MINPRNGPS + 1); //!< number of GPS satellites
const int NSYSGPS = 1;
const int SYS_NONE = 0x00; //!< navigation system: none
const int SYS_GPS = 0x01; //!< navigation system: GPS
const int SYS_SBS = 0x02; //!< navigation system: SBAS
const int SYS_GLO = 0x04; //!< navigation system: GLONASS
const int SYS_GAL = 0x08; //!< navigation system: Galileo
const int SYS_QZS = 0x10; //!< navigation system: QZSS
const int SYS_BDS = 0x20; //!< navigation system: BeiDou
const int SYS_IRN = 0x40; //!< navigation system: IRNS
const int SYS_LEO = 0x80; //!< navigation system: LEO
const int SYS_ALL = 0xFF; //!< navigation system: all
const int SYS_NONE = 0x00; //!< navigation system: none
const int SYS_GPS = 0x01; //!< navigation system: GPS
const int SYS_SBS = 0x02; //!< navigation system: SBAS
const int SYS_GLO = 0x04; //!< navigation system: GLONASS
const int SYS_GAL = 0x08; //!< navigation system: Galileo
const int SYS_QZS = 0x10; //!< navigation system: QZSS
const int SYS_BDS = 0x20; //!< navigation system: BeiDou
const int SYS_IRN = 0x40; //!< navigation system: IRNS
const int SYS_LEO = 0x80; //!< navigation system: LEO
const int SYS_ALL = 0xFF; //!< navigation system: all
@ -294,10 +334,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 +362,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 */
@ -987,6 +1051,192 @@ typedef struct half_cyc_tag { /* half-cycle correction list type */
} half_cyc_t;
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;
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;
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;
typedef struct { /* multi-signal-message header type */
unsigned char iod; /* issue of data station */
unsigned char time_s; /* cumulative session transmitting time */
unsigned char clk_str; /* clock steering indicator */
unsigned char clk_ext; /* external clock indicator */
unsigned char smooth; /* divergence free smoothing indicator */
unsigned char tint_s; /* soothing interval */
unsigned char nsat,nsig; /* number of satellites/signals */
unsigned char sats[64]; /* satellites */
unsigned char sigs[32]; /* signals */
unsigned char cellmask[64]; /* cell mask */
} msm_h_t;
const double chisqr[100] = { /* chi-sqr(n) (alpha=0.001) */
10.8, 13.8, 16.3, 18.5, 20.5, 22.5, 24.3, 26.1, 27.9, 29.6,
31.3, 32.9, 34.5, 36.1, 37.7, 39.3, 40.8, 42.3, 43.8, 45.3,
@ -1006,4 +1256,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

416
src/algorithms/libs/rtklib/rtklib_rtcm.cc Normal file
View File

@ -0,0 +1,416 @@
/*!
* \file rtklib_rtcm.cc
* \brief RTCM 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.
*
*
*----------------------------------------------------------------------------*/
#include "rtklib_rtcm.h"
#include "rtklib_rtcm2.h"
#include "rtklib_rtcm3.h"
#include "rtklib_rtkcmn.h"
//extern int encode_rtcm3(rtcm_t *rtcm, int type, int sync);
/* initialize rtcm control -----------------------------------------------------
* initialize rtcm control struct and reallocate memory for observation and
* ephemeris buffer in rtcm control struct
* args : rtcm_t *raw IO rtcm control struct
* return : status (1:ok,0:memory allocation error)
*-----------------------------------------------------------------------------*/
int init_rtcm(rtcm_t *rtcm)
{
gtime_t time0 = {0, 0.0};
obsd_t data0 = {{0, 0.0}, 0, 0, {0}, {0}, {0}, {0.0}, {0.0}, {0.0}};
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
ssr_t ssr0 = { {{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0' };
int i, j;
trace(3, "init_rtcm:\n");
rtcm->staid = rtcm->stah = rtcm->seqno = rtcm->outtype = 0;
rtcm->time = rtcm->time_s = time0;
rtcm->sta.name[0] = rtcm->sta.marker[0] = '\0';
rtcm->sta.antdes[0] = rtcm->sta.antsno[0] = '\0';
rtcm->sta.rectype[0] = rtcm->sta.recver[0] = rtcm->sta.recsno[0] = '\0';
rtcm->sta.antsetup = rtcm->sta.itrf = rtcm->sta.deltype = 0;
for (i = 0;i < 3;i++)
{
rtcm->sta.pos[i] = rtcm->sta.del[i] = 0.0;
}
rtcm->sta.hgt = 0.0;
rtcm->dgps = NULL;
for (i = 0;i < MAXSAT;i++)
{
rtcm->ssr[i] = ssr0;
}
rtcm->msg[0] = rtcm->msgtype[0] = rtcm->opt[0] = '\0';
for (i = 0;i < 6;i++) rtcm->msmtype[i][0] = '\0';
rtcm->obsflag = rtcm->ephsat = 0;
for (i = 0;i < MAXSAT;i++) for (j = 0;j < NFREQ+NEXOBS;j++)
{
rtcm->cp[i][j] = 0.0;
rtcm->lock[i][j] = rtcm->loss[i][j] = 0;
rtcm->lltime[i][j] = time0;
}
rtcm->nbyte = rtcm->nbit = rtcm->len = 0;
rtcm->word = 0;
for (i = 0;i < 100;i++) rtcm->nmsg2[i] = 0;
for (i = 0;i < 300;i++) rtcm->nmsg3[i] = 0;
rtcm->obs.data = NULL;
rtcm->nav.eph = NULL;
rtcm->nav.geph = NULL;
/* reallocate memory for observation and ephemris buffer */
if (!(rtcm->obs.data = (obsd_t *)malloc(sizeof(obsd_t)*MAXOBS)) ||
!(rtcm->nav.eph = (eph_t *)malloc(sizeof(eph_t )*MAXSAT)) ||
!(rtcm->nav.geph = (geph_t *)malloc(sizeof(geph_t)*MAXPRNGLO)))
{
free_rtcm(rtcm);
return 0;
}
rtcm->obs.n = 0;
rtcm->nav.n = MAXSAT;
rtcm->nav.ng = MAXPRNGLO;
for (i = 0;i < MAXOBS ;i++) rtcm->obs.data[i] = data0;
for (i = 0;i < MAXSAT ;i++) rtcm->nav.eph [i] = eph0;
for (i = 0;i < MAXPRNGLO;i++) rtcm->nav.geph[i] = geph0;
return 1;
}
/* free rtcm control ----------------------------------------------------------
* free observation and ephemris buffer in rtcm control struct
* args : rtcm_t *raw IO rtcm control struct
* return : none
*-----------------------------------------------------------------------------*/
void free_rtcm(rtcm_t *rtcm)
{
trace(3, "free_rtcm:\n");
/* free memory for observation and ephemeris buffer */
free(rtcm->obs.data); rtcm->obs.data = NULL; rtcm->obs.n = 0;
free(rtcm->nav.eph ); rtcm->nav.eph = NULL; rtcm->nav.n = 0;
free(rtcm->nav.geph); rtcm->nav.geph = NULL; rtcm->nav.ng = 0;
}
/* input rtcm 2 message from stream --------------------------------------------
* fetch next rtcm 2 message and input a message from byte stream
* args : rtcm_t *rtcm IO rtcm control struct
* unsigned char data I stream data (1 byte)
* return : status (-1: error message, 0: no message, 1: input observation data,
* 2: input ephemeris, 5: input station pos/ant parameters,
* 6: input time parameter, 7: input dgps corrections,
* 9: input special message)
* notes : before firstly calling the function, time in rtcm control struct has
* to be set to the approximate time within 1/2 hour in order to resolve
* ambiguity of time in rtcm messages.
* supported msgs RTCM ver.2: 1,3,9,14,16,17,18,19,22
* refer [1] for RTCM ver.2
*-----------------------------------------------------------------------------*/
int input_rtcm2(rtcm_t *rtcm, unsigned char data)
{
unsigned char preamb;
int i;
trace(5, "input_rtcm2: data=%02x\n", data);
if ((data & 0xC0) != 0x40) return 0; /* ignore if upper 2bit != 01 */
for (i = 0; i < 6; i++, data >>= 1)
{ /* decode 6-of-8 form */
rtcm->word = (rtcm->word << 1) + (data & 1);
/* synchronize frame */
if (rtcm->nbyte == 0)
{
preamb = (unsigned char)(rtcm->word>>22);
if (rtcm->word & 0x40000000) preamb ^= 0xFF; /* decode preamble */
if (preamb != RTCM2PREAMB) continue;
/* check parity */
if (!decode_word(rtcm->word, rtcm->buff)) continue;
rtcm->nbyte = 3; rtcm->nbit = 0;
continue;
}
if (++rtcm->nbit < 30) continue; else rtcm->nbit = 0;
/* check parity */
if (!decode_word(rtcm->word, rtcm->buff+rtcm->nbyte))
{
trace(2, "rtcm2 partity error: i=%d word=%08x\n", i, rtcm->word);
rtcm->nbyte = 0; rtcm->word &= 0x3;
continue;
}
rtcm->nbyte += 3;
if (rtcm->nbyte == 6) rtcm->len = (rtcm->buff[5]>>3)*3+6;
if (rtcm->nbyte < rtcm->len) continue;
rtcm->nbyte = 0; rtcm->word &= 0x3;
/* decode rtcm2 message */
return decode_rtcm2(rtcm);
}
return 0;
}
/* input rtcm 3 message from stream --------------------------------------------
* fetch next rtcm 3 message and input a message from byte stream
* args : rtcm_t *rtcm IO rtcm control struct
* unsigned char data I stream data (1 byte)
* return : status (-1: error message, 0: no message, 1: input observation data,
* 2: input ephemeris, 5: input station pos/ant parameters,
* 10: input ssr messages)
* notes : before firstly calling the function, time in rtcm control struct has
* to be set to the approximate time within 1/2 week in order to resolve
* ambiguity of time in rtcm messages.
*
* to specify input options, set rtcm->opt to the following option
* strings separated by spaces.
*
* -EPHALL : input all ephemerides
* -STA=nnn : input only message with STAID=nnn
* -GLss : select signal ss for GPS MSM (ss=1C,1P,...)
* -RLss : select signal ss for GLO MSM (ss=1C,1P,...)
* -ELss : select signal ss for GAL MSM (ss=1C,1B,...)
* -JLss : select signal ss for QZS MSM (ss=1C,2C,...)
* -CLss : select signal ss for BDS MSM (ss=2I,7I,...)
*
* supported RTCM 3 messages
* (ref [2][3][4][5][6][7][8][9][10][11][12][13][14][15])
*
* TYPE GPS GLOASS GALILEO QZSS BEIDOU SBAS
* ----------------------------------------------------------------------
* OBS C-L1 : 1001~ 1009~ - - - -
* F-L1 : 1002 1010 - - - -
* C-L12 : 1003~ 1011~ - - - -
* F-L12 : 1004 1012 - - - -
*
* NAV : 1019 1020 1045* 1044* 1047* -
* - - 1046* - - -
*
* MSM 1 : 1071~ 1081~ 1091~ 1111*~ 1121*~ 1101*~
* 2 : 1072~ 1082~ 1092~ 1112*~ 1122*~ 1102*~
* 3 : 1073~ 1083~ 1093~ 1113*~ 1123*~ 1103*~
* 4 : 1074 1084 1094 1114* 1124* 1104*
* 5 : 1075 1085 1095 1115* 1125* 1105*
* 6 : 1076 1086 1096 1116* 1126* 1106*
* 7 : 1077 1087 1097 1117* 1127* 1107*
*
* SSR OBT : 1057 1063 1240* 1246* 1258* -
* CLK : 1058 1064 1241* 1247* 1259* -
* BIAS : 1059 1065 1242* 1248* 1260* -
* OBTCLK: 1060 1066 1243* 1249* 1261* -
* URA : 1061 1067 1244* 1250* 1262* -
* HRCLK : 1062 1068 1245* 1251* 1263* -
*
* ANT INFO : 1005 1006 1007 1008 1033
* ----------------------------------------------------------------------
* (* draft, ~ only encode)
*
* for MSM observation data with multiple signals for a frequency,
* a signal is selected according to internal priority. to select
* a specified signal, use the input options.
*
* rtcm3 message format:
* +----------+--------+-----------+--------------------+----------+
* | preamble | 000000 | length | data message | parity |
* +----------+--------+-----------+--------------------+----------+
* |<-- 8 --->|<- 6 -->|<-- 10 --->|<--- length x 8 --->|<-- 24 -->|
*
*-----------------------------------------------------------------------------*/
int input_rtcm3(rtcm_t *rtcm, unsigned char data)
{
trace(5, "input_rtcm3: data=%02x\n", data);
/* synchronize frame */
if (rtcm->nbyte == 0)
{
if (data != RTCM3PREAMB) return 0;
rtcm->buff[rtcm->nbyte++] = data;
return 0;
}
rtcm->buff[rtcm->nbyte++] = data;
if (rtcm->nbyte == 3)
{
rtcm->len = getbitu(rtcm->buff, 14, 10)+3; /* length without parity */
}
if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len+3) return 0;
rtcm->nbyte = 0;
/* check parity */
if (rtk_crc24q(rtcm->buff, rtcm->len) != getbitu(rtcm->buff, rtcm->len*8, 24))
{
trace(2, "rtcm3 parity error: len=%d\n", rtcm->len);
return 0;
}
/* decode rtcm3 message */
return decode_rtcm3(rtcm);
}
/* input rtcm 2 message from file ----------------------------------------------
* fetch next rtcm 2 message and input a messsage from file
* args : rtcm_t *rtcm IO rtcm control struct
* FILE *fp I file pointer
* return : status (-2: end of file, -1...10: same as above)
* notes : same as above
*-----------------------------------------------------------------------------*/
int input_rtcm2f(rtcm_t *rtcm, FILE *fp)
{
int i, data = 0, ret;
trace(4, "input_rtcm2f: data=%02x\n", data);
for (i = 0;i < 4096;i++)
{
if ((data = fgetc(fp)) == EOF) return -2;
if ((ret = input_rtcm2(rtcm, (unsigned char)data))) return ret;
}
return 0; /* return at every 4k bytes */
}
/* input rtcm 3 message from file ----------------------------------------------
* fetch next rtcm 3 message and input a messsage from file
* args : rtcm_t *rtcm IO rtcm control struct
* FILE *fp I file pointer
* return : status (-2: end of file, -1...10: same as above)
* notes : same as above
*-----------------------------------------------------------------------------*/
int input_rtcm3f(rtcm_t *rtcm, FILE *fp)
{
int i, data = 0, ret;
trace(4, "input_rtcm3f: data=%02x\n", data);
for (i = 0;i < 4096;i++)
{
if ((data = fgetc(fp)) == EOF) return -2;
if ((ret = input_rtcm3(rtcm, (unsigned char)data))) return ret;
}
return 0; /* return at every 4k bytes */
}
/* generate rtcm 2 message -----------------------------------------------------
* generate rtcm 2 message
* args : rtcm_t *rtcm IO rtcm control struct
* int type I message type
* int sync I sync flag (1:another message follows)
* return : status (1:ok,0:error)
*-----------------------------------------------------------------------------*/
int gen_rtcm2(rtcm_t *rtcm, int type, int sync)
{
trace(4, "gen_rtcm2: type=%d sync=%d\n", type, sync);
rtcm->nbit = rtcm->len = rtcm->nbyte = 0;
/* not yet implemented */
return 0;
}
///* generate rtcm 3 message -----------------------------------------------------
// * generate rtcm 3 message
// * args : rtcm_t *rtcm IO rtcm control struct
// * int type I message type
// * int sync I sync flag (1:another message follows)
// * return : status (1:ok,0:error)
// *-----------------------------------------------------------------------------*/
//int gen_rtcm3(rtcm_t *rtcm, int type, int sync)
//{
// unsigned int crc;
// int i = 0;
//
// trace(4, "gen_rtcm3: type=%d sync=%d\n", type, sync);
//
// rtcm->nbit = rtcm->len = rtcm->nbyte = 0;
//
// /* set preamble and reserved */
// setbitu(rtcm->buff, i, 8, RTCM3PREAMB); i += 8;
// setbitu(rtcm->buff, i, 6, 0 ); i += 6;
// setbitu(rtcm->buff, i, 10, 0 ); i += 10;
//
// /* encode rtcm 3 message body */
// if (!encode_rtcm3(rtcm, type, sync)) return 0;
//
// /* padding to align 8 bit boundary */
// for (i = rtcm->nbit;i%8;i++)
// {
// setbitu(rtcm->buff, i, 1, 0);
// }
// /* message length (header+data) (bytes) */
// if ((rtcm->len = i/8) >= 3+1024)
// {
// trace(2, "generate rtcm 3 message length error len=%d\n", rtcm->len-3);
// rtcm->nbit = rtcm->len = 0;
// return 0;
// }
// /* message length without header and parity */
// setbitu(rtcm->buff, 14, 10, rtcm->len-3);
//
// /* crc-24q */
// crc = rtk_crc24q(rtcm->buff, rtcm->len);
// setbitu(rtcm->buff, i, 24, crc);
//
// /* length total (bytes) */
// rtcm->nbyte = rtcm->len+3;
//
// return 1;
//}

74
src/algorithms/libs/rtklib/rtklib_rtcm.h Normal file
View File

@ -0,0 +1,74 @@
/*!
* \file rtklib_rtcm.h
* \brief RTCM 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.
*
*
*----------------------------------------------------------------------------*/
#include "rtklib.h"
#ifndef GNSS_SDR_RTKLIB_RTCM_H_
#define GNSS_SDR_RTKLIB_RTCM_H_
#define RTCM2PREAMB 0x66 /* rtcm ver.2 frame preamble */
#define RTCM3PREAMB 0xD3 /* rtcm ver.3 frame preamble */
int init_rtcm(rtcm_t *rtcm);
void free_rtcm(rtcm_t *rtcm);
int input_rtcm2(rtcm_t *rtcm, unsigned char data);
int input_rtcm3(rtcm_t *rtcm, unsigned char data);
int input_rtcm2f(rtcm_t *rtcm, FILE *fp);
int input_rtcm3f(rtcm_t *rtcm, FILE *fp);
int gen_rtcm2(rtcm_t *rtcm, int type, int sync);
//int gen_rtcm3(rtcm_t *rtcm, int type, int sync);
#endif

550
src/algorithms/libs/rtklib/rtklib_rtcm2.cc Normal file
View File

@ -0,0 +1,550 @@
/*!
* \file rtklib_rtcm2.cc
* \brief RTCM functions for v2
* \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.
*
*
*----------------------------------------------------------------------------*/
#include "rtklib_rtcm2.h"
#include "rtklib_rtcm3.h"
#include "rtklib_rtkcmn.h"
/* adjust hourly rollover of rtcm 2 time -------------------------------------*/
void adjhour(rtcm_t *rtcm, double zcnt)
{
double tow, hour, sec;
int week;
/* if no time, get cpu time */
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
tow = time2gpst(rtcm->time, &week);
hour = floor(tow/3600.0);
sec = tow-hour*3600.0;
if (zcnt<sec-1800.0) zcnt += 3600.0;
else if (zcnt>sec+1800.0) zcnt-=3600.0;
rtcm->time = gpst2time(week, hour*3600+zcnt);
}
/* get observation data index ------------------------------------------------*/
int obsindex(obs_t *obs, gtime_t time, int sat)
{
int i, j;
for (i = 0; i<obs->n; i++)
{
if (obs->data[i].sat == sat) return i; /* field already exists */
}
if (i >= MAXOBS) return -1; /* overflow */
/* add new field */
obs->data[i].time = time;
obs->data[i].sat = sat;
for (j = 0; j<NFREQ; j++)
{
obs->data[i].L[j] = obs->data[i].P[j] = 0.0;
obs->data[i].D[j] = 0.0;
obs->data[i].SNR[j] = obs->data[i].LLI[j] = obs->data[i].code[j] = 0;
}
obs->n++;
return i;
}
/* decode type 1/9: differential gps correction/partial correction set -------*/
int decode_type1(rtcm_t *rtcm)
{
int i = 48, fact, udre, prn, sat, iod;
double prc, rrc;
trace(4, "decode_type1: len=%d\n", rtcm->len);
while (i+40 <= rtcm->len*8)
{
fact = getbitu(rtcm->buff, i, 1); i += 1;
udre = getbitu(rtcm->buff, i, 2); i += 2;
prn = getbitu(rtcm->buff, i, 5); i += 5;
prc = getbits(rtcm->buff, i, 16); i += 16;
rrc = getbits(rtcm->buff, i, 8); i += 8;
iod = getbits(rtcm->buff, i, 8); i += 8;
if (prn == 0) prn = 32;
if (prc == 0x80000000||rrc == 0xFFFF8000)
{
trace(2, "rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n", prn);
continue;
}
if (rtcm->dgps)
{
sat = satno(SYS_GPS, prn);
rtcm->dgps[sat-1].t0 = rtcm->time;
rtcm->dgps[sat-1].prc = prc*(fact?0.32:0.02);
rtcm->dgps[sat-1].rrc = rrc*(fact?0.032:0.002);
rtcm->dgps[sat-1].iod = iod;
rtcm->dgps[sat-1].udre = udre;
}
}
return 7;
}
/* decode type 3: reference station parameter --------------------------------*/
int decode_type3(rtcm_t *rtcm)
{
int i = 48;
trace(4, "decode_type3: len=%d\n", rtcm->len);
if (i+96 <= rtcm->len*8)
{
rtcm->sta.pos[0] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
rtcm->sta.pos[1] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
rtcm->sta.pos[2] = getbits(rtcm->buff, i, 32)*0.01;
}
else
{
trace(2, "rtcm2 3 length error: len=%d\n", rtcm->len);
return -1;
}
return 5;
}
/* decode type 14: gps time of week ------------------------------------------*/
int decode_type14(rtcm_t *rtcm)
{
double zcnt;
int i = 48, week, hour, leaps;
trace(4, "decode_type14: len=%d\n", rtcm->len);
zcnt = getbitu(rtcm->buff, 24, 13);
if (i+24 <= rtcm->len*8)
{
week = getbitu(rtcm->buff, i, 10); i += 10;
hour = getbitu(rtcm->buff, i, 8); i += 8;
leaps = getbitu(rtcm->buff, i, 6);
}
else
{
trace(2, "rtcm2 14 length error: len=%d\n", rtcm->len);
return -1;
}
week = adjgpsweek(week);
rtcm->time = gpst2time(week, hour*3600.0+zcnt*0.6);
rtcm->nav.leaps = leaps;
return 6;
}
/* decode type 16: gps special message ---------------------------------------*/
int decode_type16(rtcm_t *rtcm)
{
int i = 48, n = 0;
trace(4, "decode_type16: len=%d\n", rtcm->len);
while (i+8 <= rtcm->len*8 && n<90)
{
rtcm->msg[n++] = getbitu(rtcm->buff, i, 8); i += 8;
}
rtcm->msg[n] = '\0';
trace(3, "rtcm2 16 message: %s\n", rtcm->msg);
return 9;
}
/* decode type 17: gps ephemerides -------------------------------------------*/
int decode_type17(rtcm_t *rtcm)
{
eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
double toc, sqrtA;
int i = 48, week, prn, sat;
trace(4, "decode_type17: len=%d\n", rtcm->len);
if (i+480 <= rtcm->len*8)
{
week = getbitu(rtcm->buff, i, 10); i += 10;
eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14;
eph.iode = getbitu(rtcm->buff, i, 8); i += 8;
toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16;
eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16;
eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8;
eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16;
eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32;
eph.cus = getbits(rtcm->buff, i, 16); i += 16;
sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32;
eph.toes = getbitu(rtcm->buff, i, 16); i += 16;
eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24;
eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.iodc = getbitu(rtcm->buff, i, 10); i += 10;
eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22;
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8;
eph.code = getbitu(rtcm->buff, i, 2); i += 2;
eph.sva = getbitu(rtcm->buff, i, 4); i += 4;
eph.svh = getbitu(rtcm->buff, i, 6); i += 6;
eph.flag = getbitu(rtcm->buff, i, 1);
}
else
{
trace(2, "rtcm2 17 length error: len=%d\n", rtcm->len);
return -1;
}
if (prn == 0) prn = 32;
sat = satno(SYS_GPS, prn);
eph.sat = sat;
eph.week = adjgpsweek(week);
eph.toe = gpst2time(eph.week, eph.toes);
eph.toc = gpst2time(eph.week, toc);
eph.ttr = rtcm->time;
eph.A = sqrtA*sqrtA;
rtcm->nav.eph[sat-1] = eph;
rtcm->ephsat = sat;
return 2;
}
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
int decode_type18(rtcm_t *rtcm)
{
gtime_t time;
double usec, cp, tt;
int i = 48, index, freq, sync = 1, code, sys, prn, sat, loss;
trace(4, "decode_type18: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
{
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
usec = getbitu(rtcm->buff, i, 20); i += 20;
}
else
{
trace(2, "rtcm2 18 length error: len=%d\n", rtcm->len);
return -1;
}
if (freq&0x1)
{
trace(2, "rtcm2 18 not supported frequency: freq=%d\n", freq);
return -1;
}
freq>>= 1;
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
{
sync = getbitu(rtcm->buff, i, 1); i += 1;
code = getbitu(rtcm->buff, i, 1); i += 1;
sys = getbitu(rtcm->buff, i, 1); i += 1;
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
loss = getbitu(rtcm->buff, i, 5); i += 5;
cp = getbits(rtcm->buff, i, 32); i += 32;
if (prn == 0) prn = 32;
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
{
trace(2, "rtcm2 18 satellite number error: sys=%d prn=%d\n", sys, prn);
continue;
}
time = timeadd(rtcm->time, usec*1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag||fabs(tt)>1E-9)
{
rtcm->obs.n = rtcm->obsflag = 0;
}
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
{
rtcm->obs.data[index].L[freq] = -cp/256.0;
rtcm->obs.data[index].LLI[freq] = rtcm->loss[sat-1][freq] != loss;
rtcm->obs.data[index].code[freq] =
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
rtcm->loss[sat-1][freq] = loss;
}
}
rtcm->obsflag = !sync;
return sync?0:1;
}
/* decode type 19: rtk uncorrected pseudorange -------------------------------*/
int decode_type19(rtcm_t *rtcm)
{
gtime_t time;
double usec, pr, tt;
int i = 48, index, freq, sync = 1, code, sys, prn, sat;
trace(4, "decode_type19: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
{
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
usec = getbitu(rtcm->buff, i, 20); i += 20;
}
else
{
trace(2, "rtcm2 19 length error: len=%d\n", rtcm->len);
return -1;
}
if (freq&0x1)
{
trace(2, "rtcm2 19 not supported frequency: freq=%d\n", freq);
return -1;
}
freq>>=1;
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
{
sync = getbitu(rtcm->buff, i, 1); i += 1;
code = getbitu(rtcm->buff, i, 1); i += 1;
sys = getbitu(rtcm->buff, i, 1); i += 1;
prn = getbitu(rtcm->buff, i, 5); i += 5+8;
pr = getbitu(rtcm->buff, i, 32); i += 32;
if (prn == 0) prn = 32;
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
{
trace(2, "rtcm2 19 satellite number error: sys=%d prn=%d\n", sys, prn);
continue;
}
time = timeadd(rtcm->time, usec*1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag||fabs(tt)>1E-9)
{
rtcm->obs.n = rtcm->obsflag = 0;
}
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
{
rtcm->obs.data[index].P[freq] = pr*0.02;
rtcm->obs.data[index].code[freq] =
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
}
}
rtcm->obsflag = !sync;
return sync?0:1;
}
/* decode type 22: extended reference station parameter ----------------------*/
int decode_type22(rtcm_t *rtcm)
{
double del[2][3] = {{0}}, hgt = 0.0;
int i = 48, j, noh;
trace(4, "decode_type22: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
{
del[0][0] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
del[0][1] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
del[0][2] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
}
else
{
trace(2, "rtcm2 22 length error: len=%d\n", rtcm->len);
return -1;
}
if (i+24 <= rtcm->len*8)
{
i += 5; noh = getbits(rtcm->buff, i, 1); i += 1;
hgt = noh?0.0:getbitu(rtcm->buff, i, 18)/25600.0;
i += 18;
}
if (i+24 <= rtcm->len*8)
{
del[1][0] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
del[1][1] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
del[1][2] = getbits(rtcm->buff, i, 8)/1600.0;
}
rtcm->sta.deltype = 1; /* xyz */
for (j = 0; j<3; j++) rtcm->sta.del[j] = del[0][j];
rtcm->sta.hgt = hgt;
return 5;
}
/* decode type 23: antenna type definition record ----------------------------*/
int decode_type23(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 24: antenna reference point (arp) -----------------------------*/
int decode_type24(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 31: differential glonass correction ---------------------------*/
int decode_type31(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 32: differential glonass reference station parameters ---------*/
int decode_type32(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 34: glonass partial differential correction set ---------------*/
int decode_type34(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 36: glonass special message -----------------------------------*/
int decode_type36(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 37: gnss system time offset -----------------------------------*/
int decode_type37(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode type 59: proprietary message ---------------------------------------*/
int decode_type59(rtcm_t *rtcm __attribute((unused)))
{
return 0;
}
/* decode rtcm ver.2 message -------------------------------------------------*/
int decode_rtcm2(rtcm_t *rtcm)
{
double zcnt;
int staid, seqno, stah, ret = 0, type = getbitu(rtcm->buff, 8, 6);
trace(3, "decode_rtcm2: type=%2d len=%3d\n", type, rtcm->len);
if ((zcnt = getbitu(rtcm->buff, 24, 13)*0.6) >= 3600.0)
{
trace(2, "rtcm2 modified z-count error: zcnt=%.1f\n", zcnt);
return -1;
}
adjhour(rtcm, zcnt);
staid = getbitu(rtcm->buff, 14, 10);
seqno = getbitu(rtcm->buff, 37, 3);
stah = getbitu(rtcm->buff, 45, 3);
if (seqno-rtcm->seqno != 1 && seqno-rtcm->seqno != -7)
{
trace(2, "rtcm2 message outage: seqno=%d->%d\n", rtcm->seqno, seqno);
}
rtcm->seqno = seqno;
rtcm->stah = stah;
if (rtcm->outtype)
{
sprintf(rtcm->msgtype, "RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
type, rtcm->len, zcnt, staid, seqno);
}
if (type == 3||type == 22||type == 23||type == 24)
{
if (rtcm->staid != 0 && staid != rtcm->staid)
{
trace(2, "rtcm2 station id changed: %d->%d\n", rtcm->staid, staid);
}
rtcm->staid = staid;
}
if (rtcm->staid != 0 && staid != rtcm->staid)
{
trace(2, "rtcm2 station id invalid: %d %d\n", staid, rtcm->staid);
return -1;
}
switch (type)
{
case 1: ret = decode_type1 (rtcm); break;
case 3: ret = decode_type3 (rtcm); break;
case 9: ret = decode_type1 (rtcm); break;
case 14: ret = decode_type14(rtcm); break;
case 16: ret = decode_type16(rtcm); break;
case 17: ret = decode_type17(rtcm); break;
case 18: ret = decode_type18(rtcm); break;
case 19: ret = decode_type19(rtcm); break;
case 22: ret = decode_type22(rtcm); break;
case 23: ret = decode_type23(rtcm); break; /* not supported */
case 24: ret = decode_type24(rtcm); break; /* not supported */
case 31: ret = decode_type31(rtcm); break; /* not supported */
case 32: ret = decode_type32(rtcm); break; /* not supported */
case 34: ret = decode_type34(rtcm); break; /* not supported */
case 36: ret = decode_type36(rtcm); break; /* not supported */
case 37: ret = decode_type37(rtcm); break; /* not supported */
case 59: ret = decode_type59(rtcm); break; /* not supported */
}
if (ret >= 0)
{
if (1 <= type && type <= 99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++;
}
return ret;
}

80
src/algorithms/libs/rtklib/rtklib_rtcm2.h Normal file
View File

@ -0,0 +1,80 @@
/*!
* \file rtklib_rtcm2.h
* \brief RTCM v2 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_RTCM2_H_
#define GNSS_SDR_RTKLIB_RTCM2_H_
#include "rtklib.h"
void adjhour(rtcm_t *rtcm, double zcnt);
int obsindex(obs_t *obs, gtime_t time, int sat);
int decode_type1(rtcm_t *rtcm);
int decode_type3(rtcm_t *rtcm);
int decode_type14(rtcm_t *rtcm);
int decode_type16(rtcm_t *rtcm);
int decode_type17(rtcm_t *rtcm);
int decode_type18(rtcm_t *rtcm);
int decode_type19(rtcm_t *rtcm);
int decode_type22(rtcm_t *rtcm);
int decode_type23(rtcm_t *rtcm);
int decode_type24(rtcm_t *rtcm);
int decode_type31(rtcm_t *rtcm);
int decode_type32(rtcm_t *rtcm);
int decode_type34(rtcm_t *rtcm);
int decode_type36(rtcm_t *rtcm);
int decode_type37(rtcm_t *rtcm);
int decode_type59(rtcm_t *rtcm);
int decode_rtcm2(rtcm_t *rtcm);
#endif

2740
src/algorithms/libs/rtklib/rtklib_rtcm3.cc Normal file
View File

File diff suppressed because it is too large Load Diff

305
src/algorithms/libs/rtklib/rtklib_rtcm3.h Normal file
View File

@ -0,0 +1,305 @@
/*!
* \file rtklib_rtcm3.h
* \brief RTCM v3 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_RTCM3_H_
#define GNSS_SDR_RTKLIB_RTCM3_H_
#include "rtklib.h"
/* constants -----------------------------------------------------------------*/
const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */
const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */
const double RANGE_MS = SPEED_OF_LIGHT * 0.001; /* range in 1 ms */
/* msm signal id table -------------------------------------------------------*/
const char *msm_sig_gps[32] = {
/* GPS: ref [13] table 3.5-87, ref [14][15] table 3.5-91 */
"" , "1C", "1P", "1W", "1Y", "1M", "" , "2C", "2P", "2W", "2Y", "2M", /* 1-12 */
"" , "" , "2S", "2L", "2X", "" , "" , "" , "" , "5I", "5Q", "5X", /* 13-24 */
"" , "" , "" , "" , "" , "1S", "1L", "1X" /* 25-32 */
};
const char *msm_sig_glo[32] = {
/* GLONASS: ref [13] table 3.5-93, ref [14][15] table 3.5-97 */
"" , "1C", "1P", "" , "" , "" , "" , "2C", "2P", "" , "3I", "3Q",
"3X", "" , "" , "" , "" , "" , "" , "" , "" , "" , "" , "" ,
"" , "" , "" , "" , "" , "" , "" , ""
};
const char *msm_sig_gal[32] = {
/* Galileo: ref [15] table 3.5-100 */
"" , "1C", "1A", "1B", "1X", "1Z", "" , "6C", "6A", "6B", "6X", "6Z",
"" , "7I", "7Q", "7X", "" , "8I", "8Q", "8X", "" , "5I", "5Q", "5X",
"" , "" , "" , "" , "" , "" , "" , ""
};
const char *msm_sig_qzs[32] = {
/* QZSS: ref [15] table 3.5-103 */
"" , "1C", "" , "" , "" , "" , "" , "" , "6S", "6L", "6X", "" ,
"" , "" , "2S", "2L", "2X", "" , "" , "" , "" , "5I", "5Q", "5X",
"" , "" , "" , "" , "" , "1S", "1L", "1X"
};
const char *msm_sig_sbs[32] = {
/* SBAS: ref [13] table 3.5-T+005 */
"" , "1C", "" , "" , "" , "" , "" , "" , "" , "" , "" , "" ,
"" , "" , "" , "" , "" , "" , "" , "" , "" , "5I", "5Q", "5X",
"" , "" , "" , "" , "" , "" , "" , ""
};
const char *msm_sig_cmp[32] = {
/* BeiDou: ref [15] table 3.5-106 */
"" , "1I", "1Q", "1X", "" , "" , "" , "6I", "6Q", "6X", "" , "" ,
"" , "7I", "7Q", "7X", "" , "" , "" , "" , "" , "" , "" , "" ,
"" , "" , "" , "" , "" , "" , "" , ""
};
/* ssr update intervals ------------------------------------------------------*/
const double ssrudint[16] = {
1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 900, 1800, 3600, 7200, 10800
};
/* ssr 3 and 7 signal and tracking mode ids ----------------------------------*/
const int codes_gps[] = {
CODE_L1C, CODE_L1P, CODE_L1W, CODE_L1Y, CODE_L1M, CODE_L2C, CODE_L2D, CODE_L2S,
CODE_L2L, CODE_L2X, CODE_L2P, CODE_L2W, CODE_L2Y, CODE_L2M, CODE_L5I, CODE_L5Q,
CODE_L5X
};
const int codes_glo[] = {
CODE_L1C, CODE_L1P, CODE_L2C, CODE_L2P
};
const int codes_gal[] = {
CODE_L1A, CODE_L1B, CODE_L1C, CODE_L1X, CODE_L1Z, CODE_L5I, CODE_L5Q, CODE_L5X,
CODE_L7I, CODE_L7Q, CODE_L7X, CODE_L8I, CODE_L8Q, CODE_L8X, CODE_L6A, CODE_L6B,
CODE_L6C, CODE_L6X, CODE_L6Z
};
const int codes_qzs[] = {
CODE_L1C, CODE_L1S, CODE_L1L, CODE_L2S, CODE_L2L, CODE_L2X, CODE_L5I, CODE_L5Q,
CODE_L5X, CODE_L6S, CODE_L6L, CODE_L6X, CODE_L1X
};
const int codes_bds[] = {
CODE_L1I, CODE_L1Q, CODE_L1X, CODE_L7I, CODE_L7Q, CODE_L7X, CODE_L6I, CODE_L6Q,
CODE_L6X
};
const int codes_sbs[] = {
CODE_L1C, CODE_L5I, CODE_L5Q, CODE_L5X
};
double getbitg(const unsigned char *buff, int pos, int len);
void adjweek(rtcm_t *rtcm, double tow);
int adjbdtweek(int week);
void adjday_glot(rtcm_t *rtcm, double tod);
double adjcp(rtcm_t *rtcm, int sat, int freq, double cp);
int lossoflock(rtcm_t *rtcm, int sat, int freq, int lock);
unsigned char snratio(double snr);
int obsindex(obs_t *obs, gtime_t time, int sat);
int test_staid(rtcm_t *rtcm, int staid);
int decode_head1001(rtcm_t *rtcm, int *sync);
int decode_type1001(rtcm_t *rtcm);
int decode_type1002(rtcm_t *rtcm);
int decode_type1003(rtcm_t *rtcm);
int decode_type1004(rtcm_t *rtcm);
double getbits_38(const unsigned char *buff, int pos);
int decode_type1005(rtcm_t *rtcm);
int decode_type1006(rtcm_t *rtcm);
int decode_type1007(rtcm_t *rtcm);
int decode_type1008(rtcm_t *rtcm);
int decode_head1009(rtcm_t *rtcm, int *sync);
int decode_type1009(rtcm_t *rtcm);
int decode_type1010(rtcm_t *rtcm);
int decode_type1011(rtcm_t *rtcm);
int decode_type1012(rtcm_t *rtcm);
int decode_type1013(rtcm_t *rtcm);
int decode_type1019(rtcm_t *rtcm);
int decode_type1020(rtcm_t *rtcm);
int decode_type1021(rtcm_t *rtcm);
int decode_type1022(rtcm_t *rtcm);
int decode_type1023(rtcm_t *rtcm);
int decode_type1024(rtcm_t *rtcm);
int decode_type1025(rtcm_t *rtcm);
int decode_type1026(rtcm_t *rtcm);
int decode_type1027(rtcm_t *rtcm);
int decode_type1029(rtcm_t *rtcm);
int decode_type1030(rtcm_t *rtcm);
int decode_type1031(rtcm_t *rtcm);
int decode_type1032(rtcm_t *rtcm);
int decode_type1033(rtcm_t *rtcm);
int decode_type1034(rtcm_t *rtcm);
int decode_type1035(rtcm_t *rtcm);
int decode_type1037(rtcm_t *rtcm);
int decode_type1038(rtcm_t *rtcm);
int decode_type1039(rtcm_t *rtcm);
int decode_type1044(rtcm_t *rtcm);
int decode_type1045(rtcm_t *rtcm);
int decode_type1046(rtcm_t *rtcm);
int decode_type1047(rtcm_t *rtcm);
int decode_type1063(rtcm_t *rtcm);
int decode_ssr1_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *refd, int *hsize);
int decode_ssr2_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *hsize);
int decode_ssr7_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *dispe, int *mw, int *hsize);
int decode_ssr1(rtcm_t *rtcm, int sys);
int decode_ssr2(rtcm_t *rtcm, int sys);
int decode_ssr3(rtcm_t *rtcm, int sys);
int decode_ssr4(rtcm_t *rtcm, int sys);
int decode_ssr5(rtcm_t *rtcm, int sys);
int decode_ssr6(rtcm_t *rtcm, int sys);
int decode_ssr7(rtcm_t *rtcm, int sys);
void sigindex(int sys, const unsigned char *code, const int *freq, int n,
const char *opt, int *ind);
void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
const double *pr, const double *cp, const double *rr,
const double *rrf, const double *cnr, const int *lock,
const int *ex, const int *half);
int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
msm_h_t *h, int *hsize);
int decode_msm0(rtcm_t *rtcm, int sys);
int decode_msm4(rtcm_t *rtcm, int sys);
int decode_msm5(rtcm_t *rtcm, int sys);
int decode_msm6(rtcm_t *rtcm, int sys);
int decode_msm7(rtcm_t *rtcm, int sys);
int decode_type1230(rtcm_t *rtcm);
int decode_rtcm3(rtcm_t *rtcm);
#endif

153
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)
{
@ -1701,6 +1703,13 @@ void deg2dms(double deg, double *dms, int ndec)
dms[0]*=sign;
}
void deg2dms(double deg, double *dms)
{
double sign=deg<0.0?-1.0:1.0,a=fabs(deg);
dms[0]=floor(a); a=(a-dms[0])*60.0;
dms[1]=floor(a); a=(a-dms[1])*60.0;
dms[2]=a; dms[0]*=sign;
}
/* convert deg-min-sec to degree -----------------------------------------------
* convert degree-minute-second to degree
@ -2942,60 +2951,69 @@ void freenav(nav_t *nav, int opt)
/* debug trace functions -----------------------------------------------------*/
//#ifdef TRACE
//
//static FILE *fp_trace=NULL; /* file pointer of trace */
//static char file_trace[1024]; /* trace file */
//static int level_trace=0; /* level of trace */
//static unsigned int tick_trace=0; /* tick time at traceopen (ms) */
//static gtime_t time_trace={0}; /* time at traceopen */
//static pthread_mutex_t lock_trace; /* lock for trace */
//
//static void traceswap(void)
//{
// gtime_t time=utc2gpst(timeget());
// char path[1024];
//
// lock(&lock_trace);
//
// if ((int)(time2gpst(time ,NULL)/INT_SWAP_TRAC)==
// (int)(time2gpst(time_trace,NULL)/INT_SWAP_TRAC)) {
// unlock(&lock_trace);
// return;
// }
// time_trace=time;
//
// if (!reppath(file_trace,path,time,"","")) {
// unlock(&lock_trace);
// return;
// }
// if (fp_trace) fclose(fp_trace);
//
// if (!(fp_trace=fopen(path,"w"))) {
// fp_trace=stderr;
// }
// unlock(&lock_trace);
//}
//extern void traceopen(const char *file)
//{
// gtime_t time=utc2gpst(timeget());
// char path[1024];
//
// reppath(file,path,time,"","");
// if (!*path||!(fp_trace=fopen(path,"w"))) fp_trace=stderr;
// strcpy(file_trace,file);
// tick_trace=tickget();
// time_trace=time;
// initlock(&lock_trace);
//}
//extern void traceclose(void)
//{
// if (fp_trace&&fp_trace!=stderr) fclose(fp_trace);
// fp_trace=NULL;
// file_trace[0]='\0';
//}
//extern void tracelevel(int level)
//{
// level_trace=level;
//}
FILE *fp_trace = NULL; /* file pointer of trace */
char file_trace[1024]; /* trace file */
static int level_trace = 0; /* level of trace */
unsigned int tick_trace = 0; /* tick time at traceopen (ms) */
gtime_t time_trace = {0, 0.0}; /* time at traceopen */
pthread_mutex_t lock_trace; /* lock for trace */
void traceswap(void)
{
gtime_t time = utc2gpst(timeget());
char path[1024];
rtk_lock(&lock_trace);
if ((int)(time2gpst(time, NULL) / INT_SWAP_TRAC) ==
(int)(time2gpst(time_trace, NULL) / INT_SWAP_TRAC))
{
rtk_unlock(&lock_trace);
return;
}
time_trace = time;
if (!reppath(file_trace, path, time, "", ""))
{
rtk_unlock(&lock_trace);
return;
}
if (fp_trace) fclose(fp_trace);
if (!(fp_trace = fopen(path, "w")))
{
fp_trace = stderr;
}
rtk_unlock(&lock_trace);
}
void traceopen(const char *file)
{
gtime_t time = utc2gpst(timeget());
char path[1024];
reppath(file, path, time, "", "");
if (!*path || !(fp_trace = fopen(path, "w"))) fp_trace = stderr;
strcpy(file_trace, file);
tick_trace = tickget();
time_trace = time;
initlock(&lock_trace);
}
void traceclose(void)
{
if (fp_trace && fp_trace != stderr) fclose(fp_trace);
fp_trace = NULL;
file_trace[0] = '\0';
}
void tracelevel(int level)
{
level_trace = level;
}
//extern void trace(int level, const char *format, ...)
//{
// va_list ap;
@ -3010,16 +3028,17 @@ 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, ...)
//{
// va_list ap;
//
// if (!fp_trace||level>level_trace) return;
// traceswap();
// 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 tracet(int level, const char *format, ...)
{
va_list ap;
if (!fp_trace || level > level_trace) return;
traceswap();
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)))

14
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);
@ -174,6 +174,7 @@ int adjgpsweek(int week);
unsigned int tickget(void);
void sleepms(int ms);
void deg2dms(double deg, double *dms, int ndec);
void deg2dms(double deg, double *dms);
double dms2deg(const double *dms);
void ecef2pos(const double *r, double *pos);
void pos2ecef(const double *pos, double *r);
@ -182,8 +183,6 @@ void ecef2enu(const double *pos, const double *r, double *e);
void enu2ecef(const double *pos, const double *e, double *r);
void covenu(const double *pos, const double *P, double *Q);
void covecef(const double *pos, const double *Q, double *P);
void ast_args(double t, double *f);
void nut_iau1980(double t, const double *f, double *dpsi, double *deps);
void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst);
@ -214,11 +213,12 @@ int savenav(const char *file, const nav_t *nav);
void freeobs(obs_t *obs);
void freenav(nav_t *nav, int opt);
//void traceopen(const char *file);
//void traceclose(void);
//void tracelevel(int level);
void traceopen(const char *file);
void traceclose(void);
void tracelevel(int level);
void traceswap(void);
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) */

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

@ -0,0 +1,997 @@
#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"
#include "rtklib_rtcm.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 = 0, 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)
{
pcv_t pcvt0[MAXSAT] = { {0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0},{0.0}}, {{0.0},{0.0}} } };
sbsfcorr_t sbsfcorr0 = {{0, 0.0}, 0.0, 0.0, 0.0, 0, 0, 0};
sbslcorr_t sbslcorr0 = { {0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0};
sbssat_t sbssat0 = {0, 0, 0, { {0, sbsfcorr0, sbslcorr0 } }};
sbsigp_t sbsigp0[MAXNIGP] = {{{0, 0.0}, 0, 0, 0, 0.0 }};
sbsion_t sbsion0[MAXBAND+1] = {{0, 0, {*sbsigp0} }};
dgps_t dgps0[MAXSAT] = { {{0, 0.0}, 0.0, 0.0, 0, 0.0 }};
ssr_t ssr0[MAXSAT] = {{ {{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0' }};
lexeph_t lexeph0[MAXSAT] = {{ {0,0.0}, {0,0.0}, 0, 0, 0, {0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0, {0.0} }};
stec_t stec0[MAXSTA] = {{ {0,0.0}, 0, 0.0, 0.0, {0.0}, 0}};
trop_t trop0[MAXSTA] = {{ {0, 0.0}, {0.0}, {0.0}}};
pppcorr_t pppcorr0 = {0, {{0},{0}}, {{0.0},{0.0}}, {0}, {0}, {0}, {0}, {stec0}, {trop0} };
nav_t nav = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
{0, 0, (erpd_t *){0}}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0},
{0.0}, {0.0}, {0.0}, {0.0}, 0, {{0.0},{0.0}}, {{0.0},{0.0}}, {{0.0},{0.0},{0.0}},
{0.0}, {0.0}, '0', {*pcvt0}, sbssat0, {*sbsion0}, {*dgps0}, {*ssr0}, {*lexeph0},
{{0,0.0}, 0.0, {0.0}, {{0.0},{0.0}} }, pppcorr0} ;
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, 0.0};
sol_t sol0 = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0},
'0', '0', '0', 0, 0, 0 };
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
seph_t seph0 = {0, {0,0.0}, {0,0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.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, 0.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 */
if (pthread_create(&svr->thread, NULL, rtksvrthread, svr))
{
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 */
pthread_join(svr->thread, NULL);
}
/* 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);
}

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

@ -0,0 +1,138 @@
/*!
* \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"
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, {}
};
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

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

File diff suppressed because it is too large Load Diff

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

@ -0,0 +1,182 @@
/*!
* \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"
#define COMMENTH "%" /* comment line indicator for solution */
#define MSG_DISCONN "$_DISCONNECT\r\n" /* disconnect message */
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

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

File diff suppressed because it is too large Load Diff

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

@ -0,0 +1,238 @@
/*!
* \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"
/* 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) */
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

2
src/core/system_parameters/gnss_obs_codes.h
View File

@ -91,7 +91,7 @@ const unsigned int CODE_L9A = 52; //!< obs code: SA SPS (IRN)
const unsigned int CODE_L9B = 53; //!< obs code: SB RS(D) (IRN)
const unsigned int CODE_L9C = 54; //!< obs code: SC RS(P) (IRN)
const unsigned int CODE_L9X = 55; //!< obs code: SB+C (IRN)
const unsigned int MAXCODE = 55; //!< max number of obs code
const int MAXCODE = 55; //!< max number of obs code
#endif

2
src/tests/system-tests/position_test.cc
View File

@ -382,7 +382,7 @@ int Position_Gps_L1_System_Test::configure_receiver()
config->set_property("PVT.positioning_mode", "PPP_Static");
config->set_property("PVT.iono_model", "OFF");
config->set_property("PVT.trop_model", "OFF");
// config->set_property("PVT.AR_GPS", "PPP-AR");
config->set_property("PVT.AR_GPS", "PPP-AR");
return 0;
}