├── .gitignore
├── GNSS_Algorithm
    ├── CMakeLists.txt
    ├── GNSS_Algorithm.cpp
    └── src
    │   ├── convgpx.c
    │   ├── convkml.c
    │   ├── convrnx.c
    │   ├── datum.c
    │   ├── download.c
    │   ├── ephemeris.c
    │   ├── geoid.c
    │   ├── gis.c
    │   ├── ionex.c
    │   ├── lambda.c
    │   ├── options.c
    │   ├── pntpos.c
    │   ├── postpos.c
    │   ├── ppp.c
    │   ├── ppp_ar.c
    │   ├── preceph.c
    │   ├── rcv
    │       ├── binex.c
    │       ├── crescent.c
    │       ├── javad.c
    │       ├── novatel.c
    │       ├── nvs.c
    │       ├── rt17.c
    │       ├── septentrio.c
    │       ├── skytraq.c
    │       ├── swiftnav.c
    │       └── ublox.c
    │   ├── rcvraw.c
    │   ├── rinex.c
    │   ├── rtcm.c
    │   ├── rtcm2.c
    │   ├── rtcm3.c
    │   ├── rtcm3e.c
    │   ├── rtkcmn.c
    │   ├── rtklib.h
    │   ├── rtkpos.c
    │   ├── rtksvr.c
    │   ├── sbas.c
    │   ├── solution.c
    │   ├── stream.c
    │   ├── streamsvr.c
    │   ├── tides.c
    │   └── tle.c
├── README.md
├── data
    ├── 20221128.rtcm3
    └── 20221129.rtcm3
└── image
    └── GNSS-Explorer.png


/.gitignore:
--------------------------------------------------------------------------------
1 | GNSS_Algorithm/build/
2 | .vscode/
3 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(gnss_algorithm)
 3 | 
 4 | set(CMAKE_BUILD_TYPE "Release")
 5 | set(CMAKE_CXX_FLAGS "-std=c++11")
 6 | #-DEIGEN_USE_MKL_ALL")
 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 8 | 
 9 | include_directories("/usr/include/")
10 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src)
11 | 
12 | aux_source_directory(src SRC_LIST)
13 | aux_source_directory(src/rcv SRC_LIST)
14 | 
15 | add_executable(gnss_algorithm GNSS_Algorithm.cpp ${SRC_LIST})
16 | 
17 | target_link_libraries(gnss_algorithm pthread)
18 | 
19 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/GNSS_Algorithm.cpp:
--------------------------------------------------------------------------------
 1 | /*------------------------------------------------------------------------------
 2 | *  GNSS_Algorithm.cpp.c : read rinex obs/nav files and compute receiver positions
 3 | *
 4 | *          Copyright (C) 2022 by BruceZhcw, All rights reserved.
 5 | *-----------------------------------------------------------------------------*/
 6 | #include <iostream>
 7 | #include <cstring>
 8 | #include "rtklib.h"
 9 | 
10 | #define PROGNAME    "GNSS_Algorithm"	/* program name */
11 | #define MAXFILE     16                  /* max number of input files */
12 | 
13 | int main(int argc, char **argv)
14 | {
15 | 	if (argc < 2) {
16 | 		std::cerr << "Usage:   " << argv[0] << " <rinex directory>" << std::endl;
17 | 		std::cerr << "example: " << "./gnss_algorithm  /mnt/e/gnss/" << std::endl;
18 | 		return -1;
19 | 	}
20 | 
21 | 	char directory[256];
22 | 	strcpy(directory, argv[1]);
23 | 	if (directory[strlen(directory)-1] != '/') {
24 | 		strcat(directory, "/");
25 | 	}
26 | 
27 | 	prcopt_t prcopt = prcopt_default;
28 | 	solopt_t solopt = solopt_default;
29 | 	filopt_t filopt = { "" };
30 | 	gtime_t ts = { 0 }, te = { 0 };
31 | 	double tint = 0.0, pos[3] = {40.0680091,116.3355171,46};
32 | 	int i, j, ret;
33 | 
34 | 	char *infile[MAXFILE];
35 | 	char filepath1[512], filepath2[512], outfilepath[512], tracepath[512];
36 | 
37 | 	sprintf(filepath1, "%srover.obs", directory);
38 | 	sprintf(filepath2, "%srover.nav", directory);
39 | 	sprintf(outfilepath, "%srover.pos", directory);
40 | 	sprintf(tracepath, "%s%s.trace", directory, PROGNAME);
41 | 
42 | 	infile[0] = filepath1;
43 | 	infile[1] = filepath2;
44 | 	char *outfile = outfilepath;
45 | 
46 | 	solopt.posf = SOLF_LLH;
47 | 	solopt.timef = 1;
48 | 	sprintf(solopt.prog, "%s ver.%s %s", PROGNAME, VER_RTKLIB, PATCH_LEVEL);
49 | 	strcpy(filopt.trace, tracepath);
50 | 
51 | 	prcopt.snrmask.ena[0] = prcopt.snrmask.ena[1] = 1;
52 | 	for (i = 0; i < NFREQ; i++) for (j = 0; j < 9; j++)
53 | 		prcopt.snrmask.mask[i][j] = 20;
54 | 
55 | 	prcopt.mode = PMODE_SINGLE;
56 | 	prcopt.spp_mode = SPP_MODE_LX;
57 | 
58 | 	for (j = 0; j<2; j++) pos[j] *= D2R;
59 | 	pos2ecef(pos, prcopt.rb);
60 | 	matcpy(prcopt.ru, prcopt.rb, 3, 1);
61 | 
62 | 	ret = postpos(ts, te, tint, 0.0, &prcopt, &solopt, &filopt, infile, 2, outfile, "", "");
63 | 
64 | 	std::cout << "postpos over! return with: " << ret << std::endl;
65 | 
66 | 	return ret;
67 | }
68 | 
69 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/convgpx.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * convgpx.c : gpx converter
  3 | *
  4 | *          Copyright (C) 2016 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references :
  7 | *     [1] GPX The GPS Exchange Format http://www.topografix.com/gpx.asp
  8 | *
  9 | * version : $Revision:$ $Date:$
 10 | * history : 2016/06/11  1.0  new
 11 | *           2016/09/18  1.1  modify <fix> labels according GPX specs
 12 | *-----------------------------------------------------------------------------*/
 13 | #include "rtklib.h"
 14 | 
 15 | /* constants -----------------------------------------------------------------*/
 16 | 
 17 | #define HEADXML "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
 18 | #define HEADGPX "<gpx version=\"1.1\" creator=\"%s\" xmlns=\"%s\">\n"
 19 | #define TAILGPX "</gpx>"
 20 | 
 21 | static const char *XMLNS="http://www.topografix.com/GPX/1/1";
 22 | 
 23 | /* output waypoint -----------------------------------------------------------*/
 24 | static void outpoint(FILE *fp, gtime_t time, const double *pos,
 25 |                      const char *label, int stat, int outalt, int outtime)
 26 | {
 27 |     /* fix, float, sbas and ppp are rtklib extentions to GPX */
 28 |     const char *fix_label[]={"fix","float","sbas","dgps","3d","ppp"};
 29 |     double ep[6];
 30 |     
 31 |     fprintf(fp,"<wpt lat=\"%.9f\" lon=\"%.9f\">\n",pos[0]*R2D,pos[1]*R2D);
 32 |     if (outalt) {
 33 |         fprintf(fp," <ele>%.4f</ele>\n",pos[2]-(outalt==2?geoidh(pos):0.0));
 34 |     }
 35 |     if (outtime) {
 36 |         if      (outtime==2) time=gpst2utc(time);
 37 |         else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0);
 38 |         time2epoch(time,ep);
 39 |         fprintf(fp," <time>%04.0f-%02.0f-%02.0fT%02.0f:%02.0f:%05.2fZ</time>\n",
 40 |                 ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
 41 |     }
 42 |     if (outalt==2) {
 43 |         fprintf(fp," <geoidheight>%.4f</geoidheight>\n",geoidh(pos));
 44 |     }
 45 |     if (stat>=1&&stat<=6) {
 46 |         fprintf(fp," <fix>%s</fix>\n",fix_label[stat-1]);
 47 |     }
 48 |     if (*label) {
 49 |         fprintf(fp," <name>%s</name>\n",label);
 50 |     }
 51 |     fprintf(fp,"</wpt>\n");
 52 | }
 53 | /* output track --------------------------------------------------------------*/
 54 | static void outtrack(FILE *fp, const solbuf_t *solbuf, int outalt, int outtime)
 55 | {
 56 |     gtime_t time;
 57 |     double pos[3],ep[6];
 58 |     int i;
 59 |     
 60 |     fprintf(fp,"<trk>\n");
 61 |     fprintf(fp," <trkseg>\n");
 62 |     for (i=0;i<solbuf->n;i++) {
 63 |         ecef2pos(solbuf->data[i].rr,pos);
 64 |         fprintf(fp,"  <trkpt lat=\"%.9f\" lon=\"%.9f\">\n",pos[0]*R2D,
 65 |                 pos[1]*R2D);
 66 |         if (outalt) {
 67 |             fprintf(fp,"   <ele>%.4f</ele>\n",pos[2]-(outalt==2?geoidh(pos):0.0));
 68 |         }
 69 |         if (outtime) {
 70 |             time=solbuf->data[i].time;
 71 |             if      (outtime==2) time=gpst2utc(time);
 72 |             else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0);
 73 |             time2epoch(time,ep);
 74 |             fprintf(fp,"   <time>%04.0f-%02.0f-%02.0fT%02.0f:%02.0f:%05.2fZ</time>\n",
 75 |                     ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
 76 |         }
 77 |         if (outalt==2) {
 78 |             fprintf(fp,"   <geoidheight>%.4f</geoidheight>\n",geoidh(pos));
 79 |         }
 80 |         fprintf(fp,"  </trkpt>\n");
 81 |     }
 82 |     fprintf(fp," </trkseg>\n");
 83 |     fprintf(fp,"</trk>\n");
 84 | }
 85 | /* save gpx file -------------------------------------------------------------*/
 86 | static int savegpx(const char *file, const solbuf_t *solbuf, int outtrk,
 87 |                    int outpnt, int outalt, int outtime)
 88 | {
 89 |     FILE *fp;
 90 |     double pos[3];
 91 |     int i;
 92 |     
 93 |     if (!(fp=fopen(file,"w"))) {
 94 |         fprintf(stderr,"file open error : %s\n",file);
 95 |         return 0;
 96 |     }
 97 |     fprintf(fp,HEADXML);
 98 |     fprintf(fp,HEADGPX,"RTKLIB " VER_RTKLIB,XMLNS);
 99 |     
100 |     /* output waypoint */
101 |     if (outpnt) {
102 |         for (i=0;i<solbuf->n;i++) {
103 |             ecef2pos(solbuf->data[i].rr,pos);
104 |             outpoint(fp,solbuf->data[i].time,pos,"",solbuf->data[i].stat,outalt,
105 |                      outtime);
106 |         }
107 |     }
108 |     /* output waypoint of ref position */
109 |     if (norm(solbuf->rb,3)>0.0) {
110 |         ecef2pos(solbuf->rb,pos);
111 |         outpoint(fp,solbuf->data[0].time,pos,"Reference Position",0,outalt,0);
112 |     }
113 |     /* output track */
114 |     if (outtrk) {
115 |         outtrack(fp,solbuf,outalt,outtime);
116 |     }
117 |     fprintf(fp,"%s\n",TAILGPX);
118 |     fclose(fp);
119 |     return 1;
120 | }
121 | /* convert to GPX file ---------------------------------------------------------
122 | * convert solutions to GPX file [1]
123 | * args   : char   *infile   I   input solutions file
124 | *          char   *outfile  I   output google earth kml file ("":<infile>.kml)
125 | *          gtime_t ts,te    I   start/end time (gpst)
126 | *          int    tint      I   time interval (s) (0.0:all)
127 | *          int    qflg      I   quality flag (0:all)
128 | *          double *offset   I   add offset {east,north,up} (m)
129 | *          int    outtrk    I   output track    (0:off,1:on)
130 | *          int    outpnt    I   output waypoint (0:off,1:on)
131 | *          int    outalt    I   output altitude (0:off,1:elipsoidal,2:geodetic)
132 | *          int    outtime   I   output time (0:off,1:gpst,2:utc,3:jst)
133 | * return : status (0:ok,-1:file read,-2:file format,-3:no data,-4:file write)
134 | *-----------------------------------------------------------------------------*/
135 | extern int convgpx(const char *infile, const char *outfile, gtime_t ts,
136 |                    gtime_t te, double tint, int qflg, double *offset,
137 |                    int outtrk, int outpnt, int outalt, int outtime)
138 | {
139 |     solbuf_t solbuf={0};
140 |     double rr[3]={0},pos[3],dr[3];
141 |     int i,j;
142 |     char *p,file[1024];
143 |     
144 |     trace(3,"convgpx : infile=%s outfile=%s\n",infile,outfile);
145 |     
146 |     if (!*outfile) {
147 |         if ((p=strrchr(infile,'.'))) {
148 |             strncpy(file,infile,p-infile);
149 |             strcpy(file+(p-infile),".gpx");
150 |         }
151 |         else sprintf(file,"%s.gpx",infile);
152 |     }
153 |     else strcpy(file,outfile);
154 |     
155 |     /* read solution file */
156 |     if (!readsolt((char **)&infile,1,ts,te,tint,qflg,&solbuf)) return -1;
157 |     
158 | 	/* mean position */
159 | 	for (i=0;i<3;i++) {
160 |         for (j=0;j<solbuf.n;j++) rr[i]+=solbuf.data[j].rr[i];
161 |         rr[i]/=solbuf.n;
162 |     }
163 |     /* add offset */
164 |     ecef2pos(rr,pos);
165 |     enu2ecef(pos,offset,dr);
166 |     for (i=0;i<solbuf.n;i++) {
167 |         for (j=0;j<3;j++) solbuf.data[i].rr[j]+=dr[j];
168 |     }
169 |     if (norm(solbuf.rb,3)>0.0) {
170 |         for (i=0;i<3;i++) solbuf.rb[i]+=dr[i];
171 |     }
172 |     /* save gpx file */
173 |     return savegpx(file,&solbuf,outtrk,outpnt,outalt,outtime)?0:-4;
174 | }
175 | 
176 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/convkml.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * convkml.c : google earth kml converter
  3 | *
  4 | *          Copyright (C) 2007-2017 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references :
  7 | *     [1] Open Geospatial Consortium Inc., OGC 07-147r2, OGC(R) KML, 2008-04-14
  8 | *
  9 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
 10 | * history : 2007/01/20  1.0  new
 11 | *           2007/03/15  1.1  modify color sequence
 12 | *           2007/04/03  1.2  add geodetic height option
 13 | *                            support input of NMEA GGA sentence
 14 | *                            delete altitude info for track
 15 | *                            add time stamp option
 16 | *                            separate readsol.c file
 17 | *           2009/01/19  1.3  fix bug on display mark with by-q-flag option
 18 | *           2010/05/10  1.4  support api readsolt() change
 19 | *           2010/08/14  1.5  fix bug on readsolt() (2.4.0_p3)
 20 | *           2017/06/10  1.6  support wild-card in input file
 21 | *-----------------------------------------------------------------------------*/
 22 | #include "rtklib.h"
 23 | 
 24 | /* constants -----------------------------------------------------------------*/
 25 | 
 26 | #define SIZP     0.2            /* mark size of rover positions */
 27 | #define SIZR     0.3            /* mark size of reference position */
 28 | #define TINT     60.0           /* time label interval (sec) */
 29 | 
 30 | static const char *head1="<?xml version=\"1.0\" encoding=\"UTF-8\"?>";
 31 | static const char *head2="<kml xmlns=\"http://earth.google.com/kml/2.1\">";
 32 | static const char *mark="http://maps.google.com/mapfiles/kml/pal2/icon18.png";
 33 | 
 34 | /* output track --------------------------------------------------------------*/
 35 | static void outtrack(FILE *f, const solbuf_t *solbuf, const char *color,
 36 |                      int outalt, int outtime)
 37 | {
 38 |     double pos[3];
 39 |     int i;
 40 |     
 41 |     fprintf(f,"<Placemark>\n");
 42 |     fprintf(f,"<name>Rover Track</name>\n");
 43 |     fprintf(f,"<Style>\n");
 44 |     fprintf(f,"<LineStyle>\n");
 45 |     fprintf(f,"<color>%s</color>\n",color);
 46 |     fprintf(f,"</LineStyle>\n");
 47 |     fprintf(f,"</Style>\n");
 48 |     fprintf(f,"<LineString>\n");
 49 |     if (outalt) fprintf(f,"<altitudeMode>absolute</altitudeMode>\n");
 50 |     fprintf(f,"<coordinates>\n");
 51 |     for (i=0;i<solbuf->n;i++) {
 52 |         ecef2pos(solbuf->data[i].rr,pos);
 53 |         if      (outalt==0) pos[2]=0.0;
 54 |         else if (outalt==2) pos[2]-=geoidh(pos);
 55 |         fprintf(f,"%13.9f,%12.9f,%5.3f\n",pos[1]*R2D,pos[0]*R2D,pos[2]);
 56 |     }
 57 |     fprintf(f,"</coordinates>\n");
 58 |     fprintf(f,"</LineString>\n");
 59 |     fprintf(f,"</Placemark>\n");
 60 | }
 61 | /* output point --------------------------------------------------------------*/
 62 | static void outpoint(FILE *fp, gtime_t time, const double *pos,
 63 |                      const char *label, int style, int outalt, int outtime)
 64 | {
 65 |     double ep[6],alt=0.0;
 66 |     char str[256]="";
 67 |     
 68 |     fprintf(fp,"<Placemark>\n");
 69 |     if (*label) fprintf(fp,"<name>%s</name>\n",label);
 70 |     fprintf(fp,"<styleUrl>#P%d</styleUrl>\n",style);
 71 |     if (outtime) {
 72 |         if      (outtime==2) time=gpst2utc(time);
 73 |         else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0);
 74 |         time2epoch(time,ep);
 75 |         if (!*label&&fmod(ep[5]+0.005,TINT)<0.01) {
 76 |             sprintf(str,"%02.0f:%02.0f",ep[3],ep[4]);
 77 |             fprintf(fp,"<name>%s</name>\n",str);
 78 |         }
 79 |         sprintf(str,"%04.0f-%02.0f-%02.0fT%02.0f:%02.0f:%05.2fZ",
 80 |                 ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
 81 |         fprintf(fp,"<TimeStamp><when>%s</when></TimeStamp>\n",str);
 82 |     }
 83 |     fprintf(fp,"<Point>\n");
 84 |     if (outalt) {
 85 |         fprintf(fp,"<extrude>1</extrude>\n");
 86 |         fprintf(fp,"<altitudeMode>absolute</altitudeMode>\n");
 87 |         alt=pos[2]-(outalt==2?geoidh(pos):0.0);
 88 |     }
 89 |     fprintf(fp,"<coordinates>%13.9f,%12.9f,%5.3f</coordinates>\n",pos[1]*R2D,
 90 |             pos[0]*R2D,alt);
 91 |     fprintf(fp,"</Point>\n");
 92 |     fprintf(fp,"</Placemark>\n");
 93 | }
 94 | /* save kml file -------------------------------------------------------------*/
 95 | static int savekml(const char *file, const solbuf_t *solbuf, int tcolor,
 96 |                    int pcolor, int outalt, int outtime)
 97 | {
 98 |     FILE *fp;
 99 |     double pos[3];
100 |     int i,qcolor[]={0,1,2,5,4,3,0};
101 |     char *color[]={
102 |         "ffffffff","ff008800","ff00aaff","ff0000ff","ff00ffff","ffff00ff"
103 |     };
104 |     if (!(fp=fopen(file,"w"))) {
105 |         fprintf(stderr,"file open error : %s\n",file);
106 |         return 0;
107 |     }
108 |     fprintf(fp,"%s\n%s\n",head1,head2);
109 |     fprintf(fp,"<Document>\n");
110 |     for (i=0;i<6;i++) {
111 |         fprintf(fp,"<Style id=\"P%d\">\n",i);
112 |         fprintf(fp,"  <IconStyle>\n");
113 |         fprintf(fp,"    <color>%s</color>\n",color[i]);
114 |         fprintf(fp,"    <scale>%.1f</scale>\n",i==0?SIZR:SIZP);
115 |         fprintf(fp,"    <Icon><href>%s</href></Icon>\n",mark);
116 |         fprintf(fp,"  </IconStyle>\n");
117 |         fprintf(fp,"</Style>\n");
118 |     }
119 |     if (tcolor>0) {
120 |         outtrack(fp,solbuf,color[tcolor-1],outalt,outtime);
121 |     }
122 |     if (pcolor>0) {
123 |         fprintf(fp,"<Folder>\n");
124 |         fprintf(fp,"  <name>Rover Position</name>\n");
125 |         for (i=0;i<solbuf->n;i++) {
126 |             ecef2pos(solbuf->data[i].rr,pos);
127 |             outpoint(fp,solbuf->data[i].time,pos,"",
128 |                      pcolor==5?qcolor[solbuf->data[i].stat]:pcolor-1,outalt,outtime);
129 |         }
130 |         fprintf(fp,"</Folder>\n");
131 |     }
132 |     if (norm(solbuf->rb,3)>0.0) {
133 |         ecef2pos(solbuf->rb,pos);
134 |         outpoint(fp,solbuf->data[0].time,pos,"Reference Position",0,outalt,0);
135 |     }
136 |     fprintf(fp,"</Document>\n");
137 |     fprintf(fp,"</kml>\n");
138 |     fclose(fp);
139 |     return 1;
140 | }
141 | /* convert to google earth kml file --------------------------------------------
142 | * convert solutions to google earth kml file
143 | * args   : char   *infile   I   input solutions file (wild-card (*) is expanded)
144 | *          char   *outfile  I   output google earth kml file ("":<infile>.kml)
145 | *          gtime_t ts,te    I   start/end time (gpst)
146 | *          int    tint      I   time interval (s) (0.0:all)
147 | *          int    qflg      I   quality flag (0:all)
148 | *          double *offset   I   add offset {east,north,up} (m)
149 | *          int    tcolor    I   track color
150 | *                               (0:none,1:white,2:green,3:orange,4:red,5:yellow)
151 | *          int    pcolor    I   point color
152 | *                               (0:none,1:white,2:green,3:orange,4:red,5:by qflag)
153 | *          int    outalt    I   output altitude (0:off,1:elipsoidal,2:geodetic)
154 | *          int    outtime   I   output time (0:off,1:gpst,2:utc,3:jst)
155 | * return : status (0:ok,-1:file read,-2:file format,-3:no data,-4:file write)
156 | * notes  : see ref [1] for google earth kml file format
157 | *-----------------------------------------------------------------------------*/
158 | extern int convkml(const char *infile, const char *outfile, gtime_t ts,
159 |                    gtime_t te, double tint, int qflg, double *offset,
160 |                    int tcolor, int pcolor, int outalt, int outtime)
161 | {
162 |     solbuf_t solbuf={0};
163 |     double rr[3]={0},pos[3],dr[3];
164 |     int i,j,nfile,stat;
165 |     char *p,file[1024],*files[MAXEXFILE]={0};
166 |     
167 |     trace(3,"convkml : infile=%s outfile=%s\n",infile,outfile);
168 |     
169 |     /* expand wild-card of infile */
170 |     for (i=0;i<MAXEXFILE;i++) {
171 |         if (!(files[i]=(char *)malloc(1024))) {
172 |             for (i--;i>=0;i--) free(files[i]);
173 |             return -4;
174 |         }
175 |     }
176 |     if ((nfile=expath(infile,files,MAXEXFILE))<=0) {
177 |         for (i=0;i<MAXEXFILE;i++) free(files[i]);
178 |         return -3;
179 |     }
180 |     if (!*outfile) {
181 |         if ((p=strrchr(infile,'.'))) {
182 |             strncpy(file,infile,p-infile);
183 |             strcpy(file+(p-infile),".kml");
184 |         }
185 |         else sprintf(file,"%s.kml",infile);
186 |     }
187 |     else strcpy(file,outfile);
188 |     
189 |     /* read solution file */
190 |     stat=readsolt(files,nfile,ts,te,tint,qflg,&solbuf);
191 |     
192 |     for (i=0;i<MAXEXFILE;i++) free(files[i]);
193 |     
194 |     if (!stat) {
195 |         return -1;
196 |     }
197 |     /* mean position */
198 |     for (i=0;i<3;i++) {
199 |         for (j=0;j<solbuf.n;j++) rr[i]+=solbuf.data[j].rr[i];
200 |         rr[i]/=solbuf.n;
201 |     }
202 |     /* add offset */
203 |     ecef2pos(rr,pos);
204 |     enu2ecef(pos,offset,dr);
205 |     for (i=0;i<solbuf.n;i++) {
206 |         for (j=0;j<3;j++) solbuf.data[i].rr[j]+=dr[j];
207 |     }
208 |     if (norm(solbuf.rb,3)>0.0) {
209 |         for (i=0;i<3;i++) solbuf.rb[i]+=dr[i];
210 |     }
211 |     /* save kml file */
212 |     return savekml(file,&solbuf,tcolor,pcolor,outalt,outtime)?0:-4;
213 | }
214 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/datum.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * datum.c : datum transformation
  3 | *
  4 | *          Copyright (C) 2007 by T.TAKASU, All rights reserved.
  5 | *
  6 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
  7 | * history : 2007/02/08 1.0 new
  8 | *-----------------------------------------------------------------------------*/
  9 | #include "rtklib.h"
 10 | 
 11 | #define MAXPRM  400000              /* max number of parameter records */
 12 | 
 13 | typedef struct {                    /* datum trans parameter type */
 14 |     int code;                       /* mesh code */
 15 |     float db,dl;                    /* difference of latitude/longitude (sec) */
 16 | } tprm_t;
 17 | 
 18 | static tprm_t *prm=NULL;            /* datum trans parameter table */
 19 | static int n=0;                     /* datum trans parameter table size */
 20 | 
 21 | /* compare datum trans parameters --------------------------------------------*/
 22 | static int cmpprm(const void *p1, const void *p2)
 23 | {
 24 |     tprm_t *q1=(tprm_t *)p1,*q2=(tprm_t *)p2;
 25 |     return q1->code-q2->code;
 26 | }
 27 | /* search datum trans parameter ----------------------------------------------*/
 28 | static int searchprm(double lat, double lon)
 29 | {
 30 |     int i,j,k,n1,m1,n2,m2,code;
 31 |     
 32 |     lon-=6000.0;
 33 |     n1=(int)(lat/40.0); lat-=n1*40.0;
 34 |     m1=(int)(lon/60.0); lon-=m1*60.0;
 35 |     n2=(int)(lat/5.0); lat-=n2*5.0;
 36 |     m2=(int)(lon/7.5); lon-=m2*7.5;
 37 |     code=n1*1000000+m1*10000+n2*1000+m2*100+(int)(lat/0.5)*10+(int)(lon/0.75);
 38 |     
 39 |     for (i=0,j=n-1;i<j;) { /* binary search */
 40 |         k=(i+j)/2;
 41 |         if (prm[k].code==code) return k;
 42 |         if (prm[k].code<code) i=k+1; else j=k;
 43 |     }
 44 |     return -1;
 45 | }
 46 | /* tokyo datum to jgd2000 lat/lon corrections --------------------------------*/
 47 | static int dlatdlon(const double *post, double *dpos)
 48 | {
 49 |     double lat=post[0]*R2D*60.0,lon=post[1]*R2D*60.0; /* arcmin */
 50 |     double dlat=0.5,dlon=0.75,db[2][2],dl[2][2],a,b,c,d;
 51 |     int i,j,k;
 52 |     
 53 |     if (n==0) return -1;
 54 |     for (i=0;i<2;i++) for (j=0;j<2;j++) {
 55 |         if ((k=searchprm(lat+i*dlat,lon+j*dlon))<0) return -1;
 56 |         db[i][j]=prm[k].db; dl[i][j]=prm[k].dl;
 57 |     }
 58 |     a=lat/dlat-(int)(lat/dlat); c=1.0-a;
 59 |     b=lon/dlon-(int)(lon/dlon); d=1.0-b;
 60 |     dpos[0]=(db[0][0]*c*d+db[1][0]*a*d+db[0][1]*c*b+db[1][1]*a*b)*D2R/3600.0;
 61 |     dpos[1]=(dl[0][0]*c*d+dl[1][0]*a*d+dl[0][1]*c*b+dl[1][1]*a*b)*D2R/3600.0;
 62 |     return 0;
 63 | }
 64 | /* load datum transformation parameter -----------------------------------------
 65 | * load datum transformation parameter
 66 | * args   : char  *file      I   datum trans parameter file path
 67 | * return : status (0:ok,0>:error)
 68 | * notes  : parameters file shall comply with GSI TKY2JGD.par
 69 | *-----------------------------------------------------------------------------*/
 70 | extern int loaddatump(const char *file)
 71 | {
 72 |     FILE *fp;
 73 |     char buff[256];
 74 |     
 75 |     if (n>0) return 0; /* already loaded */
 76 |     
 77 |     if (!(fp=fopen(file,"r"))) {
 78 |         fprintf(stderr,"%s : datum prm file open error : %s\n",__FILE__,file);
 79 |         return -1;
 80 |     }
 81 |     if (!(prm=(tprm_t *)malloc(sizeof(tprm_t)*MAXPRM))) {
 82 |         fprintf(stderr,"%s : memory allocation error\n",__FILE__);
 83 |         return -1;
 84 |     }
 85 |     while (fgets(buff,sizeof(buff),fp)&&n<MAXPRM) {
 86 |         if (sscanf(buff,"%d %f %f",&prm[n].code,&prm[n].db,&prm[n].dl)>=3) n++;
 87 |     }
 88 |     fclose(fp);
 89 |     qsort(prm,n,sizeof(tprm_t),cmpprm); /* sort parameter table */
 90 |     return 0;
 91 | }
 92 | /* tokyo datum to JGD2000 datum ------------------------------------------------
 93 | * transform position in Tokyo datum to JGD2000 datum
 94 | * args   : double *pos      I   position in Tokyo datum   {lat,lon,h} (rad,m)
 95 | *                           O   position in JGD2000 datum {lat,lon,h} (rad,m)
 96 | * return : status (0:ok,0>:error,out of range)
 97 | * notes  : before calling, call loaddatump() to set parameter table
 98 | *-----------------------------------------------------------------------------*/
 99 | extern int tokyo2jgd(double *pos)
100 | {
101 |     double post[2],dpos[2];
102 |     
103 |     post[0]=pos[0];
104 |     post[1]=pos[1];
105 |     if (dlatdlon(post,dpos)) return -1;
106 |     pos[0]=post[0]+dpos[0];
107 |     pos[1]=post[1]+dpos[1];
108 |     return 0;
109 | }
110 | /* JGD2000 datum to Tokyo datum ------------------------------------------------
111 | * transform position in JGD2000 datum to Tokyo datum
112 | * args   : double *pos      I   position in JGD2000 datum {lat,lon,h} (rad,m)
113 | *                           O   position in Tokyo datum   {lat,lon,h} (rad,m)
114 | * return : status (0:ok,0>:error,out of range)
115 | * notes  : before calling, call loaddatump() to set parameter table
116 | *-----------------------------------------------------------------------------*/
117 | extern int jgd2tokyo(double *pos)
118 | {
119 |     double posj[2],dpos[2];
120 |     int i;
121 |     
122 |     posj[0]=pos[0];
123 |     posj[1]=pos[1];
124 |     for (i=0;i<2;i++) {
125 |         if (dlatdlon(pos,dpos)) return -1;
126 |         pos[0]=posj[0]-dpos[0];
127 |         pos[1]=posj[1]-dpos[1];
128 |     }
129 |     return 0;
130 | }
131 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/gis.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * gis.c: GIS data functions
  3 | *
  4 | *          Copyright (C) 2016 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references:
  7 | *     [1] ESRI Shapefile Technical Description, An ESRI White Paper, July, 1998
  8 | *
  9 | * version : $Revision:$ $Date:$
 10 | * history : 2016/06/10 1.0  new
 11 | *           2016/07/31 1.1  add boundary of polyline and polygon
 12 | *-----------------------------------------------------------------------------*/
 13 | #include "rtklib.h"
 14 | 
 15 | #define SHAPE_CODE  9994        /* shapefile code */
 16 | 
 17 | /* get integer big-endian ----------------------------------------------------*/
 18 | static int I4_B(uint8_t *buff)
 19 | {
 20 |     int i,val=0;
 21 |     uint8_t *p=buff,*q=(uint8_t *)&val+3;
 22 |     
 23 |     for (i=0;i<4;i++) {
 24 |         *q--=*p++;
 25 |     }
 26 |     return val;
 27 | }
 28 | /* get integer little-endian -------------------------------------------------*/
 29 | static int I4_L(uint8_t *buff)
 30 | {
 31 |     int val;
 32 |     
 33 |     memcpy(&val,buff,4);
 34 |     return val;
 35 | }
 36 | /* get double little-endian --------------------------------------------------*/
 37 | static double D8_L(uint8_t *buff)
 38 | {
 39 |     double val;
 40 |     
 41 |     memcpy(&val,buff,8);
 42 |     return val;
 43 | }
 44 | /* read shapefile header -----------------------------------------------------*/
 45 | static int read_shape_head(FILE *fp)
 46 | {
 47 |     uint8_t buff[128];
 48 |     
 49 |     if (fread(buff,100,1,fp)!=1) {
 50 |         return -1;
 51 |     }
 52 |     if (I4_B(buff)!=SHAPE_CODE) {
 53 |         return -1;
 54 |     }
 55 |     return I4_L(buff+32);
 56 | }
 57 | /* initialize boundary -------------------------------------------------------*/
 58 | static void init_bound(double *bound)
 59 | {
 60 |     bound[0]= PI/2.0;
 61 |     bound[1]=-PI/2.0;
 62 |     bound[2]= PI;
 63 |     bound[3]=-PI;
 64 | }
 65 | /* update boundary -----------------------------------------------------------*/
 66 | static void update_bound(const double *pos, double *bound)
 67 | {
 68 |     if (pos[0]<bound[0]) bound[0]=pos[0];
 69 |     if (pos[0]>bound[1]) bound[1]=pos[0];
 70 |     if (pos[1]<bound[2]) bound[2]=pos[1];
 71 |     if (pos[1]>bound[3]) bound[3]=pos[1];
 72 | }
 73 | /* add gis data --------------------------------------------------------------*/
 74 | static int gis_add(gisd_t **p, int type, void *data)
 75 | {
 76 |     gisd_t *new_data;
 77 |     
 78 |     if (!(new_data=(gisd_t *)malloc(sizeof(gisd_t)))) {
 79 |         return 0;
 80 |     }
 81 |     new_data->next=*p;
 82 |     new_data->type=type;
 83 |     new_data->data=data;
 84 |     *p=new_data;
 85 |     return 1;
 86 | }
 87 | /* read point data -----------------------------------------------------------*/
 88 | static int read_pnt(FILE *fp, double *bound, gisd_t **p)
 89 | {
 90 |     gis_pnt_t *pnt;
 91 |     double pos[3]={0};
 92 |     uint8_t buff[16];
 93 |     
 94 |     if (fread(buff,16,1,fp)!=1) {
 95 |         return 0;
 96 |     }
 97 |     if (!(pnt=(gis_pnt_t *)malloc(sizeof(gis_pnt_t)))) {
 98 |         return 0;
 99 |     }
100 |     pos[0]=D8_L(buff+8)*D2R;
101 |     pos[1]=D8_L(buff  )*D2R;
102 |     update_bound(pos,bound);
103 |     pos2ecef(pos,pnt->pos);
104 |     
105 |     return gis_add(p,1,pnt);
106 | }
107 | /* read multi-point data ------------------------------------------------------*/
108 | static int read_mpnt(FILE *fp, double *bound, gisd_t **p)
109 | {
110 |     uint8_t buff[36];
111 |     int i,np;
112 |     
113 |     if (fread(buff,36,1,fp)!=1) {
114 |         return 0;
115 |     }
116 |     np=I4_L(buff+32);
117 |     
118 |     for (i=0;i<np;i++) {
119 |         if (!read_pnt(fp,bound,p)) {
120 |             return 0;
121 |         }
122 |     }
123 |     return 1;
124 | }
125 | /* read polyline data ---------------------------------------------------------*/
126 | static int read_poly(FILE *fp, double *bound, gisd_t **p)
127 | {
128 |     gis_poly_t *poly;
129 |     double pos[3]={0};
130 |     uint8_t buff[40];
131 |     int i,j,nt,np,nr,n,*part;
132 |     
133 |     if (fread(buff,40,1,fp)!=1) {
134 |         return 0;
135 |     }
136 |     nt=I4_L(buff+32);
137 |     np=I4_L(buff+36);
138 |     
139 |     if (!(part=(int *)malloc(sizeof(int)*nt))) {
140 |         return 0;
141 |     }
142 |     for (i=0;i<nt;i++) {
143 |         size_t bytesRead = fread(buff,4,1,fp);
144 |         part[i]=I4_L(buff);
145 |     }
146 |     for (i=0;i<nt;i++) {
147 |         nr=(i<nt-1?part[i+1]:np)-part[i];
148 |         
149 |         if (!(poly=(gis_poly_t *)malloc(sizeof(gis_poly_t)))) {
150 |             free(part);
151 |             return 0;
152 |         }
153 |         if (!(poly->pos=(double *)malloc(sizeof(double)*nr*3))) {
154 |             free(poly);
155 |             free(part);
156 |             return 0;
157 |         }
158 |         init_bound(poly->bound);
159 |         
160 |         for (j=n=0;j<nr;j++) {
161 |             if (fread(buff,16,1,fp)!=1) {
162 |                 free(poly->pos);
163 |                 free(poly);
164 |                 free(part);
165 |                 return 0;
166 |             }
167 |             pos[0]=D8_L(buff+8)*D2R;
168 |             pos[1]=D8_L(buff  )*D2R;
169 |             if (pos[0]<-1E16||pos[1]<-1E16) {
170 |                 continue;
171 |             }
172 |             update_bound(pos,poly->bound);
173 |             update_bound(pos,bound);
174 |             pos2ecef(pos,poly->pos+n*3);
175 |             n++;
176 |         }
177 |         poly->npnt=n;
178 |         if (!gis_add(p,2,(void *)poly)) {
179 |             free(poly->pos);
180 |             free(poly);
181 |             free(part);
182 |             return 0;
183 |         }
184 |     }
185 |     free(part);
186 |     return 1;
187 | }
188 | /* read polygon data ---------------------------------------------------------*/
189 | static int read_polygon(FILE *fp, double *bound, gisd_t **p)
190 | {
191 |     gis_polygon_t *polygon;
192 |     double pos[3]={0};
193 |     uint8_t buff[40];
194 |     int i,j,nt,np,nr,n,*part;
195 |     
196 |     if (fread(buff,40,1,fp)!=1) {
197 |         return 0;
198 |     }
199 |     nt=I4_L(buff+32);
200 |     np=I4_L(buff+36);
201 |     
202 |     if (!(part=(int *)malloc(sizeof(int)*nt))) {
203 |         return 0;
204 |     }
205 |     for (i=0;i<nt;i++) {
206 |         size_t bytesRead = fread(buff,4,1,fp);
207 |         part[i]=I4_L(buff);
208 |     }
209 |     for (i=0;i<nt;i++) {
210 |         nr=(i<nt-1?part[i+1]:np)-part[i];
211 |         
212 |         if (!(polygon=(gis_polygon_t *)malloc(sizeof(gis_poly_t)))) {
213 |             free(part);
214 |             return 0;
215 |         }
216 |         if (!(polygon->pos=(double *)malloc(sizeof(double)*nr*3))) {
217 |             free(polygon);
218 |             free(part);
219 |             return 0;
220 |         }
221 |         init_bound(polygon->bound);
222 |         
223 |         for (j=n=0;j<nr;j++) {
224 |             if (fread(buff,16,1,fp)!=1) {
225 |                 free(polygon->pos);
226 |                 free(polygon);
227 |                 free(part);
228 |                 return 0;
229 |             }
230 |             pos[0]=D8_L(buff+8)*D2R;
231 |             pos[1]=D8_L(buff  )*D2R;
232 |             if (pos[0]<-1E16||pos[1]<-1E16) {
233 |                 continue;
234 |             }
235 |             update_bound(pos,polygon->bound);
236 |             update_bound(pos,bound);
237 |             pos2ecef(pos,polygon->pos+n*3);
238 |             n++;
239 |         }
240 |         polygon->npnt=n;
241 |         if (!gis_add(p,3,(void *)polygon)) {
242 |             free(polygon->pos);
243 |             free(polygon);
244 |             free(part);
245 |             return 0;
246 |         }
247 |     }
248 |     free(part);
249 |     return 1;
250 | }
251 | /* read shapefile records ----------------------------------------------------*/
252 | static int gis_read_record(FILE *fp, FILE *fp_idx, int type, double *bound,
253 |                            gisd_t **data)
254 | {
255 |     gisd_t *p,*next;
256 |     uint8_t buff[16];
257 |     int i,off,num,len1,len2,typ2;
258 |     
259 |     for (i=0;fread(buff,1,8,fp_idx)==8;i++) {
260 |         off =I4_B(buff  )*2;
261 |         len1=I4_B(buff+4)*2;
262 |         
263 |         if (fseek(fp,(long)off,SEEK_SET)<0||fread(buff,12,1,fp)!=1) {
264 |             return 0;
265 |         }
266 |         num =I4_B(buff  );
267 |         len2=I4_B(buff+4)*2;
268 |         typ2=I4_L(buff+8);
269 |         
270 |         if (num!=i+1||len1!=len2||type!=typ2) {
271 |             trace(2,"shapefile record error n=%d %d len=%d %d type=%d %d\n",
272 |                   i+1,num,len1,len2,type,typ2);
273 |             continue;
274 |         }
275 |         if (type==1) { /* point */
276 |             read_pnt(fp,bound,data);
277 |         }
278 |         else if (type==8) { /* multi-point */
279 |             read_mpnt(fp,bound,data);
280 |         }
281 |         else if (type==3) { /* polyline */
282 |             read_poly(fp,bound,data);
283 |         }
284 |         else if (type==5) { /* polygon */
285 |             read_polygon(fp,bound,data);
286 |         }
287 |         else { /* skip record */
288 |             for (i=0;i<len1-4;i++) {
289 |                 size_t bytesRead = fread(buff,1,1,fp);
290 |             }
291 |         }
292 |     }
293 |     /* reverse list order */
294 |     for (p=*data,*data=NULL;p;p=next) {
295 |         next=p->next;
296 |         p->next=*data;
297 |         *data=p;
298 |     }
299 |     return 1;
300 | }
301 | /* read gis data from shapefile ------------------------------------------------
302 | * read gis data from shapefile (ref [1])
303 | * args   : char   *file     I   shapefile
304 | *          gis_t  *gis      IO  GIS data
305 | * return : status (0:error)
306 | * notes  : only support point, multipoint, polyline and polygon.
307 | *          only support lat-lon for map projection.
308 | *-----------------------------------------------------------------------------*/
309 | extern int gis_read(const char *file, gis_t *gis, int layer)
310 | {
311 |     FILE *fp,*fp_idx;
312 |     char path[1024],*p,*q;
313 |     int type1=0,type2=0;
314 |     
315 |     trace(3,"gis_read file=%s layer=%d\n",file,layer);
316 |     
317 |     strcpy(path,file);
318 |     
319 |     if ((p=strrchr(path,'.'))) {
320 |         sprintf(p,".shx");
321 |     }
322 |     else {
323 |         sprintf(path+strlen(path),".shx");
324 |     }
325 |     if (!(fp=fopen(file,"rb"))) { /* shapefile */
326 |         trace(2,"shapefile open error: %s\n",file);
327 |         return 0;
328 |     }
329 |     if (!(fp_idx=fopen(path,"rb"))) { /* index file */
330 |         fclose(fp);
331 |         trace(2,"shapefile index open error: %s\n",path);
332 |         return 0;
333 |     }
334 |     /* read header */
335 |     if ((type1=read_shape_head(fp))<0||(type2=read_shape_head(fp_idx))<0||
336 |         type1!=type2) {
337 |         trace(2,"shapefile header error: %s type=%d %d\n",file,type1,type2);
338 |         fclose(fp);
339 |         fclose(fp_idx);
340 |         return 0;
341 |     }
342 |     init_bound(gis->bound);
343 |     
344 |     /* read records */
345 |     if (!gis_read_record(fp,fp_idx,type1,gis->bound,gis->data+layer)) {
346 |         fclose(fp);
347 |         fclose(fp_idx);
348 |         return 0;
349 |     }
350 |     fclose(fp);
351 |     fclose(fp_idx);
352 |     gis->name[layer][0]='\0';
353 |     gis->flag[layer]=1;
354 |     return 1;
355 | }
356 | /* free gis-data ---------------------------------------------------------------
357 | * free and initialize gis data
358 | * args   : gis_t  *gis      IO  gis data
359 | * return : none
360 | *-----------------------------------------------------------------------------*/
361 | extern void gis_free(gis_t *gis)
362 | {
363 |     gisd_t *data,*next;
364 |     int i;
365 |     
366 |     for (i=0;i<MAXGISLAYER;i++) {
367 |         for (data=gis->data[i];data;data=next) {
368 |             next=data->next;
369 |             if (data->type==2) {
370 |                 free(((gis_poly_t *)data->data)->pos);
371 |             }
372 |             else if (data->type==3) {
373 |                 free(((gis_polygon_t *)data->data)->pos);
374 |             }
375 |             free(data);
376 |         }
377 |         gis->data[i]=NULL;
378 |         gis->name[i][0]='\0';
379 |         gis->flag[i]=0;
380 |     }
381 | }
382 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/ionex.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * ionex.c : ionex functions
  3 | *
  4 | *          Copyright (C) 2011-2013 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references:
  7 | *     [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange
  8 | *         Format Version 1, February 25, 1998
  9 | *     [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere
 10 | *         Maps based on GPS Carrier Phase Data Routinely producted by CODE
 11 | *         Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996
 12 | *
 13 | * version : $Revision:$ $Date:$
 14 | * history : 2011/03/29 1.0 new
 15 | *           2013/03/05 1.1 change api readtec()
 16 | *                          fix problem in case of lat>85deg or lat<-85deg
 17 | *           2014/02/22 1.2 fix problem on compiled as C++
 18 | *-----------------------------------------------------------------------------*/
 19 | #include "rtklib.h"
 20 | 
 21 | #define SQR(x)      ((x)*(x))
 22 | #define VAR_NOTEC   SQR(30.0)   /* variance of no tec */
 23 | #define MIN_EL      0.0         /* min elevation angle (rad) */
 24 | #define MIN_HGT     -1000.0     /* min user height (m) */
 25 | 
 26 | /* get index -----------------------------------------------------------------*/
 27 | static int getindex(double value, const double *range)
 28 | {
 29 |     if (range[2]==0.0) return 0;
 30 |     if (range[1]>0.0&&(value<range[0]||range[1]<value)) return -1;
 31 |     if (range[1]<0.0&&(value<range[1]||range[0]<value)) return -1;
 32 |     return (int)floor((value-range[0])/range[2]+0.5);
 33 | }
 34 | /* get number of items -------------------------------------------------------*/
 35 | static int nitem(const double *range)
 36 | {
 37 |     return getindex(range[1],range)+1;
 38 | }
 39 | /* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
 40 | static int dataindex(int i, int j, int k, const int *ndata)
 41 | {
 42 |     if (i<0||ndata[0]<=i||j<0||ndata[1]<=j||k<0||ndata[2]<=k) return -1;
 43 |     return i+ndata[0]*(j+ndata[1]*k);
 44 | }
 45 | /* add tec data to navigation data -------------------------------------------*/
 46 | static tec_t *addtec(const double *lats, const double *lons, const double *hgts,
 47 |                      double rb, nav_t *nav)
 48 | {
 49 |     tec_t *p,*nav_tec;
 50 |     gtime_t time0={0};
 51 |     int i,n,ndata[3],indx;
 52 |     
 53 |     trace(3,"addtec  :\n");
 54 |     
 55 |     ndata[0]=nitem(lats);
 56 |     ndata[1]=nitem(lons);
 57 |     ndata[2]=nitem(hgts);
 58 |     if (ndata[0] > 1 && ndata[1] > 1 && ndata[2] > 0)
 59 |     {
 60 |         if (nav->nt >= nav->ntmax) {
 61 |             nav->ntmax += 256;
 62 |             if (!(nav_tec = (tec_t*)realloc(nav->tec, sizeof(tec_t) * nav->ntmax))) {
 63 |                 trace(1, "readionex malloc error ntmax=%d\n", nav->ntmax);
 64 |                 free(nav->tec); nav->tec = NULL; nav->nt = nav->ntmax = 0;
 65 |                 return NULL;
 66 |             }
 67 |             for (indx = nav->ntmax - 1; indx >= nav->ntmax - 256; indx--)
 68 |                 memset(&nav_tec[indx], 0, sizeof(tec_t));
 69 |             nav->tec = nav_tec;
 70 |         }
 71 |         p = nav->tec + nav->nt;
 72 |         p->time = time0;
 73 |         p->rb = rb;
 74 |         for (i = 0; i < 3; i++) {
 75 |             p->ndata[i] = ndata[i];
 76 |             p->lats[i] = lats[i];
 77 |             p->lons[i] = lons[i];
 78 |             p->hgts[i] = hgts[i];
 79 |         }
 80 |         n = ndata[0] * ndata[1] * ndata[2];
 81 | 
 82 |         if (!(p->data = (double*)malloc(sizeof(double) * n)) ||
 83 |             !(p->rms = (float*)malloc(sizeof(float) * n))) {
 84 |             return NULL;
 85 |         }
 86 |         for (i = 0; i < n; i++) {
 87 |             /* Thanks to 'if (ndata[0]>1 && ndata[1]>1 && ndata[2]>0)' we know analysis is wrong - disable 6386 */
 88 |             p->data[i] = 0.0;
 89 |             p->rms[i] = 0.0f;
 90 |         }
 91 |         nav->nt++;
 92 |         return p;
 93 |     }
 94 |     else
 95 |         return NULL;
 96 | }
 97 | /* read ionex dcb aux data ----------------------------------------------------*/
 98 | static void readionexdcb(FILE *fp, double *dcb, double *rms)
 99 | {
100 |     int i,sat;
101 |     char buff[1024],id[32],*label;
102 |     
103 |     trace(3,"readionexdcb:\n");
104 |     
105 |     for (i=0;i<MAXSAT;i++) dcb[i]=rms[i]=0.0;
106 |     
107 |     while (fgets(buff,sizeof(buff),fp)) {
108 |         if (strlen(buff)<60) continue;
109 |         label=buff+60;
110 |         
111 |         if (strstr(label,"PRN / BIAS / RMS")==label) {
112 |             
113 |             strncpy(id,buff+3,3); id[3]='\0';
114 |             
115 |             if (!(sat=satid2no(id))) {
116 |                 trace(2,"ionex invalid satellite: %s\n",id);
117 |                 continue;
118 |             }
119 |             dcb[sat-1]=str2num(buff, 6,10);
120 |             rms[sat-1]=str2num(buff,16,10);
121 |         }
122 |         else if (strstr(label,"END OF AUX DATA")==label) break;
123 |     }
124 | }
125 | /* read ionex header ---------------------------------------------------------*/
126 | static double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
127 |                          double *rb, double *nexp, double *dcb, double *rms)
128 | {
129 |     double ver=0.0;
130 |     char buff[1024],*label;
131 |     
132 |     trace(3,"readionexh:\n");
133 |     
134 |     while (fgets(buff,sizeof(buff),fp)) {
135 |         
136 |         if (strlen(buff)<60) continue;
137 |         label=buff+60;
138 |         
139 |         if (strstr(label,"IONEX VERSION / TYPE")==label) {
140 |             if (buff[20]=='I') ver=str2num(buff,0,8);
141 |         }
142 |         else if (strstr(label,"BASE RADIUS")==label) {
143 |             *rb=str2num(buff,0,8);
144 |         }
145 |         else if (strstr(label,"HGT1 / HGT2 / DHGT")==label) {
146 |             hgts[0]=str2num(buff, 2,6);
147 |             hgts[1]=str2num(buff, 8,6);
148 |             hgts[2]=str2num(buff,14,6);
149 |         }
150 |         else if (strstr(label,"LAT1 / LAT2 / DLAT")==label) {
151 |             lats[0]=str2num(buff, 2,6);
152 |             lats[1]=str2num(buff, 8,6);
153 |             lats[2]=str2num(buff,14,6);
154 |         }
155 |         else if (strstr(label,"LON1 / LON2 / DLON")==label) {
156 |             lons[0]=str2num(buff, 2,6);
157 |             lons[1]=str2num(buff, 8,6);
158 |             lons[2]=str2num(buff,14,6);
159 |         }
160 |         else if (strstr(label,"EXPONENT")==label) {
161 |             *nexp=str2num(buff,0,6);
162 |         }
163 |         else if (strstr(label,"START OF AUX DATA")==label&&
164 |                  strstr(buff,"DIFFERENTIAL CODE BIASES")) {
165 |             readionexdcb(fp,dcb,rms);
166 |         }
167 |         else if (strstr(label,"END OF HEADER")==label) {
168 |             return ver;
169 |         }
170 |     }
171 |     return 0.0;
172 | }
173 | /* read ionex body -----------------------------------------------------------*/
174 | static int readionexb(FILE *fp, const double *lats, const double *lons,
175 |                       const double *hgts, double rb, double nexp, nav_t *nav)
176 | {
177 |     tec_t *p=NULL;
178 |     gtime_t time={0};
179 |     double lat,lon[3],hgt,x;
180 |     int i,j,k,n,m,index,type=0;
181 |     char buff[1024],*label=buff+60;
182 |     
183 |     trace(3,"readionexb:\n");
184 |     
185 |     while (fgets(buff,sizeof(buff),fp)) {
186 |         
187 |         if (strlen(buff)<60) continue;
188 |         
189 |         if (strstr(label,"START OF TEC MAP")==label) {
190 |             if ((p=addtec(lats,lons,hgts,rb,nav))) type=1;
191 |         }
192 |         else if (strstr(label,"END OF TEC MAP")==label) {
193 |             type=0;
194 |             p=NULL;
195 |         }
196 |         else if (strstr(label,"START OF RMS MAP")==label) {
197 |             type=2;
198 |             p=NULL;
199 |         }
200 |         else if (strstr(label,"END OF RMS MAP")==label) {
201 |             type=0;
202 |             p=NULL;
203 |         }
204 |         else if (strstr(label,"EPOCH OF CURRENT MAP")==label) {
205 |             if (str2time(buff,0,36,&time)) {
206 |                 trace(2,"ionex epoch invalid: %-36.36s\n",buff);
207 |                 continue;
208 |             }
209 |             if (type==2) {
210 |                 for (i=nav->nt-1;i>=0;i--) {
211 |                     if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue;
212 |                     p=nav->tec+i;
213 |                     break;
214 |                 }
215 |             }
216 |             else if (p) p->time=time;
217 |         }
218 |         else if (strstr(label,"LAT/LON1/LON2/DLON/H")==label&&p) {
219 |             lat   =str2num(buff, 2,6);
220 |             lon[0]=str2num(buff, 8,6);
221 |             lon[1]=str2num(buff,14,6);
222 |             lon[2]=str2num(buff,20,6);
223 |             hgt   =str2num(buff,26,6);
224 |             
225 |             i=getindex(lat,p->lats);
226 |             k=getindex(hgt,p->hgts);
227 |             n=nitem(lon);
228 |             
229 |             for (m=0;m<n;m++) {
230 |                 if (m%16==0&&!fgets(buff,sizeof(buff),fp)) break;
231 |                 
232 |                 j=getindex(lon[0]+lon[2]*m,p->lons);
233 |                 if ((index=dataindex(i,j,k,p->ndata))<0) continue;
234 |                 
235 |                 if ((x=str2num(buff,m%16*5,5))==9999.0) continue;
236 |                 
237 |                 if (type==1) p->data[index]=x*pow(10.0,nexp);
238 |                 else p->rms[index]=(float)(x*pow(10.0,nexp));
239 |             }
240 |         }
241 |     }
242 |     return 1;
243 | }
244 | /* combine tec grid data -----------------------------------------------------*/
245 | static void combtec(nav_t *nav)
246 | {
247 |     tec_t tmp;
248 |     int i,j,n=0;
249 |     
250 |     trace(3,"combtec : nav->nt=%d\n",nav->nt);
251 |     
252 |     for (i=0;i<nav->nt-1;i++) {
253 |         for (j=i+1;j<nav->nt;j++) {
254 |             if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) {
255 |                 tmp=nav->tec[i];
256 |                 nav->tec[i]=nav->tec[j];
257 |                 nav->tec[j]=tmp;
258 |             }
259 |         }
260 |     }
261 |     for (i=0;i<nav->nt;i++) {
262 |         if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) {
263 |             free(nav->tec[n-1].data);
264 |             free(nav->tec[n-1].rms );
265 |             nav->tec[n-1]=nav->tec[i];
266 |             continue;
267 |         }
268 |         nav->tec[n++]=nav->tec[i];
269 |     }
270 |     nav->nt=n;
271 |     
272 |     trace(4,"combtec : nav->nt=%d\n",nav->nt);
273 | }
274 | /* read ionex tec grid file ----------------------------------------------------
275 | * read ionex ionospheric tec grid file
276 | * args   : char   *file       I   ionex tec grid file
277 | *                                 (wind-card * is expanded)
278 | *          nav_t  *nav        IO  navigation data
279 | *                                 nav->nt, nav->ntmax and nav->tec are modified
280 | *          int    opt         I   read option (1: no clear of tec data,0:clear)
281 | * return : none
282 | * notes  : see ref [1]
283 | *-----------------------------------------------------------------------------*/
284 | extern void readtec(const char *file, nav_t *nav, int opt)
285 | {
286 |     FILE *fp;
287 |     double lats[3]={0},lons[3]={0},hgts[3]={0},rb=0.0,nexp=-1.0;
288 |     double dcb[MAXSAT]={0},rms[MAXSAT]={0};
289 |     int i,n;
290 |     char *efiles[MAXEXFILE];
291 |     
292 |     trace(3,"readtec : file=%s\n",file);
293 |     
294 |     /* clear of tec grid data option */
295 |     if (!opt) {
296 |         free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0;
297 |     }
298 |     for (i=0;i<MAXEXFILE;i++) {
299 |         if (!(efiles[i]=(char *)malloc(1024))) {
300 |             for (i--;i>=0;i--) free(efiles[i]);
301 |             return;
302 |         }
303 |     }
304 |     /* expand wild card in file path */
305 |     n=expath(file,efiles,MAXEXFILE);
306 |     
307 |     for (i=0;i<n;i++) {
308 |         if (!(fp=fopen(efiles[i],"r"))) {
309 |             trace(2,"ionex file open error %s\n",efiles[i]);
310 |             continue;
311 |         }
312 |         /* read ionex header */
313 |         if (readionexh(fp,lats,lons,hgts,&rb,&nexp,dcb,rms)<=0.0) {
314 |             trace(2,"ionex file format error %s\n",efiles[i]);
315 |             continue;
316 |         }
317 |         /* read ionex body */
318 |         readionexb(fp,lats,lons,hgts,rb,nexp,nav);
319 |         
320 |         fclose(fp);
321 |     }
322 |     for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
323 |     
324 |     /* combine tec grid data */
325 |     if (nav->nt>0) combtec(nav);
326 |     
327 |     /* P1-P2 dcb */
328 |     for (i=0;i<MAXSAT;i++) {
329 |         nav->cbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */
330 |     }
331 | }
332 | /* interpolate tec grid data -------------------------------------------------*/
333 | static int interptec(const tec_t *tec, int k, const double *posp, double *value,
334 |                      double *rms)
335 | {
336 |     double dlat,dlon,a,b,d[4]={0},r[4]={0};
337 |     int i,j,n,index;
338 |     
339 |     trace(3,"interptec: k=%d posp=%.2f %.2f\n",k,posp[0]*R2D,posp[1]*R2D);
340 |     *value=*rms=0.0;
341 |     
342 |     if (tec->lats[2]==0.0||tec->lons[2]==0.0) return 0;
343 |     
344 |     dlat=posp[0]*R2D-tec->lats[0];
345 |     dlon=posp[1]*R2D-tec->lons[0];
346 |     if (tec->lons[2]>0.0) dlon-=floor( dlon/360)*360.0; /*  0<=dlon<360 */
347 |     else                  dlon+=floor(-dlon/360)*360.0; /* -360<dlon<=0 */
348 |     
349 |     a=dlat/tec->lats[2];
350 |     b=dlon/tec->lons[2];
351 |     i=(int)floor(a); a-=i;
352 |     j=(int)floor(b); b-=j;
353 |     
354 |     /* get gridded tec data */
355 |     for (n=0;n<4;n++) {
356 |         if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue;
357 |         d[n]=tec->data[index];
358 |         r[n]=tec->rms [index];
359 |     }
360 |     if (d[0]>0.0&&d[1]>0.0&&d[2]>0.0&&d[3]>0.0) {
361 |         
362 |         /* bilinear interpolation (inside of grid) */
363 |         *value=(1.0-a)*(1.0-b)*d[0]+a*(1.0-b)*d[1]+(1.0-a)*b*d[2]+a*b*d[3];
364 |         *rms  =(1.0-a)*(1.0-b)*r[0]+a*(1.0-b)*r[1]+(1.0-a)*b*r[2]+a*b*r[3];
365 |     }
366 |     /* nearest-neighbour extrapolation (outside of grid) */
367 |     else if (a<=0.5&&b<=0.5&&d[0]>0.0) {*value=d[0]; *rms=r[0];}
368 |     else if (a> 0.5&&b<=0.5&&d[1]>0.0) {*value=d[1]; *rms=r[1];}
369 |     else if (a<=0.5&&b> 0.5&&d[2]>0.0) {*value=d[2]; *rms=r[2];}
370 |     else if (a> 0.5&&b> 0.5&&d[3]>0.0) {*value=d[3]; *rms=r[3];}
371 |     else {
372 |         i=0;
373 |         for (n=0;n<4;n++) if (d[n]>0.0) {i++; *value+=d[n]; *rms+=r[n];}
374 |         if(i==0) return 0;
375 |         *value/=i; *rms/=i;
376 |     }
377 |     return 1;
378 | }
379 | /* ionosphere delay by tec grid data -----------------------------------------*/
380 | static int iondelay(gtime_t time, const tec_t *tec, const double *pos,
381 |                     const double *azel, int opt, double *delay, double *var)
382 | {
383 |     const double fact=40.30E16/FREQL1/FREQL1; /* tecu->L1 iono (m) */
384 |     double fs,posp[3]={0},vtec,rms,hion,rp;
385 |     int i;
386 |     
387 |     trace(3,"iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0),
388 |           pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
389 |     
390 |     *delay=*var=0.0;
391 |     
392 |     for (i=0;i<tec->ndata[2];i++) { /* for a layer */
393 |         
394 |         hion=tec->hgts[0]+tec->hgts[2]*i;
395 |         
396 |         /* ionospheric pierce point position */
397 |         fs=ionppp(pos,azel,tec->rb,hion,posp);
398 |         
399 |         if (opt&2) {
400 |             /* modified single layer mapping function (M-SLM) ref [2] */
401 |             rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1]));
402 |             fs=1.0/sqrt(1.0-rp*rp);
403 |         }
404 |         if (opt&1) {
405 |             /* earth rotation correction (sun-fixed coordinate) */
406 |             posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0;
407 |         }
408 |         /* interpolate tec grid data */
409 |         if (!interptec(tec,i,posp,&vtec,&rms)) return 0;
410 |         
411 |         *delay+=fact*fs*vtec;
412 |         *var+=fact*fact*fs*fs*rms*rms;
413 |     }
414 |     trace(4,"iondelay: delay=%7.2f std=%6.2f\n",*delay,sqrt(*var));
415 |     
416 |     return 1;
417 | }
418 | /* ionosphere model by tec grid data -------------------------------------------
419 | * compute ionospheric delay by tec grid data
420 | * args   : gtime_t time     I   time (gpst)
421 | *          nav_t  *nav      I   navigation data
422 | *          double *pos      I   receiver position {lat,lon,h} (rad,m)
423 | *          double *azel     I   azimuth/elevation angle {az,el} (rad)
424 | *          int    opt       I   model option
425 | *                                bit0: 0:earth-fixed,1:sun-fixed
426 | *                                bit1: 0:single-layer,1:modified single-layer
427 | *          double *delay    O   ionospheric delay (L1) (m)
428 | *          double *var      O   ionospheric dealy (L1) variance (m^2)
429 | * return : status (1:ok,0:error)
430 | * notes  : before calling the function, read tec grid data by calling readtec()
431 | *          return ok with delay=0 and var=VAR_NOTEC if el<MIN_EL or h<MIN_HGT
432 | *-----------------------------------------------------------------------------*/
433 | extern int iontec(gtime_t time, const nav_t *nav, const double *pos,
434 |                   const double *azel, int opt, double *delay, double *var)
435 | {
436 |     double dels[2],vars[2],a,tt;
437 |     int i,stat[2];
438 |     
439 |     trace(3,"iontec  : time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0),
440 |           pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
441 |     
442 |     if (azel[1]<MIN_EL||pos[2]<MIN_HGT) {
443 |         *delay=0.0;
444 |         *var=VAR_NOTEC;
445 |         return 1;
446 |     }
447 |     for (i=0;i<nav->nt;i++) {
448 |         if (timediff(nav->tec[i].time,time)>0.0) break;
449 |     }
450 |     if (i==0||i>=nav->nt) {
451 |         trace(2,"%s: tec grid out of period\n",time_str(time,0));
452 |         return 0;
453 |     }
454 |     if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) {
455 |         trace(2,"tec grid time interval error\n");
456 |         return 0;
457 |     }
458 |     /* ionospheric delay by tec grid data */
459 |     stat[0]=iondelay(time,nav->tec+i-1,pos,azel,opt,dels  ,vars  );
460 |     stat[1]=iondelay(time,nav->tec+i  ,pos,azel,opt,dels+1,vars+1);
461 |     
462 |     if (!stat[0]&&!stat[1]) {
463 |         trace(2,"%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n",
464 |               time_str(time,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
465 |         return 0;
466 |     }
467 |     if (stat[0]&&stat[1]) { /* linear interpolation by time */
468 |         a=timediff(time,nav->tec[i-1].time)/tt;
469 |         *delay=dels[0]*(1.0-a)+dels[1]*a;
470 |         *var  =vars[0]*(1.0-a)+vars[1]*a;
471 |     }
472 |     else if (stat[0]) { /* nearest-neighbour extrapolation by time */
473 |         *delay=dels[0];
474 |         *var  =vars[0];
475 |     }
476 |     else {
477 |         *delay=dels[1];
478 |         *var  =vars[1];
479 |     }
480 |     trace(3,"iontec  : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var));
481 |     return 1;
482 | }
483 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/lambda.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * lambda.c : integer ambiguity resolution
  3 | *
  4 | *          Copyright (C) 2007-2008 by T.TAKASU, All rights reserved.
  5 | *
  6 | * reference :
  7 | *     [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment:
  8 | *         a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82,
  9 | *         1995
 10 | *     [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for
 11 | *         integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005
 12 | *
 13 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
 14 | * history : 2007/01/13 1.0 new
 15 | *           2015/05/31 1.1 add api lambda_reduction(), lambda_search()
 16 | *-----------------------------------------------------------------------------*/
 17 | #include "rtklib.h"
 18 | 
 19 | /* constants/macros ----------------------------------------------------------*/
 20 | 
 21 | #define LOOPMAX     10000           /* maximum count of search loop */
 22 | 
 23 | #define SGN(x)      ((x)<=0.0?-1.0:1.0)
 24 | #define ROUND(x)    (floor((x)+0.5))
 25 | #define SWAP(x,y)   do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0)
 26 | 
 27 | /* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
 28 | static int LD(int n, const double *Q, double *L, double *D)
 29 | {
 30 |     int i,j,k,info=0;
 31 |     double a,*A=mat(n,n);
 32 |     
 33 |     memcpy(A,Q,sizeof(double)*n*n);
 34 |     for (i=n-1;i>=0;i--) {
 35 |         if ((D[i]=A[i+i*n])<=0.0) {info=-1; break;}
 36 |         a=sqrt(D[i]);
 37 |         for (j=0;j<=i;j++) L[i+j*n]=A[i+j*n]/a;
 38 |         for (j=0;j<=i-1;j++) for (k=0;k<=j;k++) A[j+k*n]-=L[i+k*n]*L[i+j*n];
 39 |         for (j=0;j<=i;j++) L[i+j*n]/=L[i+i*n];
 40 |     }
 41 |     free(A);
 42 |     if (info) fprintf(stderr,"%s : LD factorization error\n",__FILE__);
 43 |     return info;
 44 | }
 45 | /* integer gauss transformation ----------------------------------------------*/
 46 | static void gauss(int n, double *L, double *Z, int i, int j)
 47 | {
 48 |     int k,mu;
 49 |     
 50 |     if ((mu=(int)ROUND(L[i+j*n]))!=0) {
 51 |         for (k=i;k<n;k++) L[k+n*j]-=(double)mu*L[k+i*n];
 52 |         for (k=0;k<n;k++) Z[k+n*j]-=(double)mu*Z[k+i*n];
 53 |     }
 54 | }
 55 | /* permutations --------------------------------------------------------------*/
 56 | static void perm(int n, double *L, double *D, int j, double del, double *Z)
 57 | {
 58 |     int k;
 59 |     double eta,lam,a0,a1;
 60 |     
 61 |     eta=D[j]/del;
 62 |     lam=D[j+1]*L[j+1+j*n]/del;
 63 |     D[j]=eta*D[j+1]; D[j+1]=del;
 64 |     for (k=0;k<=j-1;k++) {
 65 |         a0=L[j+k*n]; a1=L[j+1+k*n];
 66 |         L[j+k*n]=-L[j+1+j*n]*a0+a1;
 67 |         L[j+1+k*n]=eta*a0+lam*a1;
 68 |     }
 69 |     L[j+1+j*n]=lam;
 70 |     for (k=j+2;k<n;k++) SWAP(L[k+j*n],L[k+(j+1)*n]);
 71 |     for (k=0;k<n;k++) SWAP(Z[k+j*n],Z[k+(j+1)*n]);
 72 | }
 73 | /* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
 74 | static void reduction(int n, double *L, double *D, double *Z)
 75 | {
 76 |     int i,j,k;
 77 |     double del;
 78 |     
 79 |     j=n-2; k=n-2;
 80 |     while (j>=0) {
 81 |         if (j<=k) for (i=j+1;i<n;i++) gauss(n,L,Z,i,j);
 82 |         del=D[j]+L[j+1+j*n]*L[j+1+j*n]*D[j+1];
 83 |         if (del+1E-6<D[j+1]) { /* compared considering numerical error */
 84 |             perm(n,L,D,j,del,Z);
 85 |             k=j; j=n-2;
 86 |         }
 87 |         else j--;
 88 |     }
 89 | }
 90 | /* modified lambda (mlambda) search (ref. [2]) -------------------------------
 91 | * args   : n      I  number of float parameters
 92 | *          m      I  number of fixed solution
 93 |            L,D    I  transformed covariance matrix
 94 |            zs     I  transformed double-diff phase biases
 95 |            zn     O  fixed solutions
 96 |            s      O  sum of residuals for fixed solutions                    */
 97 | static int search(int n, int m, const double *L, const double *D,
 98 |                   const double *zs, double *zn, double *s)
 99 | {
100 |     int i,j,k,c,nn=0,imax=0;
101 |     double newdist,maxdist=1E99,y;
102 |     double *S=zeros(n,n),*dist=mat(n,1),*zb=mat(n,1),*z=mat(n,1),*step=mat(n,1);
103 |     
104 |     k=n-1; dist[k]=0.0;
105 |     zb[k]=zs[k];
106 |     z[k]=ROUND(zb[k]);
107 |     y=zb[k]-z[k];
108 |     step[k]=SGN(y);  /* step towards closest integer */
109 |     for (c=0;c<LOOPMAX;c++) {
110 |         newdist=dist[k]+y*y/D[k];  /* newdist=sum(((z(j)-zb(j))^2/d(j))) */
111 |         if (newdist<maxdist) {
112 |             /* Case 1: move down */
113 |             if (k!=0) {
114 |                 dist[--k]=newdist;
115 |                 for (i=0;i<=k;i++)
116 |                     S[k+i*n]=S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];
117 |                 zb[k]=zs[k]+S[k+k*n];
118 |                 z[k]=ROUND(zb[k]); /* next valid integer */
119 |                 y=zb[k]-z[k];
120 |                 step[k]=SGN(y);
121 |             }
122 |             /* Case 2: store the found candidate and try next valid integer */
123 |             else {
124 |                 if (nn<m) {  /* store the first m initial points */
125 |                     if (nn==0||newdist>s[imax]) imax=nn;
126 |                     for (i=0;i<n;i++) zn[i+nn*n]=z[i];
127 |                     s[nn++]=newdist;
128 |                 }
129 |                 else {
130 |                     if (newdist<s[imax]) {
131 |                         for (i=0;i<n;i++) zn[i+imax*n]=z[i];
132 |                         s[imax]=newdist;
133 |                         for (i=imax=0;i<m;i++) if (s[imax]<s[i]) imax=i;
134 |                     }
135 |                     maxdist=s[imax];
136 |                 }
137 |                 z[0]+=step[0]; /* next valid integer */
138 |                 y=zb[0]-z[0];
139 |                 step[0]=-step[0]-SGN(step[0]);
140 |             }
141 |         }
142 |         /* Case 3: exit or move up */
143 |         else {
144 |             if (k==n-1) break;
145 |             else {
146 |                 k++;  /* move up */
147 |                 z[k]+=step[k];  /* next valid integer */
148 |                 y=zb[k]-z[k];
149 |                 step[k]=-step[k]-SGN(step[k]);
150 |             }
151 |         }
152 |     }
153 |     for (i=0;i<m-1;i++) { /* sort by s */
154 |         for (j=i+1;j<m;j++) {
155 |             if (s[i]<s[j]) continue;
156 |             SWAP(s[i],s[j]);
157 |             for (k=0;k<n;k++) SWAP(zn[k+i*n],zn[k+j*n]);
158 |         }
159 |     }
160 |     free(S); free(dist); free(zb); free(z); free(step);
161 |     
162 |     if (c>=LOOPMAX) {
163 |         fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
164 |         return -2;
165 |     }
166 |     return 0;
167 | }
168 | /* lambda/mlambda integer least-square estimation ------------------------------
169 | * integer least-square estimation. reduction is performed by lambda (ref.[1]),
170 | * and search by mlambda (ref.[2]).
171 | * args   : int    n      I  number of float parameters
172 | *          int    m      I  number of fixed solutions
173 | *          double *a     I  float parameters (n x 1) (double-diff phase biases)
174 | *          double *Q     I  covariance matrix of float parameters (n x n)
175 | *          double *F     O  fixed solutions (n x m)
176 | *          double *s     O  sum of squared residulas of fixed solutions (1 x m)
177 | * return : status (0:ok,other:error)
178 | * notes  : matrix stored by column-major order (fortran convension)
179 | *-----------------------------------------------------------------------------*/
180 | extern int lambda(int n, int m, const double *a, const double *Q, double *F,
181 |                   double *s)
182 | {
183 |     int info;
184 |     double *L,*D,*Z,*z,*E;
185 |     
186 |     if (n<=0||m<=0) return -1;
187 |     L=zeros(n,n); D=mat(n,1); Z=eye(n); z=mat(n,1); E=mat(n,m);
188 |     
189 |     /* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */
190 |     if (!(info=LD(n,Q,L,D))) {
191 |         
192 |         /* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */
193 |         reduction(n,L,D,Z);
194 |         matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
195 |         
196 |         /* mlambda search 
197 |             z = transformed double-diff phase biases
198 |             L,D = transformed covariance matrix */
199 |         if (!(info=search(n,m,L,D,z,E,s))) {  /* returns 0 if no error */
200 |             
201 |             info=solve("T",Z,E,n,m,F); /* F=Z'\E */
202 |         }
203 |     }
204 |     free(L); free(D); free(Z); free(z); free(E);
205 |     return info;
206 | }
207 | /* lambda reduction ------------------------------------------------------------
208 | * reduction by lambda (ref [1]) for integer least square
209 | * args   : int    n      I  number of float parameters
210 | *          double *Q     I  covariance matrix of float parameters (n x n)
211 | *          double *Z     O  lambda reduction matrix (n x n)
212 | * return : status (0:ok,other:error)
213 | *-----------------------------------------------------------------------------*/
214 | extern int lambda_reduction(int n, const double *Q, double *Z)
215 | {
216 |     double *L,*D;
217 |     int i,j,info;
218 |     
219 |     if (n<=0) return -1;
220 |     
221 |     L=zeros(n,n); D=mat(n,1);
222 |     
223 |     for (i=0;i<n;i++) for (j=0;j<n;j++) {
224 |         Z[i+j*n]=i==j?1.0:0.0;
225 |     }
226 |     /* LD factorization */
227 |     if ((info=LD(n,Q,L,D))) {
228 |         free(L); free(D);
229 |         return info;
230 |     }
231 |     /* lambda reduction */
232 |     reduction(n,L,D,Z);
233 |      
234 |     free(L); free(D);
235 |     return 0;
236 | }
237 | /* mlambda search --------------------------------------------------------------
238 | * search by  mlambda (ref [2]) for integer least square
239 | * args   : int    n      I  number of float parameters
240 | *          int    m      I  number of fixed solutions
241 | *          double *a     I  float parameters (n x 1)
242 | *          double *Q     I  covariance matrix of float parameters (n x n)
243 | *          double *F     O  fixed solutions (n x m)
244 | *          double *s     O  sum of squared residulas of fixed solutions (1 x m)
245 | * return : status (0:ok,other:error)
246 | *-----------------------------------------------------------------------------*/
247 | extern int lambda_search(int n, int m, const double *a, const double *Q,
248 |                          double *F, double *s)
249 | {
250 |     double *L,*D;
251 |     int info;
252 |     
253 |     if (n<=0||m<=0) return -1;
254 |     
255 |     L=zeros(n,n); D=mat(n,1);
256 |     
257 |     /* LD factorization */
258 |     if ((info=LD(n,Q,L,D))) {
259 |         free(L); free(D);
260 |         return info;
261 |     }
262 |     /* mlambda search */
263 |     info=search(n,m,L,D,a,F,s);
264 |     
265 |     free(L); free(D);
266 |     return info;
267 | }
268 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/options.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * options.c : options functions
  3 | *
  4 | *          Copyright (C) 2010-2020 by T.TAKASU, All rights reserved.
  5 | *
  6 | * version : $Revision:$ $Date:$
  7 | * history : 2010/07/20  1.1  moved from postpos.c
  8 | *                            added api:
  9 | *                                searchopt(),str2opt(),opt2str(),opt2buf(),
 10 | *                                loadopts(),saveopts(),resetsysopts(),
 11 | *                                getsysopts(),setsysopts()
 12 | *           2010/09/11  1.2  add options
 13 | *                                pos2-elmaskhold,pos1->snrmaskena
 14 | *                                pos1-snrmask1,2,3
 15 | *           2013/03/11  1.3  add pos1-posopt1,2,3,4,5,pos2-syncsol
 16 | *                                misc-rnxopt1,2,pos1-snrmask_r,_b,_L1,_L2,_L5
 17 | *           2014/10/21  1.4  add pos2-bdsarmode
 18 | *           2015/02/20  1.4  add ppp-fixed as pos1-posmode option
 19 | *           2015/05/10  1.5  add pos2-arthres1,2,3,4
 20 | *           2015/05/31  1.6  add pos2-armaxiter, pos1-posopt6
 21 | *                            add selection precise for pos1-pospot3
 22 | *           2015/11/26  1.7  modify pos1-frequency 4:l1+l2+l5+l6 -> l1+l5
 23 | *           2015/12/05  1.8  add misc-pppopt
 24 | *           2016/06/10  1.9  add ant2-maxaveep,ant2-initrst
 25 | *           2016/07/31  1.10 add out-outsingle,out-maxsolstd
 26 | *           2017/06/14  1.11 add out-outvel
 27 | *           2020/11/30  1.12 change options pos1-frequency, pos1-ionoopt,
 28 | *                             pos1-tropopt, pos1-sateph, pos1-navsys,
 29 | *                             pos2-gloarmode,
 30 | *-----------------------------------------------------------------------------*/
 31 | #include "rtklib.h"
 32 | 
 33 | /* system options buffer -----------------------------------------------------*/
 34 | static prcopt_t prcopt_;
 35 | static solopt_t solopt_;
 36 | static filopt_t filopt_;
 37 | static int antpostype_[2];
 38 | static double elmask_,elmaskar_,elmaskhold_;
 39 | static double antpos_[2][3];
 40 | static char exsats_[1024];
 41 | static char snrmask_[NFREQ][1024];
 42 | 
 43 | /* system options table ------------------------------------------------------*/
 44 | #define SWTOPT  "0:off,1:on"
 45 | #define MODOPT  "0:single,1:dgps,2:kinematic,3:static,4:static-start,5:movingbase,6:fixed,7:ppp-kine,8:ppp-static,9:ppp-fixed"
 46 | #define FRQOPT  "1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6"
 47 | #define TYPOPT  "0:forward,1:backward,2:combined,3:combined-nophasereset"
 48 | #define IONOPT  "0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec,5:ionex-tec,6:qzs-brdc"
 49 | #define TRPOPT  "0:off,1:saas,2:sbas,3:est-ztd,4:est-ztdgrad"
 50 | #define EPHOPT  "0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom"
 51 | #define NAVOPT  "1:gps+2:sbas+4:glo+8:gal+16:qzs+32:bds+64:navic"
 52 | #define GAROPT  "0:off,1:on,2:autocal,3:fix-and-hold"
 53 | #define WEIGHTOPT "0:elevation,1:snr"
 54 | #define SOLOPT  "0:llh,1:xyz,2:enu,3:nmea"
 55 | #define TSYOPT  "0:gpst,1:utc,2:jst"
 56 | #define TFTOPT  "0:tow,1:hms"
 57 | #define DFTOPT  "0:deg,1:dms"
 58 | #define HGTOPT  "0:ellipsoidal,1:geodetic"
 59 | #define GEOOPT  "0:internal,1:egm96,2:egm08_2.5,3:egm08_1,4:gsi2000"
 60 | #define STAOPT  "0:all,1:single"
 61 | #define STSOPT  "0:off,1:state,2:residual"
 62 | #define ARMOPT  "0:off,1:continuous,2:instantaneous,3:fix-and-hold"
 63 | #define POSOPT  "0:llh,1:xyz,2:single,3:posfile,4:rinexhead,5:rtcm,6:raw"
 64 | #define TIDEOPT "0:off,1:on,2:otl"
 65 | #define PHWOPT  "0:off,1:on,2:precise"
 66 | 
 67 | opt_t sysopts[]={
 68 |     {"pos1-posmode",    3,  (void *)&prcopt_.mode,       MODOPT },
 69 |     {"pos1-frequency",  3,  (void *)&prcopt_.nf,         FRQOPT },
 70 |     {"pos1-soltype",    3,  (void *)&prcopt_.soltype,    TYPOPT },
 71 |     {"pos1-elmask",     1,  (void *)&elmask_,            "deg"  },
 72 |     {"pos1-snrmask_r",  3,  (void *)&prcopt_.snrmask.ena[0],SWTOPT},
 73 |     {"pos1-snrmask_b",  3,  (void *)&prcopt_.snrmask.ena[1],SWTOPT},
 74 |     {"pos1-snrmask_L1", 2,  (void *)snrmask_[0],         ""     },
 75 |     {"pos1-snrmask_L2", 2,  (void *)snrmask_[1],         ""     },
 76 |     {"pos1-snrmask_L5", 2,  (void *)snrmask_[2],         ""     },
 77 |     {"pos1-dynamics",   3,  (void *)&prcopt_.dynamics,   SWTOPT },
 78 |     {"pos1-tidecorr",   3,  (void *)&prcopt_.tidecorr,   TIDEOPT},
 79 |     {"pos1-ionoopt",    3,  (void *)&prcopt_.ionoopt,    IONOPT },
 80 |     {"pos1-tropopt",    3,  (void *)&prcopt_.tropopt,    TRPOPT },
 81 |     {"pos1-sateph",     3,  (void *)&prcopt_.sateph,     EPHOPT },
 82 |     {"pos1-posopt1",    3,  (void *)&prcopt_.posopt[0],  SWTOPT },
 83 |     {"pos1-posopt2",    3,  (void *)&prcopt_.posopt[1],  SWTOPT },
 84 |     {"pos1-posopt3",    3,  (void *)&prcopt_.posopt[2],  PHWOPT },
 85 |     {"pos1-posopt4",    3,  (void *)&prcopt_.posopt[3],  SWTOPT },
 86 |     {"pos1-posopt5",    3,  (void *)&prcopt_.posopt[4],  SWTOPT },
 87 |     {"pos1-posopt6",    3,  (void *)&prcopt_.posopt[5],  SWTOPT },
 88 |     {"pos1-exclsats",   2,  (void *)exsats_,             "prn ..."},
 89 |     {"pos1-navsys",     0,  (void *)&prcopt_.navsys,     NAVOPT },
 90 |     
 91 |     {"pos2-armode",     3,  (void *)&prcopt_.modear,     ARMOPT },
 92 |     {"pos2-gloarmode",  3,  (void *)&prcopt_.glomodear,  GAROPT },
 93 |     {"pos2-bdsarmode",  3,  (void *)&prcopt_.bdsmodear,  SWTOPT },
 94 |     {"pos2-arfilter",   3,  (void *)&prcopt_.arfilter,   SWTOPT },
 95 |     {"pos2-arthres",    1,  (void *)&prcopt_.thresar[0], ""     },
 96 |     {"pos2-arthresmin", 1,  (void *)&prcopt_.thresar[5], ""     },
 97 |     {"pos2-arthresmax", 1,  (void *)&prcopt_.thresar[6], ""     },
 98 |     {"pos2-arthres1",   1,  (void *)&prcopt_.thresar[1], ""     },
 99 |     {"pos2-arthres2",   1,  (void *)&prcopt_.thresar[2], ""     },
100 |     {"pos2-arthres3",   1,  (void *)&prcopt_.thresar[3], ""     },
101 |     {"pos2-arthres4",   1,  (void *)&prcopt_.thresar[4], ""     },
102 |     {"pos2-varholdamb", 1,  (void *)&prcopt_.varholdamb, "cyc^2"},
103 |     {"pos2-gainholdamb",1,  (void *)&prcopt_.gainholdamb,""     },
104 |     {"pos2-arlockcnt",  0,  (void *)&prcopt_.minlock,    ""     },
105 |     {"pos2-minfixsats", 0,  (void *)&prcopt_.minfixsats, ""     },
106 |     {"pos2-minholdsats",0,  (void *)&prcopt_.minholdsats,""     },
107 |     {"pos2-mindropsats",0,  (void *)&prcopt_.mindropsats,""     },
108 |     {"pos2-arelmask",   1,  (void *)&elmaskar_,          "deg"  },
109 |     {"pos2-arminfix",   0,  (void *)&prcopt_.minfix,     ""     },
110 |     {"pos2-armaxiter",  0,  (void *)&prcopt_.armaxiter,  ""     },
111 |     {"pos2-elmaskhold", 1,  (void *)&elmaskhold_,        "deg"  },
112 |     {"pos2-aroutcnt",   0,  (void *)&prcopt_.maxout,     ""     },
113 |     {"pos2-maxage",     1,  (void *)&prcopt_.maxtdiff,   "s"    },
114 |     {"pos2-syncsol",    3,  (void *)&prcopt_.syncsol,    SWTOPT },
115 |     {"pos2-slipthres",  1,  (void *)&prcopt_.thresslip,  "m"    },
116 |     {"pos2-dopthres",   1,  (void *)&prcopt_.thresdop,   "m"    },
117 |     {"pos2-rejionno",   1,  (void *)&prcopt_.maxinno[0], "m"    },
118 |     {"pos2-rejcode",    1,  (void *)&prcopt_.maxinno[1], "m"    },
119 |     {"pos2-niter",      0,  (void *)&prcopt_.niter,      ""     },
120 |     {"pos2-baselen",    1,  (void *)&prcopt_.baseline[0],"m"    },
121 |     {"pos2-basesig",    1,  (void *)&prcopt_.baseline[1],"m"    },
122 |     
123 |     {"out-solformat",   3,  (void *)&solopt_.posf,       SOLOPT },
124 |     {"out-outhead",     3,  (void *)&solopt_.outhead,    SWTOPT },
125 |     {"out-outopt",      3,  (void *)&solopt_.outopt,     SWTOPT },
126 |     {"out-outvel",      3,  (void *)&solopt_.outvel,     SWTOPT },
127 |     {"out-timesys",     3,  (void *)&solopt_.times,      TSYOPT },
128 |     {"out-timeform",    3,  (void *)&solopt_.timef,      TFTOPT },
129 |     {"out-timendec",    0,  (void *)&solopt_.timeu,      ""     },
130 |     {"out-degform",     3,  (void *)&solopt_.degf,       DFTOPT },
131 |     {"out-fieldsep",    2,  (void *) solopt_.sep,        ""     },
132 |     {"out-outsingle",   3,  (void *)&prcopt_.outsingle,  SWTOPT },
133 |     {"out-maxsolstd",   1,  (void *)&solopt_.maxsolstd,  "m"    },
134 |     {"out-height",      3,  (void *)&solopt_.height,     HGTOPT },
135 |     {"out-geoid",       3,  (void *)&solopt_.geoid,      GEOOPT },
136 |     {"out-solstatic",   3,  (void *)&solopt_.solstatic,  STAOPT },
137 |     {"out-nmeaintv1",   1,  (void *)&solopt_.nmeaintv[0],"s"    },
138 |     {"out-nmeaintv2",   1,  (void *)&solopt_.nmeaintv[1],"s"    },
139 |     {"out-outstat",     3,  (void *)&solopt_.sstat,      STSOPT },
140 |     {"stats-eratio1",   1,  (void *)&prcopt_.eratio[0],  ""     },
141 |     {"stats-eratio2",   1,  (void *)&prcopt_.eratio[1],  ""     },
142 |     {"stats-eratio5",   1,  (void *)&prcopt_.eratio[2],  ""     },
143 |     {"stats-errphase",  1,  (void *)&prcopt_.err[1],     "m"    },
144 |     {"stats-errphaseel",1,  (void *)&prcopt_.err[2],     "m"    },
145 |     {"stats-errphasebl",1,  (void *)&prcopt_.err[3],     "m/10km"},
146 |     {"stats-errdoppler",1,  (void *)&prcopt_.err[4],     "Hz"   },
147 |     {"stats-snrmax",    1,  (void *)&prcopt_.err[5],     "dB.Hz"},
148 |     {"stats-errsnr",    1,  (void *)&prcopt_.err[6],     "m"    },
149 |     {"stats-errrcv",    1,  (void *)&prcopt_.err[7],     " "    },
150 |     {"stats-stdbias",   1,  (void *)&prcopt_.std[0],     "m"    },
151 |     {"stats-stdiono",   1,  (void *)&prcopt_.std[1],     "m"    },
152 |     {"stats-stdtrop",   1,  (void *)&prcopt_.std[2],     "m"    },
153 |     {"stats-prnaccelh", 1,  (void *)&prcopt_.prn[3],     "m/s^2"},
154 |     {"stats-prnaccelv", 1,  (void *)&prcopt_.prn[4],     "m/s^2"},
155 |     {"stats-prnbias",   1,  (void *)&prcopt_.prn[0],     "m"    },
156 |     {"stats-prniono",   1,  (void *)&prcopt_.prn[1],     "m"    },
157 |     {"stats-prntrop",   1,  (void *)&prcopt_.prn[2],     "m"    },
158 |     {"stats-prnpos",    1,  (void *)&prcopt_.prn[5],     "m"    },
159 |     {"stats-clkstab",   1,  (void *)&prcopt_.sclkstab,   "s/s"  },
160 |     
161 |     {"ant1-postype",    3,  (void *)&antpostype_[0],     POSOPT },
162 |     {"ant1-pos1",       1,  (void *)&antpos_[0][0],      "deg|m"},
163 |     {"ant1-pos2",       1,  (void *)&antpos_[0][1],      "deg|m"},
164 |     {"ant1-pos3",       1,  (void *)&antpos_[0][2],      "m|m"  },
165 |     {"ant1-anttype",    2,  (void *)prcopt_.anttype[0],  ""     },
166 |     {"ant1-antdele",    1,  (void *)&prcopt_.antdel[0][0],"m"   },
167 |     {"ant1-antdeln",    1,  (void *)&prcopt_.antdel[0][1],"m"   },
168 |     {"ant1-antdelu",    1,  (void *)&prcopt_.antdel[0][2],"m"   },
169 |     
170 |     {"ant2-postype",    3,  (void *)&antpostype_[1],     POSOPT },
171 |     {"ant2-pos1",       1,  (void *)&antpos_[1][0],      "deg|m"},
172 |     {"ant2-pos2",       1,  (void *)&antpos_[1][1],      "deg|m"},
173 |     {"ant2-pos3",       1,  (void *)&antpos_[1][2],      "m|m"  },
174 |     {"ant2-anttype",    2,  (void *)prcopt_.anttype[1],  ""     },
175 |     {"ant2-antdele",    1,  (void *)&prcopt_.antdel[1][0],"m"   },
176 |     {"ant2-antdeln",    1,  (void *)&prcopt_.antdel[1][1],"m"   },
177 |     {"ant2-antdelu",    1,  (void *)&prcopt_.antdel[1][2],"m"   },
178 |     {"ant2-maxaveep",   0,  (void *)&prcopt_.maxaveep    ,""    },
179 |     {"ant2-initrst",    3,  (void *)&prcopt_.initrst,    SWTOPT },
180 |     
181 |     {"misc-timeinterp", 3,  (void *)&prcopt_.intpref,    SWTOPT },
182 |     {"misc-sbasatsel",  0,  (void *)&prcopt_.sbassatsel, "0:all"},
183 |     {"misc-rnxopt1",    2,  (void *)prcopt_.rnxopt[0],   ""     },
184 |     {"misc-rnxopt2",    2,  (void *)prcopt_.rnxopt[1],   ""     },
185 |     {"misc-pppopt",     2,  (void *)prcopt_.pppopt,      ""     },
186 |     
187 |     {"file-satantfile", 2,  (void *)&filopt_.satantp,    ""     },
188 |     {"file-rcvantfile", 2,  (void *)&filopt_.rcvantp,    ""     },
189 |     {"file-staposfile", 2,  (void *)&filopt_.stapos,     ""     },
190 |     {"file-geoidfile",  2,  (void *)&filopt_.geoid,      ""     },
191 |     {"file-ionofile",   2,  (void *)&filopt_.iono,       ""     },
192 |     {"file-dcbfile",    2,  (void *)&filopt_.dcb,        ""     },
193 |     {"file-eopfile",    2,  (void *)&filopt_.eop,        ""     },
194 |     {"file-blqfile",    2,  (void *)&filopt_.blq,        ""     },
195 |     {"file-tempdir",    2,  (void *)&filopt_.tempdir,    ""     },
196 |     {"file-geexefile",  2,  (void *)&filopt_.geexe,      ""     },
197 |     {"file-solstatfile",2,  (void *)&filopt_.solstat,    ""     },
198 |     {"file-tracefile",  2,  (void *)&filopt_.trace,      ""     },
199 |     
200 |     {"",0,NULL,""} /* terminator */
201 | };
202 | /* discard space characters at tail ------------------------------------------*/
203 | static void chop(char *str)
204 | {
205 |     char *p;
206 |     if ((p=strchr(str,'#'))) *p='\0'; /* comment */
207 |     for (p=str+strlen(str)-1;p>=str&&!isgraph((int)*p);p--) *p='\0';
208 | }
209 | /* enum to string ------------------------------------------------------------*/
210 | static int enum2str(char *s, const char *comment, int val)
211 | {
212 |     char str[32],*p,*q;
213 |     int n;
214 |     
215 |     n=sprintf(str,"%d:",val);
216 |     if (!(p=strstr(comment,str))) {
217 |         return sprintf(s,"%d",val);
218 |     }
219 |     if (!(q=strchr(p+n,','))&&!(q=strchr(p+n,')'))) {
220 |         strcpy(s,p+n);
221 |         return (int)strlen(p+n);
222 |     }
223 |     strncpy(s,p+n,q-p-n); s[q-p-n]='\0';
224 |     return (int)(q-p-n);
225 | }
226 | /* string to enum ------------------------------------------------------------*/
227 | static int str2enum(const char *str, const char *comment, int *val)
228 | {
229 |     const char *p;
230 |     char s[32];
231 |     
232 |     for (p=comment;;p++) {
233 |        if (!(p=strstr(p,str))) break;
234 |        if (*(p-1)!=':') continue;
235 |        for (p-=2;'0'<=*p&&*p<='9';p--) ;
236 |        return sscanf(p+1,"%d",val)==1;
237 |     }
238 |     sprintf(s,"%.30s:",str);
239 |     if ((p=strstr(comment,s))) { /* number */
240 |         return sscanf(p,"%d",val)==1;
241 |     }
242 |     return 0;
243 | }
244 | /* search option ---------------------------------------------------------------
245 | * search option record
246 | * args   : char   *name     I  option name
247 | *          opt_t  *opts     I  options table
248 | *                              (terminated with table[i].name="")
249 | * return : option record (NULL: not found)
250 | *-----------------------------------------------------------------------------*/
251 | extern opt_t *searchopt(const char *name, const opt_t *opts)
252 | {
253 |     int i;
254 |     
255 |     trace(3,"searchopt: name=%s\n",name);
256 |     
257 |     for (i=0;*opts[i].name;i++) {
258 |         if (strstr(opts[i].name,name)) return (opt_t *)(opts+i);
259 |     }
260 |     return NULL;
261 | }
262 | /* string to option value ------------------------------------------------------
263 | * convert string to option value
264 | * args   : opt_t  *opt      O  option
265 | *          char   *str      I  option value string
266 | * return : status (1:ok,0:error)
267 | *-----------------------------------------------------------------------------*/
268 | extern int str2opt(opt_t *opt, const char *str)
269 | {
270 |     switch (opt->format) {
271 |         case 0: *(int    *)opt->var=atoi(str); break;
272 |         case 1: *(double *)opt->var=atof(str); break;
273 |         case 2: strcpy((char *)opt->var,str);  break;
274 |         case 3: return str2enum(str,opt->comment,(int *)opt->var);
275 |         default: return 0;
276 |     }
277 |     return 1;
278 | }
279 | /* option value to string ------------------------------------------------------
280 | * convert option value to string
281 | * args   : opt_t  *opt      I  option
282 | *          char   *str      O  option value string
283 | * return : length of output string
284 | *-----------------------------------------------------------------------------*/
285 | extern int opt2str(const opt_t *opt, char *str)
286 | {
287 |     char *p=str;
288 |     
289 |     trace(3,"opt2str : name=%s\n",opt->name);
290 |     
291 |     switch (opt->format) {
292 |         case 0: p+=sprintf(p,"%d"   ,*(int   *)opt->var); break;
293 |         case 1: p+=sprintf(p,"%.15g",*(double*)opt->var); break;
294 |         case 2: p+=sprintf(p,"%s"   , (char  *)opt->var); break;
295 |         case 3: p+=enum2str(p,opt->comment,*(int *)opt->var); break;
296 |     }
297 |     return (int)(p-str);
298 | }
299 | /* option to string -------------------------------------------------------------
300 | * convert option to string (keyword=value # comment)
301 | * args   : opt_t  *opt      I  option
302 | *          char   *buff     O  option string
303 | * return : length of output string
304 | *-----------------------------------------------------------------------------*/
305 | extern int opt2buf(const opt_t *opt, char *buff)
306 | {
307 |     char *p=buff;
308 |     int n;
309 |     
310 |     trace(3,"opt2buf : name=%s\n",opt->name);
311 |     
312 |     p+=sprintf(p,"%-18s =",opt->name);
313 |     p+=opt2str(opt,p);
314 |     if (*opt->comment) {
315 |         if ((n=(int)(buff+30-p))>0) p+=sprintf(p,"%*s",n,"");
316 |         p+=sprintf(p," # (%s)",opt->comment);
317 |     }
318 |     return (int)(p-buff);
319 | }
320 | /* load options ----------------------------------------------------------------
321 | * load options from file
322 | * args   : char   *file     I  options file
323 | *          opt_t  *opts     IO options table
324 | *                              (terminated with table[i].name="")
325 | * return : status (1:ok,0:error)
326 | *-----------------------------------------------------------------------------*/
327 | extern int loadopts(const char *file, opt_t *opts)
328 | {
329 |     FILE *fp;
330 |     opt_t *opt;
331 |     char buff[2048],*p;
332 |     int n=0;
333 |     
334 |     trace(3,"loadopts: file=%s\n",file);
335 |     
336 |     if (!(fp=fopen(file,"r"))) {
337 |         trace(1,"loadopts: options file open error (%s)\n",file);
338 |         return 0;
339 |     }
340 |     while (fgets(buff,sizeof(buff),fp)) {
341 |         n++;
342 |         chop(buff);
343 |         
344 |         if (buff[0]=='\0') continue;
345 |         
346 |         if (!(p=strstr(buff,"="))) {
347 |             fprintf(stderr,"invalid option %s (%s:%d)\n",buff,file,n);
348 |             continue;
349 |         }
350 |         *p++='\0';
351 |         chop(buff);
352 |         if (!(opt=searchopt(buff,opts))) continue;
353 |         
354 |         if (!str2opt(opt,p)) {
355 |             fprintf(stderr,"invalid option value %s (%s:%d)\n",buff,file,n);
356 |             continue;
357 |         }
358 |     }
359 |     fclose(fp);
360 |     
361 |     return 1;
362 | }
363 | /* save options to file --------------------------------------------------------
364 | * save options to file
365 | * args   : char   *file     I  options file
366 | *          char   *mode     I  write mode ("w":overwrite,"a":append);
367 | *          char   *comment  I  header comment (NULL: no comment)
368 | *          opt_t  *opts     I  options table
369 | *                              (terminated with table[i].name="")
370 | * return : status (1:ok,0:error)
371 | *-----------------------------------------------------------------------------*/
372 | extern int saveopts(const char *file, const char *mode, const char *comment,
373 |                     const opt_t *opts)
374 | {
375 |     FILE *fp;
376 |     char buff[2048];
377 |     int i;
378 |     
379 |     trace(3,"saveopts: file=%s mode=%s\n",file,mode);
380 |     
381 |     if (!(fp=fopen(file,mode))) {
382 |         trace(1,"saveopts: options file open error (%s)\n",file);
383 |         return 0;
384 |     }
385 |     if (comment) fprintf(fp,"# %s\n\n",comment);
386 |     
387 |     for (i=0;*opts[i].name;i++) {
388 |         opt2buf(opts+i,buff);
389 |         fprintf(fp,"%s\n",buff);
390 |     }
391 |     fclose(fp);
392 |     return 1;
393 | }
394 | /* system options buffer to options ------------------------------------------*/
395 | static void buff2sysopts(void)
396 | {
397 |     double pos[3],*rr;
398 |     char buff[1024],*p,*id;
399 |     int i,j,sat,*ps;
400 |     
401 |     prcopt_.elmin     =elmask_    *D2R;
402 |     prcopt_.elmaskar  =elmaskar_  *D2R;
403 |     prcopt_.elmaskhold=elmaskhold_*D2R;
404 |     
405 |     for (i=0;i<2;i++) {
406 |         ps=i==0?&prcopt_.rovpos:&prcopt_.refpos;
407 |         rr=i==0?prcopt_.ru:prcopt_.rb;
408 |         
409 |         if (antpostype_[i]==0) { /* lat/lon/hgt */
410 |             *ps=0;
411 |             pos[0]=antpos_[i][0]*D2R;
412 |             pos[1]=antpos_[i][1]*D2R;
413 |             pos[2]=antpos_[i][2];
414 |             pos2ecef(pos,rr);
415 |         }
416 |         else if (antpostype_[i]==1) { /* xyz-ecef */
417 |             *ps=0;
418 |             rr[0]=antpos_[i][0];
419 |             rr[1]=antpos_[i][1];
420 |             rr[2]=antpos_[i][2];
421 |         }
422 |         else *ps=antpostype_[i]-1;
423 |     }
424 |     /* excluded satellites */
425 |     for (i=0;i<MAXSAT;i++) prcopt_.exsats[i]=0;
426 |     if (exsats_[0]!='\0') {
427 |         strcpy(buff,exsats_);
428 |         for (p=strtok(buff," ");p;p=strtok(NULL," ")) {
429 |             if (*p=='+') id=p+1; else id=p;
430 |             if (!(sat=satid2no(id))) continue;
431 |             prcopt_.exsats[sat-1]=*p=='+'?2:1;
432 |         }
433 |     }
434 |     /* snrmask */
435 |     for (i=0;i<NFREQ;i++) {
436 |         for (j=0;j<9;j++) prcopt_.snrmask.mask[i][j]=0.0;
437 |         strcpy(buff,snrmask_[i]);
438 |         for (p=strtok(buff,","),j=0;p&&j<9;p=strtok(NULL,",")) {
439 |             prcopt_.snrmask.mask[i][j++]=atof(p);
440 |         }
441 |     }
442 |     /* number of frequency (4:L1+L5) TODO ????*/
443 |     /*if (prcopt_.nf==4) {
444 |         prcopt_.nf=3;
445 |         prcopt_.freqopt=1;
446 |     }*/
447 | }
448 | /* options to system options buffer ------------------------------------------*/
449 | static void sysopts2buff(void)
450 | {
451 |     double pos[3],*rr;
452 |     char id[32],*p;
453 |     int i,j,sat,*ps;
454 |     
455 |     elmask_    =prcopt_.elmin     *R2D;
456 |     elmaskar_  =prcopt_.elmaskar  *R2D;
457 |     elmaskhold_=prcopt_.elmaskhold*R2D;
458 |     
459 |     for (i=0;i<2;i++) {
460 |         ps=i==0?&prcopt_.rovpos:&prcopt_.refpos;
461 |         rr=i==0?prcopt_.ru:prcopt_.rb;
462 |         
463 |         if (*ps==0) {
464 |             antpostype_[i]=0;
465 |             ecef2pos(rr,pos);
466 |             antpos_[i][0]=pos[0]*R2D;
467 |             antpos_[i][1]=pos[1]*R2D;
468 |             antpos_[i][2]=pos[2];
469 |         }
470 |         else antpostype_[i]=*ps+1;
471 |     }
472 |     /* excluded satellites */
473 |     exsats_[0]='\0';
474 |     for (sat=1,p=exsats_;sat<=MAXSAT&&p-exsats_<(int)sizeof(exsats_)-32;sat++) {
475 |         if (prcopt_.exsats[sat-1]) {
476 |             satno2id(sat,id);
477 |             p+=sprintf(p,"%s%s%s",p==exsats_?"":" ",
478 |                        prcopt_.exsats[sat-1]==2?"+":"",id);
479 |         }
480 |     }
481 |     /* snrmask */
482 |     for (i=0;i<NFREQ;i++) {
483 |         snrmask_[i][0]='\0';
484 |         p=snrmask_[i];
485 |         for (j=0;j<9;j++) {
486 |             p+=sprintf(p,"%s%.0f",j>0?",":"",prcopt_.snrmask.mask[i][j]);
487 |         }
488 |     }
489 |     /* number of frequency (4:L1+L5) TODO ???? */
490 |     /*if (prcopt_.nf==3&&prcopt_.freqopt==1) {
491 |         prcopt_.nf=4;
492 |         prcopt_.freqopt=0;
493 |     }*/
494 | }
495 | /* reset system options to default ---------------------------------------------
496 | * reset system options to default
497 | * args   : none
498 | * return : none
499 | *-----------------------------------------------------------------------------*/
500 | extern void resetsysopts(void)
501 | {
502 |     int i,j;
503 |     
504 |     trace(3,"resetsysopts:\n");
505 |     
506 |     prcopt_=prcopt_default;
507 |     solopt_=solopt_default;
508 |     filopt_.satantp[0]='\0';
509 |     filopt_.rcvantp[0]='\0';
510 |     filopt_.stapos [0]='\0';
511 |     filopt_.geoid  [0]='\0';
512 |     filopt_.dcb    [0]='\0';
513 |     filopt_.blq    [0]='\0';
514 |     filopt_.solstat[0]='\0';
515 |     filopt_.trace  [0]='\0';
516 |     for (i=0;i<2;i++) antpostype_[i]=0;
517 |     elmask_=15.0;
518 |     elmaskar_=0.0;
519 |     elmaskhold_=0.0;
520 |     for (i=0;i<2;i++) for (j=0;j<3;j++) {
521 |         antpos_[i][j]=0.0;
522 |     }
523 |     exsats_[0] ='\0';
524 | }
525 | /* get system options ----------------------------------------------------------
526 | * get system options
527 | * args   : prcopt_t *popt   IO processing options (NULL: no output)
528 | *          solopt_t *sopt   IO solution options   (NULL: no output)
529 | *          folopt_t *fopt   IO file options       (NULL: no output)
530 | * return : none
531 | * notes  : to load system options, use loadopts() before calling the function
532 | *-----------------------------------------------------------------------------*/
533 | extern void getsysopts(prcopt_t *popt, solopt_t *sopt, filopt_t *fopt)
534 | {
535 |     trace(3,"getsysopts:\n");
536 |     
537 |     buff2sysopts();
538 |     if (popt) *popt=prcopt_;
539 |     if (sopt) *sopt=solopt_;
540 |     if (fopt) *fopt=filopt_;
541 | }
542 | /* set system options ----------------------------------------------------------
543 | * set system options
544 | * args   : prcopt_t *prcopt I  processing options (NULL: default)
545 | *          solopt_t *solopt I  solution options   (NULL: default)
546 | *          filopt_t *filopt I  file options       (NULL: default)
547 | * return : none
548 | * notes  : to save system options, use saveopts() after calling the function
549 | *-----------------------------------------------------------------------------*/
550 | extern void setsysopts(const prcopt_t *prcopt, const solopt_t *solopt,
551 |                        const filopt_t *filopt)
552 | {
553 |     trace(3,"setsysopts:\n");
554 |     
555 |     resetsysopts();
556 |     if (prcopt) prcopt_=*prcopt;
557 |     if (solopt) solopt_=*solopt;
558 |     if (filopt) filopt_=*filopt;
559 |     sysopts2buff();
560 | }
561 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/ppp_ar.c:
--------------------------------------------------------------------------------
 1 | /*------------------------------------------------------------------------------
 2 | * ppp_ar.c : ppp ambiguity resolution
 3 | *
 4 | * reference :
 5 | *    [1] H.Okumura, C-gengo niyoru saishin algorithm jiten (in Japanese),
 6 | *        Software Technology, 1991
 7 | *
 8 | *          Copyright (C) 2012-2015 by T.TAKASU, All rights reserved.
 9 | *
10 | * version : $Revision:$ $Date:$
11 | * history : 2013/03/11  1.0  new
12 | *           2016/05/10  1.1  delete codes
13 | *-----------------------------------------------------------------------------*/
14 | #include "rtklib.h"
15 | 
16 | /* ambiguity resolution in ppp -----------------------------------------------*/
17 | extern int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc,
18 |                   const nav_t *nav, const double *azel, double *x, double *P)
19 | {
20 |     return 0;
21 | }
22 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/rcv/crescent.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * crescent.c : hemisphere crescent/eclipse receiver dependent functions
  3 | *
  4 | *          Copyright (C) 2007-2020 by T.TAKASU, All rights reserved.
  5 | *
  6 | * reference :
  7 | *     [1] Hemisphere GPS, Grescent Integrator's Manual, December, 2005
  8 | *     [2] Hemisphere GPS, GPS Technical Reference, Part No. 875-0175-000,
  9 | *         Rev.D1, 2008
 10 | *     [3] Hemisphere GPS, Hemisphere GPS Technical Reference Manual, v4.0,
 11 | *         June 30, 2020
 12 | *
 13 | * version : $Revision: 1.2 $ $Date: 2008/07/14 00:05:05 $
 14 | * history : 2008/05/21 1.0  new
 15 | *           2009/04/01 1.1  support sbas, set 0 to L2 observables
 16 | *                           fix bug on getting doppler observables
 17 | *           2009/10/19 1.2  support eclipse (message bin 76)
 18 | *           2009/10/24 1.3  ignore vaild phase flag
 19 | *           2011/05/27 1.4  add -EPHALL option
 20 | *                           fix problem with ARM compiler
 21 | *           2011/07/01 1.5  suppress warning
 22 | *           2013/02/23 1.6  fix memory access violation problem on arm
 23 | *           2014/05/13 1.7  support bin65 and bin66
 24 | *                           add receiver option -TTCORR
 25 | *           2014/06/21 1.8  move decode_glostr() to rcvraw.c
 26 | *           2017/04/11 1.9  (char *) -> (signed char *)
 27 | *           2020/11/30 1.10 use integer type in stdint.h
 28 | *                           use sat2freq() instead of lam_carr()
 29 | *                           udpate reference [3]
 30 | *-----------------------------------------------------------------------------*/
 31 | #include "rtklib.h"
 32 | 
 33 | #define CRESSYNC    "$BIN"      /* hemis bin sync code */
 34 | 
 35 | #define ID_CRESPOS   1          /* hemis msg id: bin 1 position/velocity */
 36 | #define ID_CRESGLOEPH 65        /* hemis msg id: bin 65 glonass ephemeris */
 37 | #define ID_CRESGLORAW 66        /* hemis msg id: bin 66 glonass L1/L2 phase and code */
 38 | #define ID_CRESRAW2 76          /* hemis msg id: bin 76 dual-freq raw */
 39 | #define ID_CRESWAAS 80          /* hemis msg id: bin 80 waas messages */
 40 | #define ID_CRESIONUTC 94        /* hemis msg id: bin 94 ion/utc parameters */
 41 | #define ID_CRESEPH  95          /* hemis msg id: bin 95 raw ephemeris */
 42 | #define ID_CRESRAW  96          /* hemis msg id: bin 96 raw phase and code */
 43 | 
 44 | #define SNR2CN0_L1  30.0        /* hemis snr to c/n0 offset (db) L1 */
 45 | #define SNR2CN0_L2  30.0        /* hemis snr to c/n0 offset (db) L2 */
 46 | 
 47 | /* get fields (little-endian) ------------------------------------------------*/
 48 | #define U1(p) (*((uint8_t *)(p)))
 49 | #define I1(p) (*((int8_t  *)(p)))
 50 | static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;}
 51 | static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;}
 52 | static int16_t  I2(uint8_t *p) {int16_t  i; memcpy(&i,p,2); return i;}
 53 | static int32_t  I4(uint8_t *p) {int32_t  i; memcpy(&i,p,4); return i;}
 54 | static float    R4(uint8_t *p) {float    r; memcpy(&r,p,4); return r;}
 55 | static double   R8(uint8_t *p) {double   r; memcpy(&r,p,8); return r;}
 56 | 
 57 | /* checksum ------------------------------------------------------------------*/
 58 | static int chksum(const uint8_t *buff, int len)
 59 | {
 60 |     uint16_t sum=0;
 61 |     int i;
 62 |     
 63 |     for (i=8;i<len-4;i++) sum+=buff[i];
 64 |     trace(4,"checksum=%02X%02X %02X%02X:%02X%02X\n",
 65 |           sum>>8,sum&0xFF,buff[len-3],buff[len-4],buff[len-2],buff[len-1]);
 66 |     return (sum>>8)==buff[len-3]&&(sum&0xFF)==buff[len-4]&&
 67 |            buff[len-2]==0x0D&&buff[len-1]==0x0A;
 68 | }
 69 | /* decode bin 1 postion/velocity ---------------------------------------------*/
 70 | static int decode_crespos(raw_t *raw)
 71 | {
 72 |     int ns,week,mode;
 73 |     double tow,pos[3],vel[3],std;
 74 |     char tstr[64];
 75 |     uint8_t *p=raw->buff+8;
 76 |     
 77 |     trace(4,"decode_crespos: len=%d\n",raw->len);
 78 |     
 79 |     if (raw->len!=64) {
 80 |         trace(2,"crescent bin 1 message length error: len=%d\n",raw->len);
 81 |         return -1;
 82 |     }
 83 |     ns  =U1(p+1);
 84 |     week=U2(p+2);
 85 |     tow =R8(p+4);
 86 |     pos[0]=R8(p+12);
 87 |     pos[1]=R8(p+20);
 88 |     pos[2]=R4(p+28);
 89 |     vel[0]=R4(p+32);
 90 |     vel[1]=R4(p+36);
 91 |     vel[2]=R4(p+40);
 92 |     std =R4(p+44);
 93 |     mode=U2(p+48);
 94 |     time2str(gpst2time(week,tow),tstr,3);
 95 |     trace(3,"$BIN1 %s %13.9f %14.9f %10.4f %4d %3d %.3f\n",tstr,pos[0],pos[1],
 96 |           pos[2],mode==6?1:(mode>4?2:(mode>1?5:0)),ns,std);
 97 |     return 0;
 98 | }
 99 | /* decode bin 96 raw phase and code ------------------------------------------*/
100 | static int decode_cresraw(raw_t *raw)
101 | {
102 |     gtime_t time;
103 |     double tow,tows,toff=0.0,cp,pr,dop,snr,freq=FREQL1;
104 |     int i,j,n,prn,sat,week,word2,lli=0;
105 |     uint32_t word1,sn,sc;
106 |     uint8_t *p=raw->buff+8;
107 |     
108 |     trace(4,"decode_cresraw: len=%d\n",raw->len);
109 |     
110 |     if (raw->len!=312) {
111 |         trace(2,"crescent bin 96 message length error: len=%d\n",raw->len);
112 |         return -1;
113 |     }
114 |     week=U2(p+2);
115 |     tow =R8(p+4);
116 |     tows=floor(tow*1000.0+0.5)/1000.0; /* round by 1ms */
117 |     time=gpst2time(week,tows);
118 |     
119 |     /* time tag offset correction */
120 |     if (strstr(raw->opt,"-TTCORR")) {
121 |         toff=CLIGHT*(tows-tow);
122 |     }
123 |     for (i=n=0,p+=12;i<12&&n<MAXOBS;i++,p+=24) {
124 |         word1=U4(p  );
125 |         word2=I4(p+4);
126 |         if ((prn=word1&0xFF)==0) continue; /* if 0, no data */
127 |         if (!(sat=satno(prn<=MAXPRNGPS?SYS_GPS:SYS_SBS,prn))) {
128 |             trace(2,"creasent bin 96 satellite number error: prn=%d\n",prn);
129 |             continue;
130 |         }
131 |         pr=R8(p+ 8)-toff;
132 |         cp=R8(p+16)-toff;
133 |         if (!(word2&1)) cp=0.0; /* invalid phase */
134 |         sn =(word1>>8)&0xFF;
135 |         snr=sn==0?0.0:10.0*log10(0.8192*sn)+SNR2CN0_L1;
136 |         sc =(uint32_t)(word1>>24);
137 |         if (raw->time.time!=0) {
138 |             lli=(int)((uint8_t)sc-(uint8_t)raw->lockt[sat-1][0])>0;
139 |         }
140 |         raw->lockt[sat-1][0]=(uint8_t)sc;
141 |         dop=word2/16/4096.0;
142 |         
143 |         raw->obs.data[n].time=time;
144 |         raw->obs.data[n].sat =sat;
145 |         raw->obs.data[n].P[0]=pr;
146 |         raw->obs.data[n].L[0]=cp*freq/CLIGHT;
147 |         raw->obs.data[n].D[0]=-(float)(dop*freq/CLIGHT);
148 |         raw->obs.data[n].SNR[0]=(uint16_t)(snr/SNR_UNIT+0.5);
149 |         raw->obs.data[n].LLI[0]=(uint8_t)lli;
150 |         raw->obs.data[n].code[0]=CODE_L1C;
151 |         
152 |         for (j=1;j<NFREQ;j++) {
153 |             raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
154 |             raw->obs.data[n].D[j]=0.0;
155 |             raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
156 |             raw->obs.data[n].code[j]=CODE_NONE;
157 |         }
158 |         n++;
159 |     }
160 |     raw->time=time;
161 |     raw->obs.n=n;
162 |     return 1;
163 | }
164 | /* decode bin 76 dual-freq raw phase and code --------------------------------*/
165 | static int decode_cresraw2(raw_t *raw)
166 | {
167 |     gtime_t time;
168 |     double tow,tows,toff=0.0,cp[2]={0},pr1,pr[2]={0},dop[2]={0},snr[2]={0};
169 |     double freq[2]={FREQL1,FREQL2};
170 |     int i,j,n=0,prn,sat,week,lli[2]={0};
171 |     uint32_t word1,word2,word3,sc,sn;
172 |     uint8_t *p=raw->buff+8;
173 |     
174 |     trace(4,"decode_cresraw2: len=%d\n",raw->len);
175 |     
176 |     if (raw->len!=460) {
177 |         trace(2,"crescent bin 76 message length error: len=%d\n",raw->len);
178 |         return -1;
179 |     }
180 |     tow =R8(p);
181 |     week=U2(p+8);
182 |     tows=floor(tow*1000.0+0.5)/1000.0; /* round by 1ms */
183 |     time=gpst2time(week,tows);
184 |     
185 |     /* time tag offset correction */
186 |     if (strstr(raw->opt,"-TTCORR")) {
187 |         toff=CLIGHT*(tows-tow);
188 |     }
189 |     if (fabs(timediff(time,raw->time))<1e-9) {
190 |         n=raw->obs.n;
191 |     }
192 |     for (i=0,p+=16;i<15&&n<MAXOBS;i++) {
193 |         word1=U4(p+324+4*i); /* L1CACodeMSBsPRN */
194 |         if ((prn=word1&0xFF)==0) continue; /* if 0, no data */
195 |         if (!(sat=satno(prn<=MAXPRNGPS?SYS_GPS:SYS_SBS,prn))) {
196 |             trace(2,"creasent bin 76 satellite number error: prn=%d\n",prn);
197 |             continue;
198 |         }
199 |         pr1=(word1>>13)*256.0; /* upper 19bit of L1CA pseudorange */
200 |         
201 |         word1=U4(p+144+12*i); /* L1CASatObs */
202 |         word2=U4(p+148+12*i);
203 |         word3=U4(p+152+12*i);
204 |         sn=word1&0xFFF;
205 |         snr[0]=sn==0?0.0:10.0*log10(0.1024*sn)+SNR2CN0_L1;
206 |         sc=(uint32_t)(word1>>24);
207 |         if (raw->time.time!=0) {
208 |             lli[0]=(int)((uint8_t)sc-(uint8_t)raw->lockt[sat-1][0])>0;
209 |         }
210 |         else {
211 |             lli[0]=0;
212 |         }
213 |         lli[0]|=((word1>>12)&7)?2:0;
214 |         raw->lockt[sat-1][0]=(uint8_t)sc;
215 |         dop[0]=((word2>>1)&0x7FFFFF)/512.0;
216 |         if ((word2>>24)&1) dop[0]=-dop[0];
217 |         pr[0]=pr1+(word3&0xFFFF)/256.0;
218 |         cp[0]=floor(pr[0]*freq[0]/CLIGHT/8192.0)*8192.0;
219 |         cp[0]+=((word2&0xFE000000)+((word3&0xFFFF0000)>>7))/524288.0;
220 |         if      (cp[0]-pr[0]*freq[0]/CLIGHT<-4096.0) cp[0]+=8192.0;
221 |         else if (cp[0]-pr[0]*freq[0]/CLIGHT> 4096.0) cp[0]-=8192.0;
222 |         
223 |         if (i<12) {
224 |             word1=U4(p  +12*i); /* L2PSatObs */
225 |             word2=U4(p+4+12*i);
226 |             word3=U4(p+8+12*i);
227 |             sn=word1&0xFFF;
228 |             snr[1]=sn==0?0.0:10.0*log10(0.1164*sn)+SNR2CN0_L2;
229 |             sc=(uint32_t)(word1>>24);
230 |             if (raw->time.time==0) {
231 |                 lli[1]=(int)((uint8_t)sc-(uint8_t)raw->lockt[sat-1][1])>0;
232 |             }
233 |             else {
234 |                 lli[1]=0;
235 |             }
236 |             lli[1]|=((word1>>12)&7)?2:0;
237 |             raw->lockt[sat-1][1]=(uint8_t)sc;
238 |             dop[1]=((word2>>1)&0x7FFFFF)/512.0;
239 |             if ((word2>>24)&1) dop[1]=-dop[1];
240 |             pr[1]=(word3&0xFFFF)/256.0;
241 |             if (pr[1]!=0.0) {
242 |                 pr[1]+=pr1;
243 |                 if      (pr[1]-pr[0]<-128.0) pr[1]+=256.0;
244 |                 else if (pr[1]-pr[0]> 128.0) pr[1]-=256.0;
245 |                 cp[1]=floor(pr[1]*freq[1]/CLIGHT/8192.0)*8192.0;
246 |                 cp[1]+=((word2&0xFE000000)+((word3&0xFFFF0000)>>7))/524288.0;
247 |                 if      (cp[1]-pr[1]*freq[1]/CLIGHT<-4096.0) cp[1]+=8192.0;
248 |                 else if (cp[1]-pr[1]*freq[1]/CLIGHT> 4096.0) cp[1]-=8192.0;
249 |             }
250 |             else cp[1]=0.0;
251 |         }
252 |         raw->obs.data[n].time=time;
253 |         raw->obs.data[n].sat =sat;
254 |         for (j=0;j<NFREQ;j++) {
255 |             if (j==0||(j==1&&i<12)) {
256 |                 raw->obs.data[n].P[j]=pr[j]==0.0?0.0:pr[j]-toff;
257 |                 raw->obs.data[n].L[j]=cp[j]==0.0?0.0:cp[j]-toff*freq[j]/CLIGHT;
258 |                 raw->obs.data[n].D[j]=-(float)dop[j];
259 |                 raw->obs.data[n].SNR[j]=(uint16_t)(snr[j]/SNR_UNIT+0.5);
260 |                 raw->obs.data[n].LLI[j]=(uint8_t)lli[j];
261 |                 raw->obs.data[n].code[j]=j==0?CODE_L1C:CODE_L2P;
262 |             }
263 |             else {
264 |                 raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
265 |                 raw->obs.data[n].D[j]=0.0;
266 |                 raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
267 |                 raw->obs.data[n].code[j]=CODE_NONE;
268 |             }
269 |         }
270 |         n++;
271 |     }
272 |     raw->time=time;
273 |     raw->obs.n=n;
274 |     if (strstr(raw->opt,"-ENAGLO")) return 0; /* glonass follows */
275 |     return 1;
276 | }
277 | /* decode bin 95 ephemeris ---------------------------------------------------*/
278 | static int decode_creseph(raw_t *raw)
279 | {
280 |     eph_t eph={0};
281 |     uint32_t word;
282 |     int i,j,k,prn,sat;
283 |     uint8_t *p=raw->buff+8,buff[90];
284 |     
285 |     trace(4,"decode_creseph: len=%d\n",raw->len);
286 |     
287 |     if (raw->len!=140) {
288 |         trace(2,"crescent bin 95 message length error: len=%d\n",raw->len);
289 |         return -1;
290 |     }
291 |     prn=U2(p);
292 |     if (!(sat=satno(SYS_GPS,prn))) {
293 |         trace(2,"crescent bin 95 satellite number error: prn=%d\n",prn);
294 |         return -1;
295 |     }
296 |     for (i=0;i<3;i++) for (j=0;j<10;j++) {
297 |         word=U4(p+8+i*40+j*4)>>6;
298 |         for (k=0;k<3;k++) buff[i*30+j*3+k]=(uint8_t)((word>>(8*(2-k)))&0xFF);
299 |     }
300 |     if (!decode_frame(buff,&eph,NULL,NULL,NULL)) {
301 |         trace(2,"crescent bin 95 navigation frame error: prn=%d\n",prn);
302 |         return -1;
303 |     }
304 |     if (!strstr(raw->opt,"-EPHALL")) {
305 |         if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
306 |     }
307 |     eph.sat=sat;
308 |     raw->nav.eph[sat-1]=eph;
309 |     raw->ephsat=sat;
310 |     raw->ephset=0;
311 |     return 2;
312 | }
313 | /* decode bin 94 ion/utc parameters ------------------------------------------*/
314 | static int decode_cresionutc(raw_t *raw)
315 | {
316 |     int i;
317 |     uint8_t *p=raw->buff+8;
318 |     
319 |     trace(4,"decode_cresionutc: len=%d\n",raw->len);
320 |     
321 |     if (raw->len!=108) {
322 |         trace(2,"crescent bin 94 message length error: len=%d\n",raw->len);
323 |         return -1;
324 |     }
325 |     for (i=0;i<8;i++) raw->nav.ion_gps[i]=R8(p+i*8);
326 |     raw->nav.utc_gps[0]=R8(p+64);
327 |     raw->nav.utc_gps[1]=R8(p+72);
328 |     raw->nav.utc_gps[2]=(double)U4(p+80);
329 |     raw->nav.utc_gps[3]=(double)U2(p+84);
330 |     raw->nav.utc_gps[4]=I2(p+90);
331 |     return 9;
332 | }
333 | /* decode bin 80 waas messages -----------------------------------------------*/
334 | static int decode_creswaas(raw_t *raw)
335 | {
336 |     double tow;
337 |     uint32_t word;
338 |     int i,j,k,prn;
339 |     uint8_t *p=raw->buff+8;
340 |     
341 |     trace(4,"decode_creswaas: len=%d\n",raw->len);
342 |     
343 |     if (raw->len!=52) {
344 |         trace(2,"creasent bin 80 message length error: len=%d\n",raw->len);
345 |         return -1;
346 |     }
347 |     prn=U2(p);
348 |     if (prn<MINPRNSBS||MAXPRNSBS<prn) {
349 |         trace(2,"creasent bin 80 satellite number error: prn=%d\n",prn);
350 |         return -1;
351 |     }
352 |     raw->sbsmsg.prn=prn;
353 |     raw->sbsmsg.tow=U4(p+4);
354 |     tow=time2gpst(raw->time,&raw->sbsmsg.week);
355 |     if      (raw->sbsmsg.tow<tow-302400.0) raw->sbsmsg.week++;
356 |     else if (raw->sbsmsg.tow>tow+302400.0) raw->sbsmsg.week--;
357 |     
358 |     for (i=k=0;i<8&&k<29;i++) {
359 |         word=U4(p+8+i*4);
360 |         for (j=0;j<4&&k<29;j++) raw->sbsmsg.msg[k++]=(uint8_t)(word>>(3-j)*8);
361 |     }
362 |     raw->sbsmsg.msg[28]&=0xC0;
363 |     return 3;
364 | }
365 | /* decode bin 66 glonass L1/L2 code and carrier phase ------------------------*/
366 | static int decode_cresgloraw(raw_t *raw)
367 | {
368 |     gtime_t time;
369 |     double tow,tows,toff=0.0,cp[2]={0},pr1,pr[2]={0},dop[2]={0},snr[2]={0};
370 |     double freq[2];
371 |     int i,j,n=0,prn,sat,week,lli[2]={0};
372 |     uint32_t word1,word2,word3,sc,sn;
373 |     uint8_t *p=raw->buff+8;
374 |     
375 |     trace(4,"decode_cregloraw: len=%d\n",raw->len);
376 |     
377 |     if (!strstr(raw->opt,"-ENAGLO")) return 0;
378 |     
379 |     if (raw->len!=364) {
380 |         trace(2,"crescent bin 66 message length error: len=%d\n",raw->len);
381 |         return -1;
382 |     }
383 |     tow =R8(p);
384 |     week=U2(p+8);
385 |     tows=floor(tow*1000.0+0.5)/1000.0; /* round by 1ms */
386 |     time=gpst2time(week,tows);
387 |     
388 |     /* time tag offset correction */
389 |     if (strstr(raw->opt,"-TTCORR")) {
390 |         toff=CLIGHT*(tows-tow);
391 |     }
392 |     if (fabs(timediff(time,raw->time))<1e-9) {
393 |         n=raw->obs.n;
394 |     }
395 |     for (i=0,p+=16;i<12&&n<MAXOBS;i++) {
396 |         word1=U4(p+288+4*i); /* L1CACodeMSBsSlot */
397 |         if ((prn=word1&0xFF)==0) continue; /* if 0, no data */
398 |         if (!(sat=satno(SYS_GLO,prn))) {
399 |             trace(2,"creasent bin 66 satellite number error: prn=%d\n",prn);
400 |             continue;
401 |         }
402 |         pr1=(word1>>13)*256.0; /* upper 19bit of L1CA pseudorange */
403 |         
404 |         freq[0]=sat2freq(sat,CODE_L1C,&raw->nav);
405 |         freq[1]=sat2freq(sat,CODE_L2C,&raw->nav);
406 |         
407 |         /* L1Obs */
408 |         word1=U4(p  +12*i);
409 |         word2=U4(p+4+12*i);
410 |         word3=U4(p+8+12*i);
411 |         sn=word1&0xFFF;
412 |         snr[0]=sn==0?0.0:10.0*log10(0.1024*sn)+SNR2CN0_L1;
413 |         sc=(uint32_t)(word1>>24);
414 |         if (raw->time.time!=0) {
415 |             lli[0]=(int)((uint8_t)sc-(uint8_t)raw->lockt[sat-1][0])>0;
416 |         }
417 |         else {
418 |             lli[0]=0;
419 |         }
420 |         lli[0]|=((word1>>12)&7)?2:0;
421 |         raw->lockt[sat-1][0]=(uint8_t)sc;
422 |         dop[0]=((word2>>1)&0x7FFFFF)/512.0;
423 |         if ((word2>>24)&1) dop[0]=-dop[0];
424 |         pr[0]=pr1+(word3&0xFFFF)/256.0;
425 |         cp[0]=floor(pr[0]*freq[0]/CLIGHT/8192.0)*8192.0;
426 |         cp[0]+=((word2&0xFE000000)+((word3&0xFFFF0000)>>7))/524288.0;
427 |         if      (cp[0]-pr[0]*freq[0]/CLIGHT<-4096.0) cp[0]+=8192.0;
428 |         else if (cp[0]-pr[0]*freq[0]/CLIGHT> 4096.0) cp[0]-=8192.0;
429 |         
430 |         /* L2Obs */
431 |         word1=U4(p+144+12*i);
432 |         word2=U4(p+148+12*i);
433 |         word3=U4(p+152+12*i);
434 |         sn=word1&0xFFF;
435 |         snr[1]=sn==0?0.0:10.0*log10(0.1164*sn)+SNR2CN0_L2;
436 |         sc=(uint32_t)(word1>>24);
437 |         if (raw->time.time==0) {
438 |             lli[1]=(int)((uint8_t)sc-(uint8_t)raw->lockt[sat-1][1])>0;
439 |         }
440 |         else {
441 |             lli[1]=0;
442 |         }
443 |         lli[1]|=((word1>>12)&7)?2:0;
444 |         raw->lockt[sat-1][1]=(uint8_t)sc;
445 |         dop[1]=((word2>>1)&0x7FFFFF)/512.0;
446 |         if ((word2>>24)&1) dop[1]=-dop[1];
447 |         pr[1]=(word3&0xFFFF)/256.0;
448 |         if (pr[1]!=0.0) {
449 |             pr[1]+=pr1;
450 |             if      (pr[1]-pr[0]<-128.0) pr[1]+=256.0;
451 |             else if (pr[1]-pr[0]> 128.0) pr[1]-=256.0;
452 |             cp[1]=floor(pr[1]*freq[1]/CLIGHT/8192.0)*8192.0;
453 |             cp[1]+=((word2&0xFE000000)+((word3&0xFFFF0000)>>7))/524288.0;
454 |             if      (cp[1]-pr[1]*freq[1]/CLIGHT<-4096.0) cp[1]+=8192.0;
455 |             else if (cp[1]-pr[1]*freq[1]/CLIGHT> 4096.0) cp[1]-=8192.0;
456 |         }
457 |         raw->obs.data[n].time=time;
458 |         raw->obs.data[n].sat =sat;
459 |         for (j=0;j<NFREQ;j++) {
460 |             if (j==0||(j==1&&i<12)) {
461 |                 raw->obs.data[n].P[j]=pr[j]==0.0?0.0:pr[j]-toff;
462 |                 raw->obs.data[n].L[j]=cp[j]==0.0?0.0:cp[j]-toff*freq[j]/CLIGHT;
463 |                 raw->obs.data[n].D[j]=-(float)dop[j];
464 |                 raw->obs.data[n].SNR[j]=(uint16_t)(snr[j]/SNR_UNIT+0.5);
465 |                 raw->obs.data[n].LLI[j]=(uint8_t)lli[j];
466 |                 raw->obs.data[n].code[j]=j==0?CODE_L1C:CODE_L2P;
467 |             }
468 |             else {
469 |                 raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
470 |                 raw->obs.data[n].D[j]=0.0;
471 |                 raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
472 |                 raw->obs.data[n].code[j]=CODE_NONE;
473 |             }
474 |         }
475 |         n++;
476 |     }
477 |     raw->time=time;
478 |     raw->obs.n=n;
479 |     return 1;
480 | }
481 | /* decode bin 65 glonass ephemeris -------------------------------------------*/
482 | static int decode_cresgloeph(raw_t *raw)
483 | {
484 |     geph_t geph={0};
485 |     uint8_t *p=raw->buff+8,str[12];
486 |     int i,j,k,sat,prn,frq,time,no;
487 |     
488 |     trace(4,"decode_cregloeph: len=%d\n",raw->len);
489 |     
490 |     if (!strstr(raw->opt,"-ENAGLO")) return 0;
491 |     
492 |     prn =U1(p);   p+=1;
493 |     frq =U1(p)-8; p+=1+2;
494 |     time=U4(p);   p+=4;
495 |     
496 |     if (!(sat=satno(SYS_GLO,prn))) {
497 |         trace(2,"creasent bin 65 satellite number error: prn=%d\n",prn);
498 |         return -1;
499 |     }
500 |     for (i=0;i<5;i++) {
501 |         for (j=0;j<3;j++) for (k=3;k>=0;k--) {
502 |             str[k+j*4]=U1(p++);
503 |         }
504 |         if ((no=getbitu(str,1,4))!=i+1) {
505 |             trace(2,"creasent bin 65 string no error: sat=%2d no=%d %d\n",sat,
506 |                   i+1,no);
507 |             return -1;
508 |         }
509 |         memcpy(raw->subfrm[sat-1]+10*i,str,10);
510 |     }
511 |     /* decode glonass ephemeris strings */
512 |     geph.tof=raw->time;
513 |     if (!decode_glostr(raw->subfrm[sat-1],&geph,NULL)||geph.sat!=sat) return -1;
514 |     geph.frq=frq;
515 |     
516 |     if (!strstr(raw->opt,"-EPHALL")) {
517 |         if (geph.iode==raw->nav.geph[prn-1].iode) return 0; /* unchanged */
518 |     }
519 |     raw->nav.geph[prn-1]=geph;
520 |     raw->ephsat=sat;
521 |     raw->ephset=0;
522 |     return 2;
523 | }
524 | /* decode crescent raw message -----------------------------------------------*/
525 | static int decode_cres(raw_t *raw)
526 | {
527 |     int type=U2(raw->buff+4);
528 |     
529 |     trace(3,"decode_cres: type=%2d len=%d\n",type,raw->len);
530 |     
531 |     if (!chksum(raw->buff,raw->len)) {
532 |         trace(2,"crescent checksum error: type=%2d len=%d\n",type,raw->len);
533 |         return -1;
534 |     }
535 |     if (raw->outtype) {
536 |         sprintf(raw->msgtype,"HEMIS %2d (%4d):",type,raw->len);
537 |     }
538 |     switch (type) {
539 |         case ID_CRESPOS   : return decode_crespos(raw);
540 |         case ID_CRESRAW   : return decode_cresraw(raw);
541 |         case ID_CRESRAW2  : return decode_cresraw2(raw);
542 |         case ID_CRESEPH   : return decode_creseph(raw);
543 |         case ID_CRESWAAS  : return decode_creswaas(raw);
544 |         case ID_CRESIONUTC: return decode_cresionutc(raw);
545 |         case ID_CRESGLORAW: return decode_cresgloraw(raw);
546 |         case ID_CRESGLOEPH: return decode_cresgloeph(raw);
547 |     }
548 |     return 0;
549 | }
550 | /* sync code -----------------------------------------------------------------*/
551 | static int sync_cres(uint8_t *buff, uint8_t data)
552 | {
553 |     buff[0]=buff[1]; buff[1]=buff[2]; buff[2]=buff[3]; buff[3]=data;
554 |     return buff[0]==CRESSYNC[0]&&buff[1]==CRESSYNC[1]&&
555 |            buff[2]==CRESSYNC[2]&&buff[3]==CRESSYNC[3];
556 | }
557 | /* input cresent raw message ---------------------------------------------------
558 | * input next crescent raw message from stream
559 | * args   : raw_t *raw       IO  receiver raw data control struct
560 | *          uint8_t data     I   stream data (1 byte)
561 | * return : status (-1: error message, 0: no message, 1: input observation data,
562 | *                  2: input ephemeris, 3: input sbas message,
563 | *                  9: input ion/utc parameter)
564 | *
565 | * notes  : to specify input options, set raw->opt to the following option
566 | *          strings separated by spaces.
567 | *
568 | *          -EPHALL      : input all ephemerides
569 | *          -TTCORR      : time-tag offset correction
570 | *          -ENAGLO      : enable glonass messages
571 | *
572 | *-----------------------------------------------------------------------------*/
573 | extern int input_cres(raw_t *raw, uint8_t data)
574 | {
575 |     trace(5,"input_cres: data=%02x\n",data);
576 |     
577 |     /* synchronize frame */
578 |     if (raw->nbyte==0) {
579 |         if (!sync_cres(raw->buff,data)) return 0;
580 |         raw->nbyte=4;
581 |         return 0;
582 |     }
583 |     raw->buff[raw->nbyte++]=data;
584 |     
585 |     if (raw->nbyte==8) {
586 |         if ((raw->len=U2(raw->buff+6)+12)>MAXRAWLEN) {
587 |             trace(2,"cresent length error: len=%d\n",raw->len);
588 |             raw->nbyte=0;
589 |             return -1;
590 |         }
591 |     }
592 |     if (raw->nbyte<8||raw->nbyte<raw->len) return 0;
593 |     raw->nbyte=0;
594 |     
595 |     /* decode crescent raw message */
596 |     return decode_cres(raw);
597 | }
598 | /* input crescent raw message from file ----------------------------------------
599 | * input next crescent raw message from file
600 | * args   : raw_t  *raw   IO     receiver raw data control struct
601 | *          FILE   *fp    I      file pointer
602 | * return : status(-2: end of file, -1...9: same as above)
603 | *-----------------------------------------------------------------------------*/
604 | extern int input_cresf(raw_t *raw, FILE *fp)
605 | {
606 |     int i,data;
607 |     
608 |     trace(4,"input_cresf:\n");
609 |     
610 |     /* synchronize frame */
611 |     if (raw->nbyte==0) {
612 |         for (i=0;;i++) {
613 |             if ((data=fgetc(fp))==EOF) return -2;
614 |             if (sync_cres(raw->buff,(uint8_t)data)) break;
615 |             if (i>=4096) return 0;
616 |         }
617 |     }
618 |     if (fread(raw->buff+4,1,4,fp)<4) return -2;
619 |     raw->nbyte=8;
620 |     
621 |     if ((raw->len=U2(raw->buff+6)+12)>MAXRAWLEN) {
622 |         trace(2,"crescent length error: len=%d\n",raw->len);
623 |         raw->nbyte=0;
624 |         return -1;
625 |     }
626 |     if (fread(raw->buff+8,1,raw->len-8,fp)<(size_t)(raw->len-8)) return -2;
627 |     raw->nbyte=0;
628 |     
629 |     /* decode crescent raw message */
630 |     return decode_cres(raw);
631 | }
632 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/rcv/nvs.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * nvs.c : NVS receiver dependent functions
  3 | *
  4 | *    Copyright (C) 2012-2016 by M.BAVARO and T.TAKASU, All rights reserved.
  5 | *    Copyright (C) 2014-2020 by T.TAKASU, All rights reserved.
  6 | *
  7 | *     [1] Description of BINR messages which is used by RC program for RINEX
  8 | *         files accumulation, NVS
  9 | *     [2] NAVIS Navis Standard Interface Protocol BINR, NVS
 10 | *
 11 | * version : $Revision:$ $Date:$
 12 | * history : 2012/01/30 1.0  first version by M.BAVARO
 13 | *           2012/11/08 1.1  modified by T.TAKASU
 14 | *           2013/02/23 1.2  fix memory access violation problem on arm
 15 | *           2013/04/24 1.3  fix bug on cycle-slip detection
 16 | *                           add range check of gps ephemeris week
 17 | *           2013/09/01 1.4  add check error of week, time jump, obs data range
 18 | *           2014/08/26 1.5  fix bug on iode in glonass ephemeris
 19 | *           2016/01/26 1.6  fix bug on unrecognized meas data (#130)
 20 | *           2017/04/11 1.7  (char *) -> (signed char *)
 21 | *           2020/07/10 1.8  suppress warnings
 22 | *           2020/11/30 1.9  use integer type in stdint.h
 23 | *-----------------------------------------------------------------------------*/
 24 | #include "rtklib.h"
 25 | 
 26 | #define NVSSYNC     0x10        /* nvs message sync code 1 */
 27 | #define NVSENDMSG   0x03        /* nvs message sync code 1 */
 28 | #define NVSCFG      0x06        /* nvs message cfg-??? */
 29 | 
 30 | #define ID_XF5RAW   0xf5        /* nvs msg id: raw measurement data */
 31 | #define ID_X4AIONO  0x4a        /* nvs msg id: gps ionospheric data */
 32 | #define ID_X4BTIME  0x4b        /* nvs msg id: GPS/GLONASS/UTC timescale data */
 33 | #define ID_XF7EPH   0xf7        /* nvs msg id: subframe buffer */
 34 | #define ID_XE5BIT   0xe5        /* nvs msg id: bit information */
 35 | 
 36 | #define ID_XD7ADVANCED 0xd7     /* */
 37 | #define ID_X02RATEPVT  0x02     /* */
 38 | #define ID_XF4RATERAW  0xf4     /* */
 39 | #define ID_XD7SMOOTH   0xd7     /* */
 40 | #define ID_XD5BIT      0xd5     /* */
 41 | 
 42 | /* get fields (little-endian) ------------------------------------------------*/
 43 | #define U1(p) (*((uint8_t *)(p)))
 44 | #define I1(p) (*((int8_t  *)(p)))
 45 | static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;}
 46 | static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;}
 47 | static int16_t  I2(uint8_t *p) {int16_t  i; memcpy(&i,p,2); return i;}
 48 | static int32_t  I4(uint8_t *p) {int32_t  i; memcpy(&i,p,4); return i;}
 49 | static float    R4(uint8_t *p) {float    r; memcpy(&r,p,4); return r;}
 50 | static double   R8(uint8_t *p) {double   r; memcpy(&r,p,8); return r;}
 51 | 
 52 | /* ura values (ref [3] 20.3.3.3.1.1) -----------------------------------------*/
 53 | static const double ura_eph[]={
 54 |     2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0,
 55 |     3072.0,6144.0,0.0
 56 | };
 57 | /* ura value (m) to ura index ------------------------------------------------*/
 58 | static int uraindex(double value)
 59 | {
 60 |     int i;
 61 |     for (i=0;i<15;i++) if (ura_eph[i]>=value) break;
 62 |     return i;
 63 | }
 64 | /* decode NVS xf5-raw: raw measurement data ----------------------------------*/
 65 | static int decode_xf5raw(raw_t *raw)
 66 | {
 67 |     gtime_t time;
 68 |     double tadj=0.0,toff=0.0,tn;
 69 |     int dTowInt;
 70 |     double dTowUTC, dTowGPS, dTowFrac, L1, P1, D1;
 71 |     double gpsutcTimescale;
 72 |     uint8_t rcvTimeScaleCorr, sys, carrNo;
 73 |     int i,j,prn,sat,n=0,nsat,week;
 74 |     uint8_t *p=raw->buff+2;
 75 |     char *q,tstr[32],flag;
 76 |     
 77 |     trace(4,"decode_xf5raw: len=%d\n",raw->len);
 78 |     
 79 |     /* time tag adjustment option (-TADJ) */
 80 |     if ((q=strstr(raw->opt,"-tadj"))) {
 81 |         sscanf(q,"-TADJ=%lf",&tadj);
 82 |     }
 83 |     dTowUTC =R8(p);
 84 |     week = U2(p+8);
 85 |     gpsutcTimescale = R8(p+10);
 86 |     /* glonassutcTimescale = R8(p+18); */
 87 |     rcvTimeScaleCorr = I1(p+26);
 88 |     
 89 |     /* check gps week range */
 90 |     if (week>=4096) {
 91 |         trace(2,"nvs xf5raw obs week error: week=%d\n",week);
 92 |         return -1;
 93 |     }
 94 |     week=adjgpsweek(week);
 95 |     
 96 |     if ((raw->len - 31)%30) {
 97 |         
 98 |         /* Message length is not correct: there could be an error in the stream */
 99 |         trace(2,"nvs xf5raw len=%d seems not be correct\n",raw->len);
100 |         return -1;
101 |     }
102 |     nsat = (raw->len - 31)/30;
103 |     
104 |     dTowGPS = dTowUTC + gpsutcTimescale;
105 |     
106 |     /* Tweak pseudoranges to allow Rinex to represent the NVS time of measure */
107 |     dTowInt  = 10.0*floor((dTowGPS/10.0)+0.5);
108 |     dTowFrac = dTowGPS - (double) dTowInt;
109 |     time=gpst2time(week, dTowInt*0.001);
110 |     
111 |     /* time tag adjustment */
112 |     if (tadj>0.0) {
113 |         tn=time2gpst(time,&week)/tadj;
114 |         toff=(tn-floor(tn+0.5))*tadj;
115 |         time=timeadd(time,-toff);
116 |     }
117 |     /* check time tag jump and output warning */
118 |     if (raw->time.time&&fabs(timediff(time,raw->time))>86400.0) {
119 |         time2str(time,tstr,3);
120 |         trace(2,"nvs xf5raw time tag jump warning: time=%s\n",tstr);
121 |     }
122 |     if (fabs(timediff(time,raw->time))<=1e-3) {
123 |         time2str(time,tstr,3);
124 |         trace(2,"nvs xf5raw time tag duplicated: time=%s\n",tstr);
125 |         return 0;
126 |     }
127 |     for (i=0,p+=27;(i<nsat) && (n<MAXOBS); i++,p+=30) {
128 |         raw->obs.data[n].time  = time;
129 |         sys = (U1(p)==1)?SYS_GLO:((U1(p)==2)?SYS_GPS:((U1(p)==4)?SYS_SBS:SYS_NONE));
130 |         prn = U1(p+1);
131 |         if (sys == SYS_SBS) prn += 120; /* Correct this */
132 |         if (!(sat=satno(sys,prn))) {
133 |             trace(2,"nvs xf5raw satellite number error: sys=%d prn=%d\n",sys,prn);
134 |             continue;
135 |         }
136 |         carrNo = I1(p+2);
137 |         L1 = R8(p+ 4);
138 |         P1 = R8(p+12);
139 |         D1 = R8(p+20);
140 |         
141 |         /* check range error */
142 |         if (L1<-1E10||L1>1E10||P1<-1E10||P1>1E10||D1<-1E5||D1>1E5) {
143 |             trace(2,"nvs xf5raw obs range error: sat=%2d L1=%12.5e P1=%12.5e D1=%12.5e\n",
144 |                   sat,L1,P1,D1);
145 |             continue;
146 |         }
147 |         raw->obs.data[n].SNR[0]=(uint16_t)(I1(p+3)/SNR_UNIT+0.5);
148 |         if (sys==SYS_GLO) {
149 |             raw->obs.data[n].L[0]  =  L1 - toff*(FREQ1_GLO+DFRQ1_GLO*carrNo);
150 |         } else {
151 |             raw->obs.data[n].L[0]  =  L1 - toff*FREQL1;
152 |         }
153 |         raw->obs.data[n].P[0]    = (P1-dTowFrac)*CLIGHT*0.001 - toff*CLIGHT; /* in ms, needs to be converted */
154 |         raw->obs.data[n].D[0]    =  (float)D1;
155 |         
156 |         /* set LLI if meas flag 4 (carrier phase present) off -> on */
157 |         flag=U1(p+28);
158 |         raw->obs.data[n].LLI[0]=(flag&0x08)&&!(raw->halfc[sat-1][0]&0x08)?1:0;
159 |         raw->halfc[sat-1][0]=flag;
160 |         
161 |         raw->obs.data[n].code[0] = CODE_L1C;
162 |         raw->obs.data[n].sat = sat;
163 |         
164 |         for (j=1;j<NFREQ+NEXOBS;j++) {
165 |             raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
166 |             raw->obs.data[n].D[j]=0.0;
167 |             raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
168 |             raw->obs.data[n].code[j]=CODE_NONE;
169 |         }
170 |         n++;
171 |     }
172 |     raw->time=time;
173 |     raw->obs.n=n;
174 |     return 1;
175 | }
176 | /* decode ephemeris ----------------------------------------------------------*/
177 | static int decode_gpsephem(int sat, raw_t *raw)
178 | {
179 |     eph_t eph={0};
180 |     uint8_t *puiTmp = (raw->buff)+2;
181 |     uint16_t week;
182 |     double toc;
183 |     
184 |     trace(4,"decode_ephem: sat=%2d\n",sat);
185 |     
186 |     eph.crs    = R4(&puiTmp[  2]);
187 |     eph.deln   = R4(&puiTmp[  6]) * 1e+3;
188 |     eph.M0     = R8(&puiTmp[ 10]);
189 |     eph.cuc    = R4(&puiTmp[ 18]);
190 |     eph.e      = R8(&puiTmp[ 22]);
191 |     eph.cus    = R4(&puiTmp[ 30]);
192 |     eph.A      = pow(R8(&puiTmp[ 34]), 2);
193 |     eph.toes   = R8(&puiTmp[ 42]) * 1e-3;
194 |     eph.cic    = R4(&puiTmp[ 50]);
195 |     eph.OMG0   = R8(&puiTmp[ 54]);
196 |     eph.cis    = R4(&puiTmp[ 62]);
197 |     eph.i0     = R8(&puiTmp[ 66]);
198 |     eph.crc    = R4(&puiTmp[ 74]);
199 |     eph.omg    = R8(&puiTmp[ 78]);
200 |     eph.OMGd   = R8(&puiTmp[ 86]) * 1e+3;
201 |     eph.idot   = R8(&puiTmp[ 94]) * 1e+3;
202 |     eph.tgd[0] = R4(&puiTmp[102]) * 1e-3;
203 |     toc        = R8(&puiTmp[106]) * 1e-3;
204 |     eph.f2     = R4(&puiTmp[114]) * 1e+3;
205 |     eph.f1     = R4(&puiTmp[118]);
206 |     eph.f0     = R4(&puiTmp[122]) * 1e-3;
207 |     eph.sva    = uraindex(I2(&puiTmp[126]));
208 |     eph.iode   = I2(&puiTmp[128]);
209 |     eph.iodc   = I2(&puiTmp[130]);
210 |     eph.code   = I2(&puiTmp[132]);
211 |     eph.flag   = I2(&puiTmp[134]);
212 |     week       = I2(&puiTmp[136]);
213 |     eph.fit    = 0;
214 |     
215 |     if (week>=4096) {
216 |         trace(2,"nvs gps ephemeris week error: sat=%2d week=%d\n",sat,week);
217 |         return -1;
218 |     }
219 |     eph.week=adjgpsweek(week);
220 |     eph.toe=gpst2time(eph.week,eph.toes);
221 |     eph.toc=gpst2time(eph.week,toc);
222 |     eph.ttr=raw->time;
223 |     
224 |     if (!strstr(raw->opt,"-EPHALL")) {
225 |         if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
226 |     }
227 |     eph.sat=sat;
228 |     raw->nav.eph[sat-1]=eph;
229 |     raw->ephsat=sat;
230 |     raw->ephset=0;
231 |     return 2;
232 | }
233 | /* adjust daily rollover of time ---------------------------------------------*/
234 | static gtime_t adjday(gtime_t time, double tod)
235 | {
236 |     double ep[6],tod_p;
237 |     time2epoch(time,ep);
238 |     tod_p=ep[3]*3600.0+ep[4]*60.0+ep[5];
239 |     if      (tod<tod_p-43200.0) tod+=86400.0;
240 |     else if (tod>tod_p+43200.0) tod-=86400.0;
241 |     ep[3]=ep[4]=ep[5]=0.0;
242 |     return timeadd(epoch2time(ep),tod);
243 | }
244 | /* decode gloephem -----------------------------------------------------------*/
245 | static int decode_gloephem(int sat, raw_t *raw)
246 | {
247 |     geph_t geph={0};
248 |     uint8_t *p=(raw->buff)+2;
249 |     int prn,tk,tb;
250 |     
251 |     if (raw->len>=93) {
252 |         prn        =I1(p+ 1);
253 |         geph.frq   =I1(p+ 2);
254 |         geph.pos[0]=R8(p+ 3);
255 |         geph.pos[1]=R8(p+11);
256 |         geph.pos[2]=R8(p+19);
257 |         geph.vel[0]=R8(p+27) * 1e+3;
258 |         geph.vel[1]=R8(p+35) * 1e+3;
259 |         geph.vel[2]=R8(p+43) * 1e+3;
260 |         geph.acc[0]=R8(p+51) * 1e+6;
261 |         geph.acc[1]=R8(p+59) * 1e+6;
262 |         geph.acc[2]=R8(p+67) * 1e+6;
263 |         tb = R8(p+75) * 1e-3;
264 |         tk = tb;
265 |         geph.gamn  =R4(p+83);
266 |         geph.taun  =R4(p+87) * 1e-3;
267 |         geph.age   =I2(p+91);
268 |     }
269 |     else {
270 |         trace(2,"nvs NE length error: len=%d\n",raw->len);
271 |         return -1;
272 |     }
273 |     if (!(geph.sat=satno(SYS_GLO,prn))) {
274 |         trace(2,"nvs NE satellite error: prn=%d\n",prn);
275 |         return -1;
276 |     }
277 |     if (raw->time.time==0) return 0;
278 |     
279 |     geph.iode=(tb/900)&0x7F;
280 |     geph.toe=utc2gpst(adjday(raw->time,tb-10800.0));
281 |     geph.tof=utc2gpst(adjday(raw->time,tk-10800.0));
282 | #if 0
283 |     /* check illegal ephemeris by toe */
284 |     tt=timediff(raw->time,geph.toe);
285 |     if (fabs(tt)>3600.0) {
286 |         trace(3,"nvs NE illegal toe: prn=%2d tt=%6.0f\n",prn,tt);
287 |         return 0;
288 |     }
289 | #endif
290 | #if 0
291 |     /* check illegal ephemeris by frequency number consistency */
292 |     if (raw->nav.geph[prn-MINPRNGLO].toe.time&&
293 |         geph.frq!=raw->nav.geph[prn-MINPRNGLO].frq) {
294 |         trace(2,"nvs NE illegal freq change: prn=%2d frq=%2d->%2d\n",prn,
295 |               raw->nav.geph[prn-MINPRNGLO].frq,geph.frq);
296 |         return -1;
297 |     }
298 |     if (!strstr(raw->opt,"-EPHALL")) {
299 |         if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&&
300 |             geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0;
301 |     }
302 | #endif
303 |     raw->nav.geph[prn-1]=geph;
304 |     raw->ephsat=geph.sat;
305 |     raw->ephset=0;
306 |     
307 |     return 2;
308 | }
309 | /* decode NVS ephemerides in clear -------------------------------------------*/
310 | static int decode_xf7eph(raw_t *raw)
311 | {
312 |     int prn,sat,sys;
313 |     uint8_t *p=raw->buff;
314 |     
315 |     trace(4,"decode_xf7eph: len=%d\n",raw->len);
316 |     
317 |     if ((raw->len)<93) {
318 |         trace(2,"nvs xf7eph length error: len=%d\n",raw->len);
319 |         return -1;
320 |     }
321 |     sys = (U1(p+2)==1)?SYS_GPS:((U1(p+2)==2)?SYS_GLO:SYS_NONE);
322 |     prn = U1(p+3);
323 |     if (!(sat=satno(sys==1?SYS_GPS:SYS_GLO,prn))) {
324 |         trace(2,"nvs xf7eph satellite number error: prn=%d\n",prn);
325 |         return -1;
326 |     }
327 |     if (sys==SYS_GPS) {
328 |         return decode_gpsephem(sat,raw);
329 |     }
330 |     else if (sys==SYS_GLO) {
331 |         return decode_gloephem(sat,raw);
332 |     }
333 |     return 0;
334 | }
335 | /* decode NVS rxm-sfrb: subframe buffer --------------------------------------*/
336 | static int decode_xe5bit(raw_t *raw)
337 | {
338 |     int prn;
339 |     int iBlkStartIdx, iExpLen, iIdx;
340 |     uint32_t words[10];
341 |     uint8_t uiDataBlocks, uiDataType;
342 |     uint8_t *p=raw->buff;
343 |     
344 |     trace(4,"decode_xe5bit: len=%d\n",raw->len);
345 |     
346 |     p += 2;         /* Discard preamble and message identifier */
347 |     uiDataBlocks = U1(p);
348 |     
349 |     if (uiDataBlocks>=16) {
350 |         trace(2,"nvs xf5bit message error: data blocks %u\n", uiDataBlocks);
351 |         return -1;
352 |     }
353 |     iBlkStartIdx = 1;
354 |     for (iIdx = 0; iIdx < uiDataBlocks; iIdx++) {
355 |         iExpLen = (iBlkStartIdx+10);
356 |         if ((raw->len) < iExpLen) {
357 |             trace(2,"nvs xf5bit message too short (expected at least %d)\n", iExpLen);
358 |             return -1;
359 |         }
360 |         uiDataType = U1(p+iBlkStartIdx+1);
361 |         
362 |         switch (uiDataType) {
363 |             case 1: /* Glonass */
364 |                 iBlkStartIdx += 19;
365 |                 break;
366 |             case 2: /* GPS */
367 |                 iBlkStartIdx += 47;
368 |                 break;
369 |             case 4: /* SBAS */
370 |                 prn = U1(p+(iBlkStartIdx+2)) + 120;
371 |                 
372 |                 /* sat = satno(SYS_SBS, prn); */
373 |                 /* sys = satsys(sat,&prn); */
374 |                 memset(words, 0, 10*sizeof(uint32_t));
375 |                 for (iIdx=0, iBlkStartIdx+=7; iIdx<10; iIdx++, iBlkStartIdx+=4) {
376 |                     words[iIdx]=U4(p+iBlkStartIdx);
377 |                 }
378 |                 words[7] >>= 6;
379 |                 return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg) ? 3 : 0;
380 |             default:
381 |                 trace(2,"nvs xf5bit SNS type unknown (got %d)\n", uiDataType);
382 |                 return -1;
383 |         }
384 |     }
385 |     return 0;
386 | }
387 | /* decode NVS x4aiono --------------------------------------------------------*/
388 | static int decode_x4aiono(raw_t *raw)
389 | {
390 |     uint8_t *p=raw->buff+2;
391 |     
392 |     trace(4,"decode_x4aiono: len=%d\n", raw->len);
393 |     
394 |     raw->nav.ion_gps[0] = R4(p   );
395 |     raw->nav.ion_gps[1] = R4(p+ 4);
396 |     raw->nav.ion_gps[2] = R4(p+ 8);
397 |     raw->nav.ion_gps[3] = R4(p+12);
398 |     raw->nav.ion_gps[4] = R4(p+16);
399 |     raw->nav.ion_gps[5] = R4(p+20);
400 |     raw->nav.ion_gps[6] = R4(p+24);
401 |     raw->nav.ion_gps[7] = R4(p+28);
402 |     
403 |     return 9;
404 | }
405 | /* decode NVS x4btime --------------------------------------------------------*/
406 | static int decode_x4btime(raw_t *raw)
407 | {
408 |     uint8_t *p=raw->buff+2;
409 |     
410 |     trace(4,"decode_x4btime: len=%d\n", raw->len);
411 |     
412 |     raw->nav.utc_gps[1] = R8(p   );
413 |     raw->nav.utc_gps[0] = R8(p+ 8);
414 |     raw->nav.utc_gps[2] = I4(p+16);
415 |     raw->nav.utc_gps[3] = I2(p+20);
416 |     raw->nav.utc_gps[4] = I1(p+22);
417 |     
418 |     return 9;
419 | }
420 | /* decode NVS raw message ----------------------------------------------------*/
421 | static int decode_nvs(raw_t *raw)
422 | {
423 |     int type=U1(raw->buff+1);
424 |     
425 |     trace(3,"decode_nvs: type=%02x len=%d\n",type,raw->len);
426 |     
427 |     sprintf(raw->msgtype,"NVS: type=%2d len=%3d",type,raw->len);
428 |     
429 |     switch (type) {
430 |         case ID_XF5RAW:  return decode_xf5raw (raw);
431 |         case ID_XF7EPH:  return decode_xf7eph (raw);
432 |         case ID_XE5BIT:  return decode_xe5bit (raw);
433 |         case ID_X4AIONO: return decode_x4aiono(raw);
434 |         case ID_X4BTIME: return decode_x4btime(raw);
435 |         default: break;
436 |     }
437 |     return 0;
438 | }
439 | /* input NVS raw message from stream -------------------------------------------
440 | * fetch next NVS raw data and input a message from stream
441 | * args   : raw_t *raw   IO    receiver raw data control struct
442 | *          uint8_t data I     stream data (1 byte)
443 | * return : status (-1: error message, 0: no message, 1: input observation data,
444 | *                  2: input ephemeris, 3: input sbas message,
445 | *                  9: input ion/utc parameter)
446 | *
447 | * notes  : to specify input options, set raw->opt to the following option
448 | *          strings separated by spaces.
449 | *
450 | *          -EPHALL    : input all ephemerides
451 | *          -TADJ=tint : adjust time tags to multiples of tint (sec)
452 | *
453 | *-----------------------------------------------------------------------------*/
454 | extern int input_nvs(raw_t *raw, uint8_t data)
455 | {
456 |     trace(5,"input_nvs: data=%02x\n",data);
457 |     
458 |     /* synchronize frame */
459 |     if ((raw->nbyte==0) && (data==NVSSYNC)) {
460 |         
461 |         /* Search a 0x10 */
462 |         raw->buff[0] = data;
463 |         raw->nbyte=1;
464 |         return 0;
465 |     }
466 |     if ((raw->nbyte==1) && (data != NVSSYNC) && (data != NVSENDMSG)) {
467 |         
468 |         /* Discard double 0x10 and 0x10 0x03 at beginning of frame */
469 |         raw->buff[1]=data;
470 |         raw->nbyte=2;
471 |         raw->flag=0;
472 |         return 0;
473 |     }
474 |     /* This is all done to discard a double 0x10 */
475 |     if (data==NVSSYNC) raw->flag = (raw->flag +1) % 2;
476 |     if ((data!=NVSSYNC) || (raw->flag)) {
477 |         
478 |         /* Store the new byte */
479 |         raw->buff[(raw->nbyte++)] = data;
480 |     }
481 |     /* Detect ending sequence */
482 |     if ((data==NVSENDMSG) && (raw->flag)) {
483 |         raw->len   = raw->nbyte;
484 |         raw->nbyte = 0;
485 |         
486 |         /* Decode NVS raw message */
487 |         return decode_nvs(raw);
488 |     }
489 |     if (raw->nbyte == MAXRAWLEN) {
490 |         trace(2,"nvs message size error: len=%d\n",raw->nbyte);
491 |         raw->nbyte=0;
492 |         return -1;
493 |     }
494 |     return 0;
495 | }
496 | /* input NVS raw message from file ---------------------------------------------
497 | * fetch next NVS raw data and input a message from file
498 | * args   : raw_t  *raw  IO    receiver raw data control struct
499 | *          FILE   *fp   I     file pointer
500 | * return : status(-2: end of file, -1...9: same as above)
501 | *-----------------------------------------------------------------------------*/
502 | extern int input_nvsf(raw_t *raw, FILE *fp)
503 | {
504 |     int i,data, odd=0;
505 |     
506 |     trace(4,"input_nvsf:\n");
507 |     
508 |     /* synchronize frame */
509 |     for (i=0;;i++) {
510 |         if ((data=fgetc(fp))==EOF) return -2;
511 |         
512 |         /* Search a 0x10 */
513 |         if (data==NVSSYNC) {
514 |             
515 |             /* Store the frame begin */
516 |             raw->buff[0] = data;
517 |             if ((data=fgetc(fp))==EOF) return -2;
518 |             
519 |             /* Discard double 0x10 and 0x10 0x03 */
520 |             if ((data != NVSSYNC) && (data != NVSENDMSG)) {
521 |                 raw->buff[1]=data;
522 |                 break;
523 |             }
524 |         }
525 |         if (i>=4096) return 0;
526 |     }
527 |     raw->nbyte = 2;
528 |     for (i=0;;i++) {
529 |         if ((data=fgetc(fp))==EOF) return -2;
530 |         if (data==NVSSYNC) odd=(odd+1)%2;
531 |         if ((data!=NVSSYNC) || odd) {
532 |             
533 |             /* Store the new byte */
534 |             raw->buff[(raw->nbyte++)] = data;
535 |         }
536 |         /* Detect ending sequence */
537 |         if ((data==NVSENDMSG) && odd) break;
538 |         if (i>=4096) return 0;
539 |     }
540 |     raw->len = raw->nbyte;
541 |     if ((raw->len) > MAXRAWLEN) {
542 |         trace(2,"nvs length error: len=%d\n",raw->len);
543 |         return -1;
544 |     }
545 |     /* decode nvs raw message */
546 |     return decode_nvs(raw);
547 | }
548 | /* generate NVS binary message -------------------------------------------------
549 | * generate NVS binary message from message string
550 | * args   : char  *msg   I      message string
551 | *            "RESTART  [arg...]" system reset
552 | *            "CFG-SERI [arg...]" configure serial port property
553 | *            "CFG-FMT  [arg...]" configure output message format
554 | *            "CFG-RATE [arg...]" configure binary measurement output rates
555 | *          uint8_t *buff O binary message
556 | * return : length of binary message (0: error)
557 | * note   : see reference [1][2] for details.
558 | *-----------------------------------------------------------------------------*/
559 | extern int gen_nvs(const char *msg, uint8_t *buff)
560 | {
561 |     uint8_t *q=buff;
562 |     char mbuff[1024],*args[32],*p;
563 |     uint32_t byte;
564 |     int iRate,n,narg=0;
565 |     uint8_t ui100Ms;
566 |     
567 |     trace(4,"gen_nvs: msg=%s\n",msg);
568 |     
569 |     strcpy(mbuff,msg);
570 |     for (p=strtok(mbuff," ");p&&narg<32;p=strtok(NULL," ")) {
571 |         args[narg++]=p;
572 |     }
573 |     if (narg<1) {
574 |         return 0;
575 |     }
576 |     *q++=NVSSYNC; /* DLE */
577 |     
578 |     if (!strcmp(args[0],"CFG-PVTRATE")) {
579 |         *q++=ID_XD7ADVANCED;
580 |         *q++=ID_X02RATEPVT;
581 |         if (narg>1) {
582 |             iRate = atoi(args[1]);
583 |             *q++ = (uint8_t) iRate;
584 |         }
585 |     }
586 |     else if (!strcmp(args[0],"CFG-RAWRATE")) {
587 |         *q++=ID_XF4RATERAW;
588 |         if (narg>1) {
589 |             iRate = atoi(args[1]);
590 |             switch(iRate) {
591 |                 case 2:  ui100Ms =  5; break;
592 |                 case 5:  ui100Ms =  2; break;
593 |                 case 10: ui100Ms =  1; break;
594 |                 default: ui100Ms = 10; break;
595 |             }
596 |             *q++ = ui100Ms;
597 |         }
598 |     }
599 |     else if (!strcmp(args[0],"CFG-SMOOTH")) {
600 |         *q++=ID_XD7SMOOTH;
601 |         *q++ = 0x03;
602 |         *q++ = 0x01;
603 |         *q++ = 0x00;
604 |     }
605 |     else if (!strcmp(args[0],"CFG-BINR")) {
606 |         for (n=1;(n<narg);n++) {
607 |             if (sscanf(args[n], "%2x",&byte)) *q++=(uint8_t)byte;
608 |         }
609 |     }
610 |     else return 0;
611 |     
612 |     n=(int)(q-buff);
613 |     
614 |     *q++=0x10; /* ETX */
615 |     *q=0x03;   /* DLE */
616 |     return n+2;
617 | }
618 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/rtcm.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * rtcm.c : rtcm functions
  3 | *
  4 | *          Copyright (C) 2009-2020 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references :
  7 | *     [1] RTCM Recommended Standards for Differential GNSS (Global Navigation
  8 | *         Satellite Systems) Service version 2.3, August 20, 2001
  9 | *     [7] RTCM Standard 10403.1 - Amendment 5, Differential GNSS (Global
 10 | *         Navigation Satellite Systems) Services - version 3, July 1, 2011
 11 | *     [10] RTCM Paper 059-2011-SC104-635 (draft Galileo and QZSS ssr messages)
 12 | *     [15] RTCM Standard 10403.2, Differential GNSS (Global Navigation Satellite
 13 | *          Systems) Services - version 3, with amendment 1/2, November 7, 2013
 14 | *     [16] Proposal of new RTCM SSR Messages (ssr_1_gal_qzss_sbas_dbs_v05)
 15 | *          2014/04/17
 16 | *     [17] RTCM Standard 10403.3, Differential GNSS (Global Navigation Satellite
 17 | *          Systems) Services - version 3, with amendment 1, April 28, 2020
 18 | *     [18] IGS State Space Representation (SSR) Format version 1.00, October 5,
 19 | *          2020
 20 | *
 21 | * version : $Revision:$ $Date:$
 22 | * history : 2009/04/10 1.0  new
 23 | *           2009/06/29 1.1  support type 1009-1012 to get synchronous-gnss-flag
 24 | *           2009/12/04 1.2  support type 1010,1012,1020
 25 | *           2010/07/15 1.3  support type 1057-1068 for ssr corrections
 26 | *                           support type 1007,1008,1033 for antenna info
 27 | *           2010/09/08 1.4  fix problem of ephemeris and ssr sequence upset
 28 | *                           (2.4.0_p8)
 29 | *           2012/05/11 1.5  comply with RTCM 3 final SSR format (RTCM 3
 30 | *                           Amendment 5) (ref [7]) (2.4.1_p6)
 31 | *           2012/05/14 1.6  separate rtcm2.c, rtcm3.c
 32 | *                           add options to select used codes for msm
 33 | *           2013/04/27 1.7  comply with rtcm 3.2 with amendment 1/2 (ref[15])
 34 | *           2013/12/06 1.8  support SBAS/BeiDou SSR messages (ref[16])
 35 | *           2018/01/29 1.9  support RTCM 3.3 (ref[17])
 36 | *                           crc24q() -> rtk_crc24q()
 37 | *           2018/10/10 1.10 fix bug on initializing rtcm struct
 38 | *                           add rtcm option -GALINAV, -GALFNAV
 39 | *           2018/11/05 1.11 add notes for api gen_rtcm3()
 40 | *           2020/11/30 1.12 modify API gen_rtcm3()
 41 | *                           support NavIC/IRNSS MSM and ephemeris (ref [17])
 42 | *                           allocate double size of ephemeris buffer to support
 43 | *                            multiple ephemeris sets in init_rtcm()
 44 | *                           delete references [2]-[6],[8],[9],[11]-[14]
 45 | *                           update reference [17]
 46 | *                           use integer types in stdint.h
 47 | *-----------------------------------------------------------------------------*/
 48 | #include "rtklib.h"
 49 | 
 50 | /* function prototypes -------------------------------------------------------*/
 51 | extern int decode_rtcm2(rtcm_t *rtcm);
 52 | extern int decode_rtcm3(rtcm_t *rtcm);
 53 | extern int encode_rtcm3(rtcm_t *rtcm, int type, int subtype, int sync);
 54 | 
 55 | /* constants -----------------------------------------------------------------*/
 56 | 
 57 | #define RTCM2PREAMB 0x66        /* rtcm ver.2 frame preamble */
 58 | #define RTCM3PREAMB 0xD3        /* rtcm ver.3 frame preamble */
 59 | 
 60 | /* initialize rtcm control -----------------------------------------------------
 61 | * initialize rtcm control struct and reallocate memory for observation and
 62 | * ephemeris buffer in rtcm control struct
 63 | * args   : rtcm_t *raw   IO     rtcm control struct
 64 | * return : status (1:ok,0:memory allocation error)
 65 | *-----------------------------------------------------------------------------*/
 66 | extern int init_rtcm(rtcm_t *rtcm)
 67 | {
 68 |     gtime_t time0={0};
 69 |     obsd_t data0={{0}};
 70 |     eph_t  eph0 ={0,-1,-1};
 71 |     geph_t geph0={0,-1};
 72 |     ssr_t ssr0={{{0}}};
 73 |     int i,j;
 74 |     
 75 |     trace(3,"init_rtcm:\n");
 76 |     
 77 |     rtcm->staid=rtcm->stah=rtcm->seqno=rtcm->outtype=0;
 78 |     rtcm->time=rtcm->time_s=time0;
 79 |     rtcm->sta.name[0]=rtcm->sta.marker[0]='\0';
 80 |     rtcm->sta.antdes[0]=rtcm->sta.antsno[0]='\0';
 81 |     rtcm->sta.rectype[0]=rtcm->sta.recver[0]=rtcm->sta.recsno[0]='\0';
 82 |     rtcm->sta.antsetup=rtcm->sta.itrf=rtcm->sta.deltype=0;
 83 |     for (i=0;i<3;i++) {
 84 |         rtcm->sta.pos[i]=rtcm->sta.del[i]=0.0;
 85 |     }
 86 |     rtcm->sta.hgt=0.0;
 87 |     rtcm->dgps=NULL;
 88 |     for (i=0;i<MAXSAT;i++) {
 89 |         rtcm->ssr[i]=ssr0;
 90 |     }
 91 |     rtcm->msg[0]=rtcm->msgtype[0]=rtcm->opt[0]='\0';
 92 |     for (i=0;i<6;i++) rtcm->msmtype[i][0]='\0';
 93 |     rtcm->obsflag=rtcm->ephsat=0;
 94 |     for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ+NEXOBS;j++) {
 95 |         rtcm->cp[i][j]=0.0;
 96 |         rtcm->lock[i][j]=rtcm->loss[i][j]=0;
 97 |         rtcm->lltime[i][j]=time0;
 98 |     }
 99 |     rtcm->nbyte=rtcm->nbit=rtcm->len=0;
100 |     rtcm->word=0;
101 |     for (i=0;i<100;i++) rtcm->nmsg2[i]=0;
102 |     for (i=0;i<400;i++) rtcm->nmsg3[i]=0;
103 |     
104 |     rtcm->obs.data=NULL;
105 |     rtcm->nav.eph =NULL;
106 |     rtcm->nav.geph=NULL;
107 |     
108 |     /* reallocate memory for observation and ephemeris buffer */
109 |     if (!(rtcm->obs.data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
110 |         !(rtcm->nav.eph =(eph_t  *)malloc(sizeof(eph_t )*MAXSAT*2))||
111 |         !(rtcm->nav.geph=(geph_t *)malloc(sizeof(geph_t)*MAXPRNGLO))) {
112 |         free_rtcm(rtcm);
113 |         return 0;
114 |     }
115 |     rtcm->obs.n=0;
116 |     rtcm->nav.n=MAXSAT*2;
117 |     rtcm->nav.ng=MAXPRNGLO;
118 |     for (i=0;i<MAXOBS   ;i++) rtcm->obs.data[i]=data0;
119 |     for (i=0;i<MAXSAT*2 ;i++) rtcm->nav.eph [i]=eph0;
120 |     for (i=0;i<MAXPRNGLO;i++) rtcm->nav.geph[i]=geph0;
121 |     return 1;
122 | }
123 | /* free rtcm control ----------------------------------------------------------
124 | * free observation and ephemeris buffer in rtcm control struct
125 | * args   : rtcm_t *raw   IO     rtcm control struct
126 | * return : none
127 | *-----------------------------------------------------------------------------*/
128 | extern void free_rtcm(rtcm_t *rtcm)
129 | {
130 |     trace(3,"free_rtcm:\n");
131 |     
132 |     /* free memory for observation and ephemeris buffer */
133 |     free(rtcm->obs.data); rtcm->obs.data=NULL; rtcm->obs.n=0;
134 |     free(rtcm->nav.eph ); rtcm->nav.eph =NULL; rtcm->nav.n=0;
135 |     free(rtcm->nav.geph); rtcm->nav.geph=NULL; rtcm->nav.ng=0;
136 | }
137 | /* input RTCM 2 message from stream --------------------------------------------
138 | * fetch next RTCM 2 message and input a message from byte stream
139 | * args   : rtcm_t *rtcm IO   rtcm control struct
140 | *          uint8_t data     I   stream data (1 byte)
141 | * return : status (-1: error message, 0: no message, 1: input observation data,
142 | *                  2: input ephemeris, 5: input station pos/ant parameters,
143 | *                  6: input time parameter, 7: input dgps corrections,
144 | *                  9: input special message)
145 | * notes  : before firstly calling the function, time in rtcm control struct has
146 | *          to be set to the approximate time within 1/2 hour in order to resolve
147 | *          ambiguity of time in rtcm messages.
148 | *          supported msgs RTCM ver.2: 1,3,9,14,16,17,18,19,22
149 | *          refer [1] for RTCM ver.2
150 | *-----------------------------------------------------------------------------*/
151 | extern int input_rtcm2(rtcm_t *rtcm, uint8_t data)
152 | {
153 |     uint8_t preamb;
154 |     int i;
155 |     
156 |     trace(5,"input_rtcm2: data=%02x\n",data);
157 |     
158 |     if ((data&0xC0)!=0x40) return 0; /* ignore if upper 2bit != 01 */
159 |     
160 |     for (i=0;i<6;i++,data>>=1) { /* decode 6-of-8 form */
161 |         rtcm->word=(rtcm->word<<1)+(data&1);
162 |         
163 |         /* synchronize frame */
164 |         if (rtcm->nbyte==0) {
165 |             preamb=(uint8_t)(rtcm->word>>22);
166 |             if (rtcm->word&0x40000000) preamb^=0xFF; /* decode preamble */
167 |             if (preamb!=RTCM2PREAMB) continue;
168 |             
169 |             /* check parity */
170 |             if (!decode_word(rtcm->word,rtcm->buff)) continue;
171 |             rtcm->nbyte=3; rtcm->nbit=0;
172 |             continue;
173 |         }
174 |         if (++rtcm->nbit<30) continue; else rtcm->nbit=0;
175 |         
176 |         /* check parity */
177 |         if (!decode_word(rtcm->word,rtcm->buff+rtcm->nbyte)) {
178 |             trace(2,"rtcm2 partity error: i=%d word=%08x\n",i,rtcm->word);
179 |             rtcm->nbyte=0; rtcm->word&=0x3;
180 |             continue;
181 |         }
182 |         rtcm->nbyte+=3;
183 |         if (rtcm->nbyte==6) rtcm->len=(rtcm->buff[5]>>3)*3+6;
184 |         if (rtcm->nbyte<rtcm->len) continue;
185 |         rtcm->nbyte=0; rtcm->word&=0x3;
186 |         
187 |         /* decode rtcm2 message */
188 |         return decode_rtcm2(rtcm);
189 |     }
190 |     return 0;
191 | }
192 | /* input RTCM 3 message from stream --------------------------------------------
193 | * fetch next RTCM 3 message and input a message from byte stream
194 | * args   : rtcm_t *rtcm IO   rtcm control struct
195 | *          uint8_t data     I   stream data (1 byte)
196 | * return : status (-1: error message, 0: no message, 1: input observation data,
197 | *                  2: input ephemeris, 5: input station pos/ant parameters,
198 | *                  10: input ssr messages)
199 | * notes  : before firstly calling the function, time in rtcm control struct has
200 | *          to be set to the approximate time within 1/2 week in order to resolve
201 | *          ambiguity of time in rtcm messages.
202 | *          
203 | *          to specify input options, set rtcm->opt to the following option
204 | *          strings separated by spaces.
205 | *
206 | *          -EPHALL  : input all ephemerides (default: only new)
207 | *          -STA=nnn : input only message with STAID=nnn (default: all)
208 | *          -GLss    : select signal ss for GPS MSM (ss=1C,1P,...)
209 | *          -RLss    : select signal ss for GLO MSM (ss=1C,1P,...)
210 | *          -ELss    : select signal ss for GAL MSM (ss=1C,1B,...)
211 | *          -JLss    : select signal ss for QZS MSM (ss=1C,2C,...)
212 | *          -CLss    : select signal ss for BDS MSM (ss=2I,7I,...)
213 | *          -ILss    : select signal ss for IRN MSM (ss=5A,9A,...)
214 | *          -GALINAV : select I/NAV for Galileo ephemeris (default: all)
215 | *          -GALFNAV : select F/NAV for Galileo ephemeris (default: all)
216 | *
217 | *          supported RTCM 3 messages (ref [7][10][15][16][17][18])
218 | *
219 | *            TYPE       :  GPS   GLONASS Galileo  QZSS     BDS    SBAS    NavIC
220 | *         ----------------------------------------------------------------------
221 | *          OBS COMP L1  : 1001~   1009~     -       -       -       -       -
222 | *              FULL L1  : 1002    1010      -       -       -       -       -
223 | *              COMP L1L2: 1003~   1011~     -       -       -       -       -
224 | *              FULL L1L2: 1004    1012      -       -       -       -       -
225 | *
226 | *          NAV          : 1019    1020    1045**  1044    1042      -     1041
227 | *                           -       -     1046**    -       63*     -       -
228 | *
229 | *          MSM 1        : 1071~   1081~   1091~   1111~   1121~   1101~   1131~
230 | *              2        : 1072~   1082~   1092~   1112~   1122~   1102~   1132~
231 | *              3        : 1073~   1083~   1093~   1113~   1123~   1103~   1133~
232 | *              4        : 1074    1084    1094    1114    1124    1104    1134
233 | *              5        : 1075    1085    1095    1115    1125    1105    1135 
234 | *              6        : 1076    1086    1096    1116    1126    1106    1136 
235 | *              7        : 1077    1087    1097    1117    1127    1107    1137 
236 | *
237 | *          SSR ORBIT    : 1057    1063    1240*   1246*   1258*     -       -
238 | *              CLOCK    : 1058    1064    1241*   1247*   1259*     -       -
239 | *              CODE BIAS: 1059    1065    1242*   1248*   1260*     -       -
240 | *              OBT/CLK  : 1060    1066    1243*   1249*   1261*     -       -
241 | *              URA      : 1061    1067    1244*   1250*   1262*     -       -
242 | *              HR-CLOCK : 1062    1068    1245*   1251*   1263*     -       -
243 | *              PHAS BIAS:   11*     -       12*     13*     14*     -       -
244 | *
245 | *          ANT/RCV INFO : 1007    1008    1033
246 | *          STA POSITION : 1005    1006
247 | *
248 | *          PROPRIETARY  : 4076 (IGS)
249 | *         ----------------------------------------------------------------------
250 | *                            (* draft, ** 1045:F/NAV,1046:I/NAV, ~ only encode)
251 | *
252 | *          for MSM observation data with multiple signals for a frequency,
253 | *          a signal is selected according to internal priority. to select
254 | *          a specified signal, use the input options.
255 | *
256 | *          RTCM 3 message format:
257 | *            +----------+--------+-----------+--------------------+----------+
258 | *            | preamble | 000000 |  length   |    data message    |  parity  |
259 | *            +----------+--------+-----------+--------------------+----------+
260 | *            |<-- 8 --->|<- 6 -->|<-- 10 --->|<--- length x 8 --->|<-- 24 -->|
261 | *            
262 | *-----------------------------------------------------------------------------*/
263 | extern int input_rtcm3(rtcm_t *rtcm, uint8_t data)
264 | {
265 |     trace(5,"input_rtcm3: data=%02x\n",data);
266 |     
267 |     /* synchronize frame */
268 |     if (rtcm->nbyte==0) {
269 |         if (data!=RTCM3PREAMB) return 0;
270 |         rtcm->buff[rtcm->nbyte++]=data;
271 |         return 0;
272 |     }
273 |     rtcm->buff[rtcm->nbyte++]=data;
274 |     
275 |     if (rtcm->nbyte==3) {
276 |         rtcm->len=getbitu(rtcm->buff,14,10)+3; /* length without parity */
277 |     }
278 |     if (rtcm->nbyte<3||rtcm->nbyte<rtcm->len+3) return 0;
279 |     rtcm->nbyte=0;
280 |     
281 |     /* check parity */
282 |     if (rtk_crc24q(rtcm->buff,rtcm->len)!=getbitu(rtcm->buff,rtcm->len*8,24)) {
283 |         trace(2,"rtcm3 parity error: len=%d\n",rtcm->len);
284 |         return 0;
285 |     }
286 |     /* decode rtcm3 message */
287 |     return decode_rtcm3(rtcm);
288 | }
289 | /* input RTCM 2 message from file ----------------------------------------------
290 | * fetch next RTCM 2 message and input a messsage from file
291 | * args   : rtcm_t *rtcm IO   rtcm control struct
292 | *          FILE  *fp    I    file pointer
293 | * return : status (-2: end of file, -1...10: same as above)
294 | * notes  : same as above
295 | *-----------------------------------------------------------------------------*/
296 | extern int input_rtcm2f(rtcm_t *rtcm, FILE *fp)
297 | {
298 |     int i,data=0,ret;
299 |     
300 |     trace(4,"input_rtcm2f: data=%02x\n",data);
301 |     
302 |     for (i=0;i<4096;i++) {
303 |         if ((data=fgetc(fp))==EOF) return -2;
304 |         if ((ret=input_rtcm2(rtcm,(uint8_t)data))) return ret;
305 |     }
306 |     return 0; /* return at every 4k bytes */
307 | }
308 | /* input RTCM 3 message from file ----------------------------------------------
309 | * fetch next RTCM 3 message and input a messsage from file
310 | * args   : rtcm_t *rtcm IO   rtcm control struct
311 | *          FILE  *fp    I    file pointer
312 | * return : status (-2: end of file, -1...10: same as above)
313 | * notes  : same as above
314 | *-----------------------------------------------------------------------------*/
315 | extern int input_rtcm3f(rtcm_t *rtcm, FILE *fp)
316 | {
317 |     int i,data=0,ret;
318 |     
319 |     trace(4,"input_rtcm3f: data=%02x\n",data);
320 |     
321 |     for (i=0;i<4096;i++) {
322 |         if ((data=fgetc(fp))==EOF) return -2;
323 |         if ((ret=input_rtcm3(rtcm,(uint8_t)data))) return ret;
324 |     }
325 |     return 0; /* return at every 4k bytes */
326 | }
327 | /* generate RTCM 2 message -----------------------------------------------------
328 | * generate RTCM 2 message
329 | * args   : rtcm_t *rtcm   IO rtcm control struct
330 | *          int    type    I  message type
331 | *          int    sync    I  sync flag (1:another message follows)
332 | * return : status (1:ok,0:error)
333 | *-----------------------------------------------------------------------------*/
334 | extern int gen_rtcm2(rtcm_t *rtcm, int type, int sync)
335 | {
336 |     trace(4,"gen_rtcm2: type=%d sync=%d\n",type,sync);
337 |     
338 |     rtcm->nbit=rtcm->len=rtcm->nbyte=0;
339 |     
340 |     /* not yet implemented */
341 |     
342 |     return 0;
343 | }
344 | /* generate RTCM 3 message -----------------------------------------------------
345 | * generate RTCM 3 message
346 | * args   : rtcm_t *rtcm   IO rtcm control struct
347 | *          int    type    I  message type
348 | *          int    subtype   I   message subtype
349 | *          int    sync    I  sync flag (1:another message follows)
350 | * return : status (1:ok,0:error)
351 | * notes  : For rtcm 3 msm, the {nsat} x {nsig} in rtcm->obs should not exceed
352 | *          64. If {nsat} x {nsig} of the input obs data exceeds 64, separate
353 | *          them to multiple ones and call gen_rtcm3() multiple times as user
354 | *          responsibility.
355 | *          ({nsat} = number of valid satellites, {nsig} = number of signals in
356 | *          the obs data) 
357 | *-----------------------------------------------------------------------------*/
358 | extern int gen_rtcm3(rtcm_t *rtcm, int type, int subtype, int sync)
359 | {
360 |     uint32_t crc;
361 |     int i=0;
362 |     
363 |     trace(4,"gen_rtcm3: type=%d subtype=%d sync=%d\n",type,subtype,sync);
364 |     
365 |     rtcm->nbit=rtcm->len=rtcm->nbyte=0;
366 |     
367 |     /* set preamble and reserved */
368 |     setbitu(rtcm->buff,i, 8,RTCM3PREAMB); i+= 8;
369 |     setbitu(rtcm->buff,i, 6,0          ); i+= 6;
370 |     setbitu(rtcm->buff,i,10,0          ); i+=10;
371 |     
372 |     /* encode rtcm 3 message body */
373 |     if (!encode_rtcm3(rtcm,type,subtype,sync)) return 0;
374 |     
375 |     /* padding to align 8 bit boundary */
376 |     for (i=rtcm->nbit;i%8;i++) {
377 |         setbitu(rtcm->buff,i,1,0);
378 |     }
379 |     /* message length (header+data) (bytes) */
380 |     if ((rtcm->len=i/8)>=3+1024) {
381 |         trace(2,"generate rtcm 3 message length error len=%d\n",rtcm->len-3);
382 |         rtcm->nbit=rtcm->len=0;
383 |         return 0;
384 |     }
385 |     /* message length without header and parity */
386 |     setbitu(rtcm->buff,14,10,rtcm->len-3);
387 |     
388 |     /* crc-24q */
389 |     crc=rtk_crc24q(rtcm->buff,rtcm->len);
390 |     setbitu(rtcm->buff,i,24,crc);
391 |     
392 |     /* length total (bytes) */
393 |     rtcm->nbyte=rtcm->len+3;
394 |     
395 |     return 1;
396 | }
397 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/rtcm2.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * rtcm2.c : rtcm ver.2 message functions
  3 | *
  4 | *          Copyright (C) 2009-2014 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references :
  7 | *     see rtcm.c
  8 | *
  9 | * version : $Revision:$ $Date:$
 10 | * history : 2011/11/28 1.0  separated from rtcm.c
 11 | *           2014/10/21 1.1  fix problem on week rollover in rtcm 2 type 14
 12 | *-----------------------------------------------------------------------------*/
 13 | #include "rtklib.h"
 14 | 
 15 | /* adjust hourly rollover of rtcm 2 time -------------------------------------*/
 16 | static void adjhour(rtcm_t *rtcm, double zcnt)
 17 | {
 18 |     double tow,hour,sec;
 19 |     int week;
 20 |     
 21 |     /* if no time, get cpu time */
 22 |     if (rtcm->time.time==0) rtcm->time=utc2gpst(timeget());
 23 |     tow=time2gpst(rtcm->time,&week);
 24 |     hour=floor(tow/3600.0);
 25 |     sec=tow-hour*3600.0;
 26 |     if      (zcnt<sec-1800.0) zcnt+=3600.0;
 27 |     else if (zcnt>sec+1800.0) zcnt-=3600.0;
 28 |     rtcm->time=gpst2time(week,hour*3600+zcnt);
 29 | }
 30 | /* get observation data index ------------------------------------------------*/
 31 | static int obsindex(obs_t *obs, gtime_t time, int sat)
 32 | {
 33 |     int i,j;
 34 |     
 35 |     for (i=0;i<obs->n;i++) {
 36 |         if (obs->data[i].sat==sat) return i; /* field already exists */
 37 |     }
 38 |     if (i>=MAXOBS) return -1; /* overflow */
 39 |     
 40 |     /* add new field */
 41 |     obs->data[i].time=time;
 42 |     obs->data[i].sat=sat;
 43 |     for (j=0;j<NFREQ;j++) {
 44 |         obs->data[i].L[j]=obs->data[i].P[j]=0.0;
 45 |         obs->data[i].D[j]=0.0;
 46 |         obs->data[i].SNR[j]=obs->data[i].LLI[j]=obs->data[i].code[j]=0;
 47 |     }
 48 |     obs->n++;
 49 |     return i;
 50 | }
 51 | /* decode type 1/9: differential gps correction/partial correction set -------*/
 52 | static int decode_type1(rtcm_t *rtcm)
 53 | {
 54 |     int i=48,fact,udre,prn,sat,iod;
 55 |     double prc,rrc;
 56 |     
 57 |     trace(4,"decode_type1: len=%d\n",rtcm->len);
 58 |     
 59 |     while (i+40<=rtcm->len*8) {
 60 |         fact=getbitu(rtcm->buff,i, 1); i+= 1;
 61 |         udre=getbitu(rtcm->buff,i, 2); i+= 2;
 62 |         prn =getbitu(rtcm->buff,i, 5); i+= 5;
 63 |         prc =getbits(rtcm->buff,i,16); i+=16;
 64 |         rrc =getbits(rtcm->buff,i, 8); i+= 8;
 65 |         iod =getbits(rtcm->buff,i, 8); i+= 8;
 66 |         if (prn==0) prn=32;
 67 |         if (prc==0x80000000||rrc==0xFFFF8000) {
 68 |             trace(2,"rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n",prn);
 69 |             continue;
 70 |         }
 71 |         if (rtcm->dgps) {
 72 |             sat=satno(SYS_GPS,prn);
 73 |             rtcm->dgps[sat-1].t0=rtcm->time;
 74 |             rtcm->dgps[sat-1].prc=prc*(fact?0.32:0.02);
 75 |             rtcm->dgps[sat-1].rrc=rrc*(fact?0.032:0.002);
 76 |             rtcm->dgps[sat-1].iod=iod;
 77 |             rtcm->dgps[sat-1].udre=udre;
 78 |         }
 79 |     }
 80 |     return 7;
 81 | }
 82 | /* decode type 3: reference station parameter --------------------------------*/
 83 | static int decode_type3(rtcm_t *rtcm)
 84 | {
 85 |     int i=48;
 86 |     
 87 |     trace(4,"decode_type3: len=%d\n",rtcm->len);
 88 |     
 89 |     if (i+96<=rtcm->len*8) {
 90 |         rtcm->sta.pos[0]=getbits(rtcm->buff,i,32)*0.01; i+=32;
 91 |         rtcm->sta.pos[1]=getbits(rtcm->buff,i,32)*0.01; i+=32;
 92 |         rtcm->sta.pos[2]=getbits(rtcm->buff,i,32)*0.01;
 93 |     }
 94 |     else {
 95 |         trace(2,"rtcm2 3 length error: len=%d\n",rtcm->len);
 96 |         return -1;
 97 |     }
 98 |     return 5;
 99 | }
100 | /* decode type 14: gps time of week ------------------------------------------*/
101 | static int decode_type14(rtcm_t *rtcm)
102 | {
103 |     double zcnt;
104 |     int i=48,week,hour,leaps;
105 |     
106 |     trace(4,"decode_type14: len=%d\n",rtcm->len);
107 |     
108 |     zcnt=getbitu(rtcm->buff,24,13);
109 |     if (i+24<=rtcm->len*8) {
110 |         week =getbitu(rtcm->buff,i,10); i+=10;
111 |         hour =getbitu(rtcm->buff,i, 8); i+= 8;
112 |         leaps=getbitu(rtcm->buff,i, 6);
113 |     }
114 |     else {
115 |         trace(2,"rtcm2 14 length error: len=%d\n",rtcm->len);
116 |         return -1;
117 |     }
118 |     week=adjgpsweek(week);
119 |     rtcm->time=gpst2time(week,hour*3600.0+zcnt*0.6);
120 |     rtcm->nav.utc_gps[4]=leaps;
121 |     return 6;
122 | }
123 | /* decode type 16: gps special message ---------------------------------------*/
124 | static int decode_type16(rtcm_t *rtcm)
125 | {
126 |     int i=48,n=0;
127 |     
128 |     trace(4,"decode_type16: len=%d\n",rtcm->len);
129 |     
130 |     while (i+8<=rtcm->len*8&&n<90) {
131 |         rtcm->msg[n++]=getbitu(rtcm->buff,i,8); i+=8;
132 |     }
133 |     rtcm->msg[n]='\0';
134 |     
135 |     trace(3,"rtcm2 16 message: %s\n",rtcm->msg);
136 |     return 9;
137 | }
138 | /* decode type 17: gps ephemerides -------------------------------------------*/
139 | static int decode_type17(rtcm_t *rtcm)
140 | {
141 |     eph_t eph={0};
142 |     double toc,sqrtA;
143 |     int i=48,week,prn,sat;
144 |     
145 |     trace(4,"decode_type17: len=%d\n",rtcm->len);
146 |     
147 |     if (i+480<=rtcm->len*8) {
148 |         week      =getbitu(rtcm->buff,i,10);              i+=10;
149 |         eph.idot  =getbits(rtcm->buff,i,14)*P2_43*SC2RAD; i+=14;
150 |         eph.iode  =getbitu(rtcm->buff,i, 8);              i+= 8;
151 |         toc       =getbitu(rtcm->buff,i,16)*16.0;         i+=16;
152 |         eph.f1    =getbits(rtcm->buff,i,16)*P2_43;        i+=16;
153 |         eph.f2    =getbits(rtcm->buff,i, 8)*P2_55;        i+= 8;
154 |         eph.crs   =getbits(rtcm->buff,i,16)*P2_5;         i+=16;
155 |         eph.deln  =getbits(rtcm->buff,i,16)*P2_43*SC2RAD; i+=16;
156 |         eph.cuc   =getbits(rtcm->buff,i,16)*P2_29;        i+=16;
157 |         eph.e     =getbitu(rtcm->buff,i,32)*P2_33;        i+=32;
158 |         eph.cus   =getbits(rtcm->buff,i,16);              i+=16;
159 |         sqrtA     =getbitu(rtcm->buff,i,32)*P2_19;        i+=32;
160 |         eph.toes  =getbitu(rtcm->buff,i,16);              i+=16;
161 |         eph.OMG0  =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
162 |         eph.cic   =getbits(rtcm->buff,i,16)*P2_29;        i+=16;
163 |         eph.i0    =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
164 |         eph.cis   =getbits(rtcm->buff,i,16)*P2_29;        i+=16;
165 |         eph.omg   =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
166 |         eph.crc   =getbits(rtcm->buff,i,16)*P2_5;         i+=16;
167 |         eph.OMGd  =getbits(rtcm->buff,i,24)*P2_43*SC2RAD; i+=24;
168 |         eph.M0    =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
169 |         eph.iodc  =getbitu(rtcm->buff,i,10);              i+=10;
170 |         eph.f0    =getbits(rtcm->buff,i,22)*P2_31;        i+=22;
171 |         prn       =getbitu(rtcm->buff,i, 5);              i+= 5+3;
172 |         eph.tgd[0]=getbits(rtcm->buff,i, 8)*P2_31;        i+= 8;
173 |         eph.code  =getbitu(rtcm->buff,i, 2);              i+= 2;
174 |         eph.sva   =getbitu(rtcm->buff,i, 4);              i+= 4;
175 |         eph.svh   =getbitu(rtcm->buff,i, 6);              i+= 6;
176 |         eph.flag  =getbitu(rtcm->buff,i, 1);
177 |     }
178 |     else {
179 |         trace(2,"rtcm2 17 length error: len=%d\n",rtcm->len);
180 |         return -1;
181 |     }
182 |     if (prn==0) prn=32;
183 |     sat=satno(SYS_GPS,prn);
184 |     eph.sat=sat;
185 |     eph.week=adjgpsweek(week);
186 |     eph.toe=gpst2time(eph.week,eph.toes);
187 |     eph.toc=gpst2time(eph.week,toc);
188 |     eph.ttr=rtcm->time;
189 |     eph.A=sqrtA*sqrtA;
190 |     rtcm->nav.eph[sat-1]=eph;
191 |     rtcm->ephset=0;
192 |     rtcm->ephsat=sat;
193 |     return 2;
194 | }
195 | /* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
196 | static int decode_type18(rtcm_t *rtcm)
197 | {
198 |     gtime_t time;
199 |     double usec,cp,tt;
200 |     int i=48,index,freq,sync=1,code,sys,prn,sat,loss;
201 |     
202 |     trace(4,"decode_type18: len=%d\n",rtcm->len);
203 |     
204 |     if (i+24<=rtcm->len*8) {
205 |         freq=getbitu(rtcm->buff,i, 2); i+= 2+2;
206 |         usec=getbitu(rtcm->buff,i,20); i+=20;
207 |     }
208 |     else {
209 |         trace(2,"rtcm2 18 length error: len=%d\n",rtcm->len);
210 |         return -1;
211 |     }
212 |     if (freq&0x1) {
213 |         trace(2,"rtcm2 18 not supported frequency: freq=%d\n",freq);
214 |         return -1;
215 |     }
216 |     freq>>=1;
217 |     
218 |     while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) {
219 |         sync=getbitu(rtcm->buff,i, 1); i+= 1;
220 |         code=getbitu(rtcm->buff,i, 1); i+= 1;
221 |         sys =getbitu(rtcm->buff,i, 1); i+= 1;
222 |         prn =getbitu(rtcm->buff,i, 5); i+= 5+3;
223 |         loss=getbitu(rtcm->buff,i, 5); i+= 5;
224 |         cp  =getbits(rtcm->buff,i,32); i+=32;
225 |         if (prn==0) prn=32;
226 |         if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) {
227 |             trace(2,"rtcm2 18 satellite number error: sys=%d prn=%d\n",sys,prn);
228 |             continue;
229 |         }
230 |         time=timeadd(rtcm->time,usec*1E-6);
231 |         if (sys) time=utc2gpst(time); /* convert glonass time to gpst */
232 |         
233 |         tt=timediff(rtcm->obs.data[0].time,time);
234 |         if (rtcm->obsflag||fabs(tt)>1E-9) {
235 |             rtcm->obs.n=rtcm->obsflag=0;
236 |         }
237 |         if ((index=obsindex(&rtcm->obs,time,sat))>=0) {
238 |             rtcm->obs.data[index].L[freq]=-cp/256.0;
239 |             rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][freq]!=loss;
240 |             rtcm->obs.data[index].code[freq]=
241 |                 !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
242 |             rtcm->loss[sat-1][freq]=loss;
243 |         }
244 |     }
245 |     rtcm->obsflag=!sync;
246 |     return sync?0:1;
247 | }
248 | /* decode type 19: rtk uncorrected pseudorange -------------------------------*/
249 | static int decode_type19(rtcm_t *rtcm)
250 | {
251 |     gtime_t time;
252 |     double usec,pr,tt;
253 |     int i=48,index,freq,sync=1,code,sys,prn,sat;
254 |     
255 |     trace(4,"decode_type19: len=%d\n",rtcm->len);
256 |     
257 |     if (i+24<=rtcm->len*8) {
258 |         freq=getbitu(rtcm->buff,i, 2); i+= 2+2;
259 |         usec=getbitu(rtcm->buff,i,20); i+=20;
260 |     }
261 |     else {
262 |         trace(2,"rtcm2 19 length error: len=%d\n",rtcm->len);
263 |         return -1;
264 |     }
265 |     if (freq&0x1) {
266 |         trace(2,"rtcm2 19 not supported frequency: freq=%d\n",freq);
267 |         return -1;
268 |     }
269 |     freq>>=1;
270 |     
271 |     while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) {
272 |         sync=getbitu(rtcm->buff,i, 1); i+= 1;
273 |         code=getbitu(rtcm->buff,i, 1); i+= 1;
274 |         sys =getbitu(rtcm->buff,i, 1); i+= 1;
275 |         prn =getbitu(rtcm->buff,i, 5); i+= 5+8;
276 |         pr  =getbitu(rtcm->buff,i,32); i+=32;
277 |         if (prn==0) prn=32;
278 |         if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) {
279 |             trace(2,"rtcm2 19 satellite number error: sys=%d prn=%d\n",sys,prn);
280 |             continue;
281 |         }
282 |         time=timeadd(rtcm->time,usec*1E-6);
283 |         if (sys) time=utc2gpst(time); /* convert glonass time to gpst */
284 |         
285 |         tt=timediff(rtcm->obs.data[0].time,time);
286 |         if (rtcm->obsflag||fabs(tt)>1E-9) {
287 |             rtcm->obs.n=rtcm->obsflag=0;
288 |         }
289 |         if ((index=obsindex(&rtcm->obs,time,sat))>=0) {
290 |             rtcm->obs.data[index].P[freq]=pr*0.02;
291 |             rtcm->obs.data[index].code[freq]=
292 |                 !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
293 |         }
294 |     }
295 |     rtcm->obsflag=!sync;
296 |     return sync?0:1;
297 | }
298 | /* decode type 22: extended reference station parameter ----------------------*/
299 | static int decode_type22(rtcm_t *rtcm)
300 | {
301 |     double del[2][3]={{0}},hgt=0.0;
302 |     int i=48,j,noh;
303 |     
304 |     trace(4,"decode_type22: len=%d\n",rtcm->len);
305 |     
306 |     if (i+24<=rtcm->len*8) {
307 |         del[0][0]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
308 |         del[0][1]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
309 |         del[0][2]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
310 |     }
311 |     else {
312 |         trace(2,"rtcm2 22 length error: len=%d\n",rtcm->len);
313 |         return -1;
314 |     }
315 |     if (i+24<=rtcm->len*8) {
316 |         i+=5; noh=getbits(rtcm->buff,i,1); i+=1;
317 |         hgt=noh?0.0:getbitu(rtcm->buff,i,18)/25600.0;
318 |         i+=18;
319 |     }
320 |     if (i+24<=rtcm->len*8) {
321 |         del[1][0]=getbits(rtcm->buff,i,8)/1600.0; i+=8;
322 |         del[1][1]=getbits(rtcm->buff,i,8)/1600.0; i+=8;
323 |         del[1][2]=getbits(rtcm->buff,i,8)/1600.0;
324 |     }
325 |     rtcm->sta.deltype=1; /* xyz */
326 |     for (j=0;j<3;j++) rtcm->sta.del[j]=del[0][j];
327 |     rtcm->sta.hgt=hgt;
328 |     return 5;
329 | }
330 | /* decode type 23: antenna type definition record ----------------------------*/
331 | static int decode_type23(rtcm_t *rtcm)
332 | {
333 |     return 0;
334 | }
335 | /* decode type 24: antenna reference point (arp) -----------------------------*/
336 | static int decode_type24(rtcm_t *rtcm)
337 | {
338 |     return 0;
339 | }
340 | /* decode type 31: differential glonass correction ---------------------------*/
341 | static int decode_type31(rtcm_t *rtcm)
342 | {
343 |     return 0;
344 | }
345 | /* decode type 32: differential glonass reference station parameters ---------*/
346 | static int decode_type32(rtcm_t *rtcm)
347 | {
348 |     return 0;
349 | }
350 | /* decode type 34: glonass partial differential correction set ---------------*/
351 | static int decode_type34(rtcm_t *rtcm)
352 | {
353 |     return 0;
354 | }
355 | /* decode type 36: glonass special message -----------------------------------*/
356 | static int decode_type36(rtcm_t *rtcm)
357 | {
358 |     return 0;
359 | }
360 | /* decode type 37: gnss system time offset -----------------------------------*/
361 | static int decode_type37(rtcm_t *rtcm)
362 | {
363 |     return 0;
364 | }
365 | /* decode type 59: proprietary message ---------------------------------------*/
366 | static int decode_type59(rtcm_t *rtcm)
367 | {
368 |     return 0;
369 | }
370 | /* decode rtcm ver.2 message -------------------------------------------------*/
371 | extern int decode_rtcm2(rtcm_t *rtcm)
372 | {
373 |     double zcnt;
374 |     int staid,seqno,stah,ret=0,type=getbitu(rtcm->buff,8,6);
375 |     
376 |     trace(3,"decode_rtcm2: type=%2d len=%3d\n",type,rtcm->len);
377 |     
378 |     if ((zcnt=getbitu(rtcm->buff,24,13)*0.6)>=3600.0) {
379 |         trace(2,"rtcm2 modified z-count error: zcnt=%.1f\n",zcnt);
380 |         return -1;
381 |     }
382 |     adjhour(rtcm,zcnt);
383 |     staid=getbitu(rtcm->buff,14,10);
384 |     seqno=getbitu(rtcm->buff,37, 3);
385 |     stah =getbitu(rtcm->buff,45, 3);
386 |     if (seqno-rtcm->seqno!=1&&seqno-rtcm->seqno!=-7) {
387 |         trace(2,"rtcm2 message outage: seqno=%d->%d\n",rtcm->seqno,seqno);
388 |     }
389 |     rtcm->seqno=seqno;
390 |     rtcm->stah =stah;
391 |     
392 |     if (rtcm->outtype) {
393 |         sprintf(rtcm->msgtype,"RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
394 |                 type,rtcm->len,zcnt,staid,seqno);
395 |     }
396 |     if (type==3||type==22||type==23||type==24) {
397 |         if (rtcm->staid!=0&&staid!=rtcm->staid) {
398 |            trace(2,"rtcm2 station id changed: %d->%d\n",rtcm->staid,staid);
399 |         }
400 |         rtcm->staid=staid;
401 |     }
402 |     if (rtcm->staid!=0&&staid!=rtcm->staid) {
403 |         trace(2,"rtcm2 station id invalid: %d %d\n",staid,rtcm->staid);
404 |         return -1;
405 |     }
406 |     switch (type) {
407 |         case  1: ret=decode_type1 (rtcm); break;
408 |         case  3: ret=decode_type3 (rtcm); break;
409 |         case  9: ret=decode_type1 (rtcm); break;
410 |         case 14: ret=decode_type14(rtcm); break;
411 |         case 16: ret=decode_type16(rtcm); break;
412 |         case 17: ret=decode_type17(rtcm); break;
413 |         case 18: ret=decode_type18(rtcm); break;
414 |         case 19: ret=decode_type19(rtcm); break;
415 |         case 22: ret=decode_type22(rtcm); break;
416 |         case 23: ret=decode_type23(rtcm); break; /* not supported */
417 |         case 24: ret=decode_type24(rtcm); break; /* not supported */
418 |         case 31: ret=decode_type31(rtcm); break; /* not supported */
419 |         case 32: ret=decode_type32(rtcm); break; /* not supported */
420 |         case 34: ret=decode_type34(rtcm); break; /* not supported */
421 |         case 36: ret=decode_type36(rtcm); break; /* not supported */
422 |         case 37: ret=decode_type37(rtcm); break; /* not supported */
423 |         case 59: ret=decode_type59(rtcm); break; /* not supported */
424 |     }
425 |     if (ret>=0) {
426 |         if (1<=type&&type<=99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++;
427 |     }
428 |     return ret;
429 | }
430 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/tides.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * tides.c : tidal displacement corrections
  3 | *
  4 | *          Copyright (C) 2015-2017 by T.TAKASU, All rights reserved.
  5 | *
  6 | * options : -DIERS_MODEL use IERS tide model
  7 | *
  8 | * references :
  9 | *     [1] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
 10 | *     [2] D.D.McCarthy and G.Petit, IERS Technical Note 32, IERS Conventions
 11 | *         2003, November 2003
 12 | *     [3] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
 13 | *         Space Technology Library, 2004
 14 | *     [4] J.Kouba, A Guide to using International GNSS Service (IGS) products,
 15 | *         May 2009
 16 | *     [5] G.Petit and B.Luzum (eds), IERS Technical Note No. 36, IERS
 17 | *         Conventions (2010), 2010
 18 | *
 19 | * version : $Revision:$ $Date:$
 20 | * history : 2015/05/10 1.0  separated from ppp.c
 21 | *           2015/06/11 1.1  fix bug on computing days in tide_oload() (#128)
 22 | *           2017/04/11 1.2  fix bug on calling geterp() in timdedisp()
 23 | *-----------------------------------------------------------------------------*/
 24 | #include "rtklib.h"
 25 | 
 26 | #define SQR(x)      ((x)*(x))
 27 | 
 28 | #define AS2R        (D2R/3600.0)    /* arc sec to radian */
 29 | #define GME         3.986004415E+14 /* earth gravitational constant */
 30 | #define GMS         1.327124E+20    /* sun gravitational constant */
 31 | #define GMM         4.902801E+12    /* moon gravitational constant */
 32 | 
 33 | /* function prototypes -------------------------------------------------------*/
 34 | #ifdef IERS_MODEL
 35 | extern int dehanttideinel_(double *xsta, int *year, int *mon, int *day,
 36 |                            double *fhr, double *xsun, double *xmon,
 37 |                            double *dxtide);
 38 | #endif
 39 | 
 40 | /* solar/lunar tides (ref [2] 7) ---------------------------------------------*/
 41 | #ifndef IERS_MODEL
 42 | static void tide_pl(const double *eu, const double *rp, double GMp,
 43 |                     const double *pos, double *dr)
 44 | {
 45 |     const double H3=0.292,L3=0.015;
 46 |     double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl;
 47 |     int i;
 48 |     
 49 |     trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
 50 |     
 51 |     if ((r=norm(rp,3))<=0.0) return;
 52 |     
 53 |     for (i=0;i<3;i++) ep[i]=rp[i]/r;
 54 |     
 55 |     K2=GMp/GME*SQR(RE_WGS84)*SQR(RE_WGS84)/(r*r*r);
 56 |     K3=K2*RE_WGS84/r;
 57 |     latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]);
 58 |     cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]);
 59 |     
 60 |     /* step1 in phase (degree 2) */
 61 |     p=(3.0*sinl*sinl-1.0)/2.0;
 62 |     H2=0.6078-0.0006*p;
 63 |     L2=0.0847+0.0002*p;
 64 |     a=dot(ep,eu,3);
 65 |     dp=K2*3.0*L2*a;
 66 |     du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a);
 67 |     
 68 |     /* step1 in phase (degree 3) */
 69 |     dp+=K3*L3*(7.5*a*a-1.5);
 70 |     du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a);
 71 |     
 72 |     /* step1 out-of-phase (only radial) */
 73 |     du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp);
 74 |     du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp));
 75 |     
 76 |     dr[0]=dp*ep[0]+du*eu[0];
 77 |     dr[1]=dp*ep[1]+du*eu[1];
 78 |     dr[2]=dp*ep[2]+du*eu[2];
 79 |     
 80 |     trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
 81 | }
 82 | /* displacement by solid earth tide (ref [2] 7) ------------------------------*/
 83 | static void tide_solid(const double *rsun, const double *rmoon,
 84 |                        const double *pos, const double *E, double gmst, int opt,
 85 |                        double *dr)
 86 | {
 87 |     double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l;
 88 |     
 89 |     trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt);
 90 |     
 91 |     /* step1: time domain */
 92 |     eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8];
 93 |     tide_pl(eu,rsun, GMS,pos,dr1);
 94 |     tide_pl(eu,rmoon,GMM,pos,dr2);
 95 |     
 96 |     /* step2: frequency domain, only K1 radial */
 97 |     sin2l=sin(2.0*pos[0]);
 98 |     du=-0.012*sin2l*sin(gmst+pos[1]);
 99 |     
100 |     dr[0]=dr1[0]+dr2[0]+du*E[2];
101 |     dr[1]=dr1[1]+dr2[1]+du*E[5];
102 |     dr[2]=dr1[2]+dr2[2]+du*E[8];
103 |     
104 |     /* eliminate permanent deformation */
105 |     if (opt&8) {
106 |         sinl=sin(pos[0]); 
107 |         du=0.1196*(1.5*sinl*sinl-0.5);
108 |         dn=0.0247*sin2l;
109 |         dr[0]+=du*E[2]+dn*E[1];
110 |         dr[1]+=du*E[5]+dn*E[4];
111 |         dr[2]+=du*E[8]+dn*E[7];
112 |     }
113 |     trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
114 | }
115 | #endif /* !IERS_MODEL */
116 | 
117 | /* displacement by ocean tide loading (ref [2] 7) ----------------------------*/
118 | static void tide_oload(gtime_t tut, const double *odisp, double *denu)
119 | {
120 |     const double args[][5]={
121 |         {1.40519E-4, 2.0,-2.0, 0.0, 0.00},  /* M2 */
122 |         {1.45444E-4, 0.0, 0.0, 0.0, 0.00},  /* S2 */
123 |         {1.37880E-4, 2.0,-3.0, 1.0, 0.00},  /* N2 */
124 |         {1.45842E-4, 2.0, 0.0, 0.0, 0.00},  /* K2 */
125 |         {0.72921E-4, 1.0, 0.0, 0.0, 0.25},  /* K1 */
126 |         {0.67598E-4, 1.0,-2.0, 0.0,-0.25},  /* O1 */
127 |         {0.72523E-4,-1.0, 0.0, 0.0,-0.25},  /* P1 */
128 |         {0.64959E-4, 1.0,-3.0, 1.0,-0.25},  /* Q1 */
129 |         {0.53234E-5, 0.0, 2.0, 0.0, 0.00},  /* Mf */
130 |         {0.26392E-5, 0.0, 1.0,-1.0, 0.00},  /* Mm */
131 |         {0.03982E-5, 2.0, 0.0, 0.0, 0.00}   /* Ssa */
132 |     };
133 |     const double ep1975[]={1975,1,1,0,0,0};
134 |     double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0};
135 |     int i,j;
136 |     
137 |     trace(3,"tide_oload:\n");
138 |     
139 |     /* angular argument: see subroutine arg.f for reference [1] */
140 |     time2epoch(tut,ep);
141 |     fday=ep[3]*3600.0+ep[4]*60.0+ep[5];
142 |     ep[3]=ep[4]=ep[5]=0.0;
143 |     days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
144 |     t=(27392.500528+1.000000035*days)/36525.0;
145 |     t2=t*t; t3=t2*t;
146 |     
147 |     a[0]=fday;
148 |     a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */
149 |     a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */
150 |     a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */
151 |     a[4]=2.0*PI;
152 |     
153 |     /* displacements by 11 constituents */
154 |     for (i=0;i<11;i++) {
155 |         ang=0.0;
156 |         for (j=0;j<5;j++) ang+=a[j]*args[i][j];
157 |         for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R);
158 |     }
159 |     denu[0]=-dp[1];
160 |     denu[1]=-dp[2];
161 |     denu[2]= dp[0];
162 |     
163 |     trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
164 | }
165 | /* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/
166 | static void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar)
167 | {
168 |     const double ep2000[]={2000,1,1,0,0,0};
169 |     double y,y2,y3;
170 |     
171 |     y=timediff(tut,epoch2time(ep2000))/86400.0/365.25;
172 |     
173 |     if (y<3653.0/365.25) { /* until 2010.0 */
174 |         y2=y*y; y3=y2*y;
175 |         *xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */
176 |         *yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3;
177 |     }
178 |     else { /* after 2010.0 */
179 |         *xp_bar= 23.513+7.6141*y; /* (mas) */
180 |         *yp_bar=358.891-0.6287*y;
181 |     }
182 | }
183 | /* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/
184 | static void tide_pole(gtime_t tut, const double *pos, const double *erpv,
185 |                       double *denu)
186 | {
187 |     double xp_bar,yp_bar,m1,m2,cosl,sinl;
188 |     
189 |     trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
190 |     
191 |     /* iers mean pole (mas) */
192 |     iers_mean_pole(tut,&xp_bar,&yp_bar);
193 |     
194 |     /* ref [7] eq.7.24 */
195 |     m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */
196 |     m2=-erpv[1]/AS2R+yp_bar*1E-3;
197 |     
198 |     /* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */
199 |     cosl=cos(pos[1]);
200 |     sinl=sin(pos[1]);
201 |     denu[0]=  9E-3*sin(pos[0])    *(m1*sinl-m2*cosl); /* de= Slambda (m) */
202 |     denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta  (m) */
203 |     denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr      (m) */
204 |     
205 |     trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
206 | }
207 | /* tidal displacement ----------------------------------------------------------
208 | * displacements by earth tides
209 | * args   : gtime_t tutc     I   time in utc
210 | *          double *rr       I   site position (ecef) (m)
211 | *          int    opt       I   options (or of the followings)
212 | *                                 1: solid earth tide
213 | *                                 2: ocean tide loading
214 | *                                 4: pole tide
215 | *                                 8: elimate permanent deformation
216 | *          double *erp      I   earth rotation parameters (NULL: not used)
217 | *          double *odisp    I   ocean loading parameters  (NULL: not used)
218 | *                                 odisp[0+i*6]: consituent i amplitude radial(m)
219 | *                                 odisp[1+i*6]: consituent i amplitude west  (m)
220 | *                                 odisp[2+i*6]: consituent i amplitude south (m)
221 | *                                 odisp[3+i*6]: consituent i phase radial  (deg)
222 | *                                 odisp[4+i*6]: consituent i phase west    (deg)
223 | *                                 odisp[5+i*6]: consituent i phase south   (deg)
224 | *                                (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,
225 | *                                   8:Mf,9:Mm,10:Ssa)
226 | *          double *dr       O   displacement by earth tides (ecef) (m)
227 | * return : none
228 | * notes  : see ref [1], [2] chap 7
229 | *          see ref [4] 5.2.1, 5.2.2, 5.2.3
230 | *          ver.2.4.0 does not use ocean loading and pole tide corrections
231 | *-----------------------------------------------------------------------------*/
232 | extern void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
233 |                      const double *odisp, double *dr)
234 | {
235 |     gtime_t tut;
236 |     double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0};
237 |     int i;
238 | #ifdef IERS_MODEL
239 |     double ep[6],fhr;
240 |     int year,mon,day;
241 | #endif
242 |     
243 |     trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0));
244 |     
245 |     if (erp) {
246 |         geterp(erp,utc2gpst(tutc),erpv);
247 |     }
248 |     tut=timeadd(tutc,erpv[2]);
249 |     
250 |     dr[0]=dr[1]=dr[2]=0.0;
251 |     
252 |     if (norm(rr,3)<=0.0) return;
253 |     
254 |     pos[0]=asin(rr[2]/norm(rr,3));
255 |     pos[1]=atan2(rr[1],rr[0]);
256 |     xyz2enu(pos,E);
257 |     
258 |     if (opt&1) { /* solid earth tides */
259 |         
260 |         /* sun and moon position in ecef */
261 |         sunmoonpos(tutc,erpv,rs,rm,&gmst);
262 |         
263 | #ifdef IERS_MODEL
264 |         time2epoch(tutc,ep);
265 |         year=(int)ep[0];
266 |         mon =(int)ep[1];
267 |         day =(int)ep[2];
268 |         fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0;
269 |         
270 |         /* call DEHANTTIDEINEL */
271 |         dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt);
272 | #else
273 |         tide_solid(rs,rm,pos,E,gmst,opt,drt);
274 | #endif
275 |         for (i=0;i<3;i++) dr[i]+=drt[i];
276 |     }
277 |     if ((opt&2)&&odisp) { /* ocean tide loading */
278 |         tide_oload(tut,odisp,denu);
279 |         matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
280 |         for (i=0;i<3;i++) dr[i]+=drt[i];
281 |     }
282 |     if ((opt&4)&&erp) { /* pole tide */
283 |         tide_pole(tut,pos,erpv,denu);
284 |         matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
285 |         for (i=0;i<3;i++) dr[i]+=drt[i];
286 |     }
287 |     trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
288 | }
289 | 


--------------------------------------------------------------------------------
/GNSS_Algorithm/src/tle.c:
--------------------------------------------------------------------------------
  1 | /*------------------------------------------------------------------------------
  2 | * tle.c: NORAD TLE (two line element) functions
  3 | *
  4 | *          Copyright (C) 2012-2020 by T.TAKASU, All rights reserved.
  5 | *
  6 | * references:
  7 | *     [1] F.R.Hoots and R.L.Roehrich, Spacetrack report No.3, Models for
  8 | *         propagation of NORAD element sets, December 1980
  9 | *     [2] D.A.Vallado, P.Crawford, R.Hujsak and T.S.Kelso, Revisiting
 10 | *         Spacetrack Report #3, AIAA 2006-6753, 2006
 11 | *     [3] CelesTrak (http://www.celestrak.com)
 12 | *
 13 | * version : $Revision:$ $Date:$
 14 | * history : 2012/11/01 1.0  new
 15 | *           2013/01/25 1.1  fix bug on binary search
 16 | *           2014/08/26 1.2  fix bug on tle_pos() to get tle by satid or desig
 17 | *           2020/11/30 1.3  fix problem on duplicated names in a satellite
 18 | *-----------------------------------------------------------------------------*/
 19 | #include "rtklib.h"
 20 | 
 21 | /* SGP4 model propagator by STR#3 (ref [1] sec.6,11) -------------------------*/
 22 | 
 23 | #define DE2RA       0.174532925E-1
 24 | #define E6A         1.E-6
 25 | #define PIO2        1.57079633
 26 | #define QO          120.0
 27 | #define SO          78.0
 28 | #define TOTHRD      0.66666667
 29 | #define TWOPI       6.2831853
 30 | #define X3PIO2      4.71238898
 31 | #define XJ2         1.082616E-3
 32 | #define XJ3         -0.253881E-5
 33 | #define XJ4         -1.65597E-6
 34 | #define XKE         0.743669161E-1
 35 | #define XKMPER      6378.135
 36 | #define XMNPDA      1440.0
 37 | #define AE          1.0
 38 | #define CK2         5.413080E-4         /* = 0.5*XJ2*AE*AE */
 39 | #define CK4         0.62098875E-6       /* = -0.375*XJ4*AE*AE*AE*AE */
 40 | #define QOMS2T      1.88027916E-9       /* = pow((QO-SO)*AE/XKMPER,4.0) */
 41 | #define S           1.01222928          /* = AE*(1.0+SO/XKMPER) */
 42 | 
 43 | static void SGP4_STR3(double tsince, const tled_t *data, double *rs)
 44 | {
 45 |     double xnodeo,omegao,xmo,eo,xincl,xno,xndt2o,xndd6o,bstar;
 46 |     double a1,cosio,theta2,x3thm1,eosq,betao2,betao,del1,ao,delo,xnodp,aodp,s4;
 47 |     double qoms24,perige,pinvsq,tsi,eta,etasq,eeta,psisq,coef,coef1,c1,c2,c3,c4;
 48 |     double c5,sinio,a3ovk2,x1mth2,theta4,xmdot,x1m5th,omgdot,xhdot1,xnodot;
 49 |     double omgcof,xmcof,xnodcf,t2cof,xlcof,aycof,delmo,sinmo,x7thm1,c1sq,d2,d3;
 50 |     double d4,t3cof,t4cof,t5cof,xmdf,omgadf,xnoddf,omega,xmp,tsq,xnode,delomg;
 51 |     double delm,tcube,tfour,a,e,xl,beta,xn,axn,xll,aynl,xlt,ayn,capu,sinepw;
 52 |     double cosepw,epw,ecose,esine,elsq,pl,r,rdot,rfdot,betal,cosu,sinu,u,sin2u;
 53 |     double cos2u,rk,uk,xnodek,xinck,rdotk,rfdotk,sinuk,cosuk,sinik,cosik,sinnok;
 54 |     double cosnok,xmx,xmy,ux,uy,uz,vx,vy,vz,x,y,z,xdot,ydot,zdot;
 55 |     double temp,temp1,temp2,temp3,temp4,temp5,temp6,tempa,tempe,templ;
 56 |     int i,isimp;
 57 |     
 58 |     xnodeo=data->OMG*DE2RA;
 59 |     omegao=data->omg*DE2RA;
 60 |     xmo=data->M*DE2RA;
 61 |     xincl=data->inc*DE2RA;
 62 |     temp=TWOPI/XMNPDA/XMNPDA;
 63 |     xno=data->n*temp*XMNPDA;
 64 |     xndt2o=data->ndot*temp;
 65 |     xndd6o=data->nddot*temp/XMNPDA;
 66 |     bstar=data->bstar/AE;
 67 |     eo=data->ecc;
 68 |     /*
 69 |     * recover original mean motion (xnodp) and semimajor axis (aodp)
 70 |     * from input elements
 71 |     */
 72 |     a1=pow(XKE/xno,TOTHRD);
 73 |     cosio=cos(xincl);
 74 |     theta2=cosio*cosio;
 75 |     x3thm1=3.0*theta2-1.0;
 76 |     eosq=eo*eo;
 77 |     betao2=1.0-eosq;
 78 |     betao=sqrt(betao2);
 79 |     del1=1.5*CK2*x3thm1/(a1*a1*betao*betao2);
 80 |     ao=a1*(1.0-del1*(0.5*TOTHRD+del1*(1.0+134.0/81.0*del1)));
 81 |     delo=1.5*CK2*x3thm1/(ao*ao*betao*betao2);
 82 |     xnodp=xno/(1.0+delo);
 83 |     aodp=ao/(1.0-delo);
 84 |     /*
 85 |     * initialization
 86 |     * for perigee less than 220 kilometers, the isimp flag is set and
 87 |     * the equations are truncated to linear variation in sqrt a and
 88 |     * quadratic variation in mean anomaly. also, the c3 term, the
 89 |     * delta omega term, and the delta m term are dropped.
 90 |     */
 91 |     isimp=0;
 92 |     if ((aodp*(1.0-eo)/AE)<(220.0/XKMPER+AE)) isimp=1;
 93 |     
 94 |     /* for perigee below 156 km, the values of s and qoms2t are altered */
 95 |     s4=S;
 96 |     qoms24=QOMS2T;
 97 |     perige=(aodp*(1.0-eo)-AE)*XKMPER;
 98 |     if (perige<156.0) {
 99 |         s4=perige-78.0;
100 |         if (perige<=98.0) s4=20.0;
101 |         qoms24=pow((120.0-s4)*AE/XKMPER,4.0);
102 |         s4=s4/XKMPER+AE;
103 |     }
104 |     pinvsq=1.0/(aodp*aodp*betao2*betao2);
105 |     tsi=1.0/(aodp-s4);
106 |     eta=aodp*eo*tsi;
107 |     etasq=eta*eta;
108 |     eeta=eo*eta;
109 |     psisq=fabs(1.0-etasq);
110 |     coef=qoms24*pow(tsi,4.0);
111 |     coef1=coef/pow(psisq,3.5);
112 |     c2=coef1*xnodp*(aodp*(1.0+1.5*etasq+eeta*(4.0+etasq))+0.75*
113 |             CK2*tsi/psisq*x3thm1*(8.0+3.0*etasq*(8.0+etasq)));
114 |     c1=bstar*c2;
115 |     sinio=sin(xincl);
116 |     a3ovk2=-XJ3/CK2*pow(AE,3.0);
117 |     c3=coef*tsi*a3ovk2*xnodp*AE*sinio/eo;
118 |     x1mth2=1.0-theta2;
119 |     c4=2.0*xnodp*coef1*aodp*betao2*(eta*
120 |             (2.0+0.5*etasq)+eo*(0.5+2.0*etasq)-2.0*CK2*tsi/
121 |             (aodp*psisq)*(-3.0*x3thm1*(1.0-2.0*eeta+etasq*
122 |             (1.5-0.5*eeta))+0.75*x1mth2*(2.0*etasq-eeta*
123 |             (1.0+etasq))*cos(2.0*omegao)));
124 |     c5=2.0*coef1*aodp*betao2*(1.0+2.75*(etasq+eeta)+eeta*etasq);
125 |     theta4=theta2*theta2;
126 |     temp1=3.0*CK2*pinvsq*xnodp;
127 |     temp2=temp1*CK2*pinvsq;
128 |     temp3=1.25*CK4*pinvsq*pinvsq*xnodp;
129 |     xmdot=xnodp+0.5*temp1*betao*x3thm1+0.0625*temp2*betao*
130 |             (13.0-78.0*theta2+137.0*theta4);
131 |     x1m5th=1.0-5.0*theta2;
132 |     omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7.0-114.0*theta2+
133 |             395.0*theta4)+temp3*(3.0-36.0*theta2+49.0*theta4);
134 |     xhdot1=-temp1*cosio;
135 |     xnodot=xhdot1+(0.5*temp2*(4.0-19.0*theta2)+2.0*temp3*(3.0-
136 |             7.0*theta2))*cosio;
137 |     omgcof=bstar*c3*cos(omegao);
138 |     xmcof=-TOTHRD*coef*bstar*AE/eeta;
139 |     xnodcf=3.5*betao2*xhdot1*c1;
140 |     t2cof=1.5*c1;
141 |     xlcof=0.125*a3ovk2*sinio*(3.0+5.0*cosio)/(1.0+cosio);
142 |     aycof=0.25*a3ovk2*sinio;
143 |     delmo=pow(1.0+eta*cos(xmo),3.0);
144 |     sinmo=sin(xmo);
145 |     x7thm1=7.0*theta2-1.0;
146 |     
147 |     if (isimp!=1) {
148 |         c1sq=c1*c1;
149 |         d2=4.0*aodp*tsi*c1sq;
150 |         temp=d2*tsi*c1/3.0;
151 |         d3=(17.0*aodp+s4)*temp;
152 |         d4=0.5*temp*aodp*tsi*(221.0*aodp+31.0*s4)*c1;
153 |         t3cof=d2+2.0*c1sq;
154 |         t4cof=0.25*(3.0*d3+c1*(12.0*d2+10.0*c1sq));
155 |         t5cof=0.2*(3.0*d4+12.0*c1*d3+6.0*d2*d2+15.0*c1sq*(2.0*d2+c1sq));
156 |     }
157 |     else {
158 |         d2=d3=d4=t3cof=t4cof=t5cof=0.0;
159 |     }
160 |     /* update for secular gravity and atmospheric drag */
161 |     xmdf=xmo+xmdot*tsince;
162 |     omgadf=omegao+omgdot*tsince;
163 |     xnoddf=xnodeo+xnodot*tsince;
164 |     omega=omgadf;
165 |     xmp=xmdf;
166 |     tsq=tsince*tsince;
167 |     xnode=xnoddf+xnodcf*tsq;
168 |     tempa=1.0-c1*tsince;
169 |     tempe=bstar*c4*tsince;
170 |     templ=t2cof*tsq;
171 |     if (isimp==1) {
172 |         delomg=omgcof*tsince;
173 |         delm=xmcof*(pow(1.0+eta*cos(xmdf),3.0)-delmo);
174 |         temp=delomg+delm;
175 |         xmp=xmdf+temp;
176 |         omega=omgadf-temp;
177 |         tcube=tsq*tsince;
178 |         tfour=tsince*tcube;
179 |         tempa=tempa-d2*tsq-d3*tcube-d4*tfour;
180 |         tempe=tempe+bstar*c5*(sin(xmp)-sinmo);
181 |         templ=templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof);
182 |     }
183 |     a=aodp*pow(tempa,2.0);
184 |     e=eo-tempe;
185 |     xl=xmp+omega+xnode+xnodp*templ;
186 |     beta=sqrt(1.0-e*e);
187 |     xn=XKE/pow(a,1.5);
188 |     
189 |     /* long period periodics */
190 |     axn=e*cos(omega);
191 |     temp=1.0/(a*beta*beta);
192 |     xll=temp*xlcof*axn;
193 |     aynl=temp*aycof;
194 |     xlt=xl+xll;
195 |     ayn=e*sin(omega)+aynl;
196 |     
197 |     /* solve keplers equation */
198 |     capu=fmod(xlt-xnode,TWOPI);
199 |     temp2=capu;
200 |     for (i=0;i<10;i++) {
201 |         sinepw=sin(temp2);
202 |         cosepw=cos(temp2);
203 |         temp3=axn*sinepw;
204 |         temp4=ayn*cosepw;
205 |         temp5=axn*cosepw;
206 |         temp6=ayn*sinepw;
207 |         epw=(capu-temp4+temp3-temp2)/(1.0-temp5-temp6)+temp2;
208 |         if (fabs(epw-temp2)<=E6A) break;
209 |         temp2=epw;
210 |     }
211 |     /* short period preliminary quantities */
212 |     ecose=temp5+temp6;
213 |     esine=temp3-temp4;
214 |     elsq=axn*axn+ayn*ayn;
215 |     temp=1.0-elsq;
216 |     pl=a*temp;
217 |     r=a*(1.0-ecose);
218 |     temp1=1.0/r;
219 |     rdot=XKE*sqrt(a)*esine*temp1;
220 |     rfdot=XKE*sqrt(pl)*temp1;
221 |     temp2=a*temp1;
222 |     betal=sqrt(temp);
223 |     temp3=1.0/(1.0+betal);
224 |     cosu=temp2*(cosepw-axn+ayn*esine*temp3);
225 |     sinu=temp2*(sinepw-ayn-axn*esine*temp3);
226 |     u=atan2(sinu,cosu);
227 |     sin2u=2.0*sinu*cosu;
228 |     cos2u=2.0*cosu*cosu-1.0;
229 |     temp=1.0/pl;
230 |     temp1=CK2*temp;
231 |     temp2=temp1*temp;
232 |     
233 |     /* update for short periodics */
234 |     rk=r*(1.0-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
235 |     uk=u-0.25*temp2*x7thm1*sin2u;
236 |     xnodek=xnode+1.5*temp2*cosio*sin2u;
237 |     xinck=xincl+1.5*temp2*cosio*sinio*cos2u;
238 |     rdotk=rdot-xn*temp1*x1mth2*sin2u;
239 |     rfdotk=rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
240 |     
241 |     /* orientation vectors */
242 |     sinuk=sin(uk);
243 |     cosuk=cos(uk);
244 |     sinik=sin(xinck);
245 |     cosik=cos(xinck);
246 |     sinnok=sin(xnodek);
247 |     cosnok=cos(xnodek);
248 |     xmx=-sinnok*cosik;
249 |     xmy=cosnok*cosik;
250 |     ux=xmx*sinuk+cosnok*cosuk;
251 |     uy=xmy*sinuk+sinnok*cosuk;
252 |     uz=sinik*sinuk;
253 |     vx=xmx*cosuk-cosnok*sinuk;
254 |     vy=xmy*cosuk-sinnok*sinuk;
255 |     vz=sinik*cosuk;
256 |     
257 |     /* position and velocity */
258 |     x=rk*ux;
259 |     y=rk*uy;
260 |     z=rk*uz;
261 |     xdot=rdotk*ux+rfdotk*vx;
262 |     ydot=rdotk*uy+rfdotk*vy;
263 |     zdot=rdotk*uz+rfdotk*vz;
264 |     
265 |     rs[0]=x*XKMPER/AE*1E3; /* (m) */
266 |     rs[1]=y*XKMPER/AE*1E3;
267 |     rs[2]=z*XKMPER/AE*1E3;
268 |     rs[3]=xdot*XKMPER/AE*XMNPDA/86400.0*1E3; /* (m/s) */
269 |     rs[4]=ydot*XKMPER/AE*XMNPDA/86400.0*1E3;
270 |     rs[5]=zdot*XKMPER/AE*XMNPDA/86400.0*1E3;
271 | }
272 | /* drop spaces at string tail ------------------------------------------------*/
273 | static void chop(char *buff)
274 | {
275 |     int i;
276 |     for (i=strlen(buff)-1;i>=0;i--) {
277 |         if (buff[i]==' '||buff[i]=='\r'||buff[i]=='\n') buff[i]='\0';
278 |         else break;
279 |     }
280 | }
281 | /* test TLE line checksum ----------------------------------------------------*/
282 | static int checksum(const char *buff)
283 | {
284 |     int i,cs=0;
285 |     
286 |     if (strlen(buff)<69) return 0;
287 |     
288 |     for (i=0;i<68;i++) {
289 |         if ('0'<=buff[i]&&buff[i]<='9') cs+=(int)(buff[i]-'0');
290 |         else if (buff[i]=='-') cs+=1;
291 |     }
292 |     return (int)(buff[68]-'0')==cs%10;
293 | }
294 | /* decode TLE line 1 ---------------------------------------------------------*/
295 | static int decode_line1(const char *buff, tled_t *data)
296 | {
297 |     double year,doy,nddot,exp1,bstar,exp2,ep[6]={2000,1,1};
298 |     
299 |     strncpy(data->satno,buff+2,5);       /* satellite number */
300 |     data->satno[5]='\0';
301 |     chop(data->satno);
302 |     
303 |     data->satclass=buff[7];              /* satellite classification */
304 |     strncpy(data->desig,buff+9,8);       /* international designator */
305 |     data->desig[8]='\0';
306 |     chop(data->desig);
307 |     
308 |     year      =str2num(buff,18, 2);      /* epoch year */
309 |     doy       =str2num(buff,20,12);      /* epoch day of year */
310 |     data->ndot=str2num(buff,33,10);      /* 1st time derivative of n */
311 |     nddot     =str2num(buff,44, 6);      /* 2nd time derivative of n */
312 |     exp1      =str2num(buff,50, 2);
313 |     bstar     =str2num(buff,53, 6);      /* Bstar drag term */
314 |     exp2      =str2num(buff,59, 2);
315 |     data->etype=(int)str2num(buff,62,1); /* ephemeris type */
316 |     data->eleno=(int)str2num(buff,64,4); /* ephemeris number */
317 |     data->nddot=nddot*1E-5*pow(10.0,exp1);
318 |     data->bstar=bstar*1E-5*pow(10.0,exp2);
319 |     
320 |     ep[0]=year+(year<57.0?2000.0:1900.0);
321 |     data->epoch=timeadd(epoch2time(ep),(doy-1.0)*86400.0);
322 |     
323 |     data->inc=data->OMG=data->ecc=data->omg=data->M=data->n=0.0;
324 |     data->rev=0;
325 |     return 1;
326 | }
327 | /* decode TLE line 2 ---------------------------------------------------------*/
328 | static int decode_line2(const char *buff, tled_t *data)
329 | {
330 |     char satno[16];
331 |     
332 |     strncpy(satno,buff+2,5);             /* satellite number */
333 |     satno[5]='\0';
334 |     chop(satno);
335 |     
336 |     data->inc=str2num(buff, 8, 8);       /* inclination (deg) */
337 |     data->OMG=str2num(buff,17, 8);       /* RAAN (deg) */
338 |     data->ecc=str2num(buff,26, 7)*1E-7;  /* eccentricity */
339 |     data->omg=str2num(buff,34, 8);       /* argument of perigee (deg) */
340 |     data->M  =str2num(buff,43, 8);       /* mean anomaly (deg) */
341 |     data->n  =str2num(buff,52,11);       /* mean motion (rev/day) */
342 |     data->rev=(int)str2num(buff,63,5);   /* revolution number */
343 |     
344 |     if (strcmp(satno,data->satno)) {
345 |         trace(2,"tle satno mismatch: %s %s\n",data->satno,satno);
346 |         return 0;
347 |     }
348 |     if (data->n<=0.0||data->ecc<0.0) {
349 |         trace(2,"tle data error: %s\n",satno);
350 |         return 0;
351 |     }
352 |     return 1;
353 | }
354 | /* add TLE data --------------------------------------------------------------*/
355 | static int add_data(tle_t *tle, const tled_t *data)
356 | {
357 |     tled_t *tle_data;
358 |     
359 |     if (tle->n>=tle->nmax) {
360 |         tle->nmax=tle->nmax<=0?1024:tle->nmax*2;
361 |         
362 |         if (!(tle_data=(tled_t *)realloc(tle->data,sizeof(tled_t)*tle->nmax))) {
363 |             trace(1,"tle malloc error\n");
364 |             free(tle->data); tle->data=NULL; tle->n=tle->nmax=0;
365 |             return 0;
366 |         }
367 |         tle->data=tle_data;
368 |     }
369 |     tle->data[tle->n++]=*data;
370 |     return 1;
371 | }
372 | /* compare TLE data by satellite name ----------------------------------------*/
373 | static int cmp_tle_data(const void *p1, const void *p2)
374 | {
375 |     const tled_t *q1=(const tled_t *)p1,*q2=(const tled_t *)p2;
376 |     return strcmp(q1->name,q2->name);
377 | }
378 | /* read TLE file ---------------------------------------------------------------
379 | * read NORAD TLE (two line element) data file (ref [2],[3])
380 | * args   : char   *file     I   NORAD TLE data file
381 | *          tle_t  *tle      O   TLE data
382 | * return : status (1:ok,0:error)
383 | * notes  : before calling the function, the TLE data should be initialized.
384 | *          the file should be in a two line (only TLE) or three line (satellite
385 | *          name + TLE) format.
386 | *          the characters after # in a line are treated as comments.
387 | *-----------------------------------------------------------------------------*/
388 | extern int tle_read(const char *file, tle_t *tle)
389 | {
390 |     FILE *fp;
391 |     tled_t data={{0}};
392 |     char *p,buff[256];
393 |     int line=0;
394 |     
395 |     if (!(fp=fopen(file,"r"))) {
396 |         trace(2,"tle file open error: %s\n",file);
397 |         return 0;
398 |     }
399 |     while (fgets(buff,sizeof(buff),fp)) {
400 |         
401 |         /* delete comments */
402 |         if ((p=strchr(buff,'#'))) *p='\0';
403 |         chop(buff);
404 |         
405 |         if (buff[0]=='1'&&checksum(buff)) {
406 |             
407 |             /* decode TLE line 1 */
408 |             if (decode_line1(buff,&data)) line=1;
409 |         }
410 |         else if (line==1&&buff[0]=='2'&&checksum(buff)) {
411 |             
412 |             /* decode TLE line 2 */
413 |             if (!decode_line2(buff,&data)) continue;
414 |             
415 |             /* add TLE data */
416 |             if (!add_data(tle,&data)) {
417 |                 fclose(fp);
418 |                 return 0;
419 |             }
420 |             data.name[0]='\0';
421 |             data.alias[0]='\0';
422 |         }
423 |         else if (buff[0]) {
424 |             
425 |             /* satellite name in three line format */
426 |             strcpy(data.alias,buff);
427 |             
428 |             /* omit words in parentheses */
429 |             if ((p=strchr(data.alias,'('))) *p='\0';
430 |             chop(data.alias);
431 |             line=0;
432 |         }
433 |     }
434 |     fclose(fp);
435 |     
436 |     /* sort tle data by satellite name */
437 |     if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data);
438 |     return 1;
439 | }
440 | /* read TLE satellite name file ------------------------------------------------
441 | * read TLE satellite name file
442 | * args   : char   *file     I   TLE satellite name file
443 | *          tle_t  *tle      IO  TLE data
444 | * return : status (1:ok,0:error)
445 | * notes  : before calling the function, call tle_read() to read tle table
446 | *          the TLE satellite name file contains the following record as a text
447 | *          line. strings after # are treated as comments.
448 | *
449 | *          name satno [desig [# comment]]
450 | *
451 | *            name : satellite name
452 | *            satno: satellite catalog number
453 | *            desig: international designator (optional)
454 | *-----------------------------------------------------------------------------*/
455 | extern int tle_name_read(const char *file, tle_t *tle)
456 | {
457 |     FILE *fp;
458 |     tled_t data;
459 |     char *p,buff[256],name[256],satno[256],desig[256];
460 |     int i;
461 |     
462 |     if (!(fp=fopen(file,"r"))) {
463 |         trace(2,"tle satellite name file open error: %s\n",file);
464 |         return 0;
465 |     }
466 |     while (fgets(buff,sizeof(buff),fp)) {
467 |         
468 |         if ((p=strchr(buff,'#'))) *p='\0';
469 |         
470 |         desig[0]='\0';
471 |         
472 |         if (sscanf(buff,"%s %s %s",name,satno,desig)<2) continue;
473 |         satno[5]='\0';
474 |         
475 |         for (i=0;i<tle->n;i++) {
476 |             if (!strcmp(tle->data[i].satno,satno)||
477 |                 !strcmp(tle->data[i].desig,desig)) break;
478 |         }
479 |         if (i>=tle->n) {
480 |             trace(4,"no tle data: satno=%s desig=%s\n",satno,desig);
481 |             continue;
482 |         }
483 |         if (!*tle->data[i].name) {
484 |             strncpy(tle->data[i].name,name,31);
485 |             tle->data[i].name[31]='\0';
486 |         }
487 |         else {
488 |             data=tle->data[i];
489 |             strncpy(data.name,name,31);
490 |             data.name[31]='\0';
491 |             if (!add_data(tle,&data)) {
492 |                 break;
493 |             }
494 |         }
495 |     }
496 |     fclose(fp);
497 |     
498 |     /* sort tle data by satellite name */
499 |     if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data);
500 |     return 1;
501 | }
502 | /* satellite position and velocity with TLE data -------------------------------
503 | * compute satellite position and velocity in ECEF with TLE data
504 | * args   : gtime_t time     I   time (GPST)
505 | *          char   *name     I   satellite name           ("": not specified)
506 | *          char   *satno    I   satellite catalog number ("": not specified)
507 | *          char   *desig    I   international designaor  ("": not specified)
508 | *          tle_t  *tle      I   TLE data
509 | *          erp_t  *erp      I   EOP data (NULL: not used)
510 | *          double *rs       O   sat position/velocity {x,y,z,vx,vy,vz} (m,m/s)
511 | * return : status (1:ok,0:error)
512 | * notes  : the coordinates of the position and velocity are ECEF (ITRF)
513 | *          if erp == NULL, polar motion and ut1-utc are neglected
514 | *-----------------------------------------------------------------------------*/
515 | extern int tle_pos(gtime_t time, const char *name, const char *satno,
516 |                    const char *desig, const tle_t *tle, const erp_t *erp,
517 |                    double *rs)
518 | {
519 |     gtime_t tutc;
520 |     double tsince,rs_tle[6],rs_pef[6],gmst;
521 |     double R1[9]={0},R2[9]={0},R3[9]={0},W[9],erpv[5]={0};
522 |     int i=0,j,k,stat=1;
523 |     
524 |     /* binary search by satellite name */
525 |     if (*name) {
526 |         for (i=j=0,k=tle->n-1;j<=k;) {
527 |             i=(j+k)/2;
528 |             if (!(stat=strcmp(name,tle->data[i].name))) break;
529 |             if (stat<0) k=i-1; else j=i+1;
530 |         }
531 |     }
532 |     /* serial search by catalog no or international designator */
533 |     if (stat&&(*satno||*desig)) {
534 |         for (i=0;i<tle->n;i++) {
535 |             if (!strcmp(tle->data[i].satno,satno)||
536 |                 !strcmp(tle->data[i].desig,desig)) break;
537 |         }
538 |         if (i<tle->n) stat=0;
539 |     }
540 |     if (stat) {
541 |         trace(4,"no tle data: name=%s satno=%s desig=%s\n",name,satno,desig);
542 |         return 0;
543 |     }
544 |     tutc=gpst2utc(time);
545 |     
546 |     /* time since epoch (min) */
547 |     tsince=timediff(tutc,tle->data[i].epoch)/60.0;
548 |     
549 |     /* SGP4 model propagator by STR#3 */
550 |     SGP4_STR3(tsince,tle->data+i,rs_tle);
551 |     
552 |     /* erp values */
553 |     if (erp) geterp(erp,time,erpv);
554 |     
555 |     /* GMST (rad) */
556 |     gmst=utc2gmst(tutc,erpv[2]);
557 |     
558 |     /* TEME (true equator, mean eqinox) -> ECEF (ref [2] IID, Appendix C) */
559 |     R1[0]=1.0; R1[4]=R1[8]=cos(-erpv[1]); R1[7]=sin(-erpv[1]); R1[5]=-R1[7];
560 |     R2[4]=1.0; R2[0]=R2[8]=cos(-erpv[0]); R2[2]=sin(-erpv[0]); R2[6]=-R2[2];
561 |     R3[8]=1.0; R3[0]=R3[4]=cos(gmst); R3[3]=sin(gmst); R3[1]=-R3[3];
562 |     matmul("NN",3,1,3,1.0,R3,rs_tle  ,0.0,rs_pef  );
563 |     matmul("NN",3,1,3,1.0,R3,rs_tle+3,0.0,rs_pef+3);
564 |     rs_pef[3]+=OMGE*rs_pef[1];
565 |     rs_pef[4]-=OMGE*rs_pef[0];
566 |     matmul("NN",3,3,3,1.0,R1,R2,0.0,W);
567 |     matmul("NN",3,1,3,1.0,W,rs_pef  ,0.0,rs  );
568 |     matmul("NN",3,1,3,1.0,W,rs_pef+3,0.0,rs+3);
569 |     return 1;
570 | }
571 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # GNSS Algorithm based on RTKLIB
 2 | 
 3 | ![GNSS-Explorer.png](https://github.com/brucezhcw/GNSS-Explorer/blob/linux/image/GNSS-Explorer.png) 
 4 | 
 5 | 本项目代码以RTKLIB为基础，用EKF扩展卡尔曼滤波算法实现了低精度绝对观测伪距 + 高精度相对观测多普勒数据的融合。
 6 | 相对观测多普勒数据精度较高（多普勒速度误差典型值10cm/s），结合卡尔曼滤波算法，可以保证定位轨迹局部的连续性，
 7 | 绝对观测伪距精度较低，但可以保证全局无偏，二者融合效果非常棒
 8 | 
 9 | 目前代码维护两个分支对应两个平台：linux + Visual Studio
10 | 
11 | ## linux平台下标准cmake编译运行流程:
12 | ```
13 | cd GNSS_Algorithm
14 | mkdir build
15 | cd build
16 | cmake ..
17 | make
18 | 
19 | ./gnss_algorithm YOUR_PATH_TO_RINEX_DATA_DIRECTORY
20 | ```
21 | 
22 | 参考资料：
23 | 
24 | [GNSS算法进阶（二）- kalman滤波单点定位算法代码实现](https://zhuanlan.zhihu.com/p/577601009)
25 | 
26 | [智能手机抗差SPP算法 - MobileGNSS-SPP](https://github.com/salmoshu/MobileGNSS-SPP)


--------------------------------------------------------------------------------
/data/20221128.rtcm3:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/brucezhcw/GNSS-Explorer/d3ba47d0f697fa6664a362660b628f4e6a11a665/data/20221128.rtcm3


--------------------------------------------------------------------------------
/data/20221129.rtcm3:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/brucezhcw/GNSS-Explorer/d3ba47d0f697fa6664a362660b628f4e6a11a665/data/20221129.rtcm3


--------------------------------------------------------------------------------
/image/GNSS-Explorer.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/brucezhcw/GNSS-Explorer/d3ba47d0f697fa6664a362660b628f4e6a11a665/image/GNSS-Explorer.png


--------------------------------------------------------------------------------