add more work on rtklib functions

src/algorithms/libs/rtklib/rtklib.h

@@ -66,7 +66,6 @@
 
 
 const int FILEPATHSEP = '/';
-
 const double RE_WGS84 = 6378137.0;             //!< earth semimajor axis (WGS84) (m)
 const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84)
 

src/algorithms/libs/rtklib/rtklib_rtkcmn.cc

@@ -1703,6 +1703,13 @@ void deg2dms(double deg, double *dms, int ndec)
     dms[0]*=sign;
 }
 
+void deg2dms(double deg, double *dms)
+{
+    double sign=deg<0.0?-1.0:1.0,a=fabs(deg);
+    dms[0]=floor(a); a=(a-dms[0])*60.0;
+    dms[1]=floor(a); a=(a-dms[1])*60.0;
+    dms[2]=a; dms[0]*=sign;
+}
 
 /* convert deg-min-sec to degree -----------------------------------------------
  * convert degree-minute-second to degree

src/algorithms/libs/rtklib/rtklib_rtkcmn.h

@@ -174,6 +174,7 @@ int adjgpsweek(int week);
 unsigned int tickget(void);
 void sleepms(int ms);
 void deg2dms(double deg, double *dms, int ndec);
+void deg2dms(double deg, double *dms);
 double dms2deg(const double *dms);
 void ecef2pos(const double *r, double *pos);
 void pos2ecef(const double *pos, double *r);
@@ -182,8 +183,6 @@ void ecef2enu(const double *pos, const double *r, double *e);
 void enu2ecef(const double *pos, const double *e, double *r);
 void covenu(const double *pos, const double *P, double *Q);
 void covecef(const double *pos, const double *Q, double *P);
-
-
 void ast_args(double t, double *f);
 void nut_iau1980(double t, const double *f, double *dpsi, double *deps);
 void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst);

src/algorithms/libs/rtklib/rtklib_rtksvr.cc

@@ -6,13 +6,13 @@
 #include "rtklib_sbas.h"
 #include "rtklib_preceph.h"
 #include "rtklib_stream.h"
+#include "rtklib_rtcm.h"
 
 /* write solution header to output stream ------------------------------------*/
 void writesolhead(stream_t *stream, const solopt_t *solopt)
 {
     unsigned char buff[1024];
     int n;
-
     n = outsolheads(buff,solopt);
     strwrite(stream,buff,n);
 }
@@ -179,7 +179,8 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
     }
     else if (ret == 3)
     { /* sbas message */
-        if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) {
+        if (sbsmsg && (sbssat == sbsmsg->prn || sbssat == 0))
+        {
             if (svr->nsbs<MAXSBSMSG)
             {
                 svr->sbsmsg[svr->nsbs++] = *sbsmsg;
@@ -357,7 +358,8 @@ void decodefile(rtksvr_t *svr, int index)
 
     /* check file path completed */
     if ((nb = svr->nb[index]) <= 2 ||
-        svr->buff[index][nb-2]!='\r'||svr->buff[index][nb-1]!='\n') {
+        svr->buff[index][nb-2] != '\r' || svr->buff[index][nb-1] != '\n')
+        {
         rtksvrunlock(svr);
         return;
     }
@@ -370,7 +372,8 @@ void decodefile(rtksvr_t *svr, int index)
 
         /* read sp3 precise ephemeris */
         readsp3(file,&nav,0);
-        if (nav.ne<=0) {
+        if (nav.ne <= 0)
+        {
             tracet(1,"sp3 file read error: %s\n",file);
             return;
         }

src/algorithms/libs/rtklib/rtklib_solution.cc

@@ -53,6 +53,8 @@
 
 #include <ctype.h>
 #include "rtklib_solution.h"
+#include "rtklib_rtkcmn.h"
+#include "rtklib_rtksvr.h"
 
 
 /* constants and macros ------------------------------------------------------*/
@@ -60,16 +62,15 @@
 #define SQR_SOL(x)     ((x)<0.0?-(x)*(x):(x)*(x))
 #define SQRT_SOL(x)    ((x)<0.0?0.0:sqrt(x))
 
-#define MAXFIELD   64           /* max number of fields in a record */
-#define MAXNMEA    256          /* max length of nmea sentence */
+const int MAXFIELD = 64;          /* max number of fields in a record */
+const int MAXNMEA = 256;          /* max length of nmea sentence */
 
-#define KNOT2M     0.514444444  /* m/knot */
+const double KNOT2M = 0.514444444;  /* m/knot */
 
 static const int solq_nmea[] = {  /* nmea quality flags to rtklib sol quality */
     /* nmea 0183 v.2.3 quality flags: */
     /*  0=invalid, 1=gps fix (sps), 2=dgps fix, 3=pps fix, 4=rtk, 5=float rtk */
     /*  6=estimated (dead reckoning), 7=manual input, 8=simulation */
-
     SOLQ_NONE ,SOLQ_SINGLE, SOLQ_DGPS, SOLQ_PPP , SOLQ_FIX,
     SOLQ_FLOAT,SOLQ_DR    , SOLQ_NONE, SOLQ_NONE, SOLQ_NONE
 };
@@ -899,7 +900,7 @@ void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax)
     if (cyclic)
     {
         if (nmax <= 2) nmax=2;
-        if (!(solbuf->data=malloc(sizeof(sol_t)*nmax)))
+        if (!(solbuf->data = (sol_t*)malloc(sizeof(sol_t)*nmax)))
         {
             trace(1,"initsolbuf: memory allocation error\n");
             return;
@@ -952,7 +953,8 @@ int sort_solstat(solstatbuf_t *statbuf)
 
     if (statbuf->n <= 0) return 0;
 
-    if (!(statbuf_data=realloc(statbuf->data,sizeof(solstat_t)*statbuf->n))) {
+    if (!(statbuf_data = (solstat_t*)realloc(statbuf->data,sizeof(solstat_t)*statbuf->n)))
+    {
         trace(1,"sort_solstat: memory allocation error\n");
         free(statbuf->data); statbuf->data = NULL; statbuf->n = statbuf->nmax = 0;
         return 0;
@@ -1140,7 +1142,7 @@ int outpos(unsigned char *buff, const char *s, const sol_t *sol,
     covenu(pos,P,Q);
     if (opt->height == 1)
     { /* geodetic height */
-        pos[2]-=geoidh(pos);
+        // pos[2] -= geoidh(pos);
     }
     if (opt->degf)
     {
@@ -1205,7 +1207,7 @@ int outnmea_rmc(unsigned char *buff, const sol_t *sol)
     time2epoch(time,ep);
     ecef2pos(sol->rr,pos);
     ecef2enu(pos,sol->rr+3,enuv);
-    vel=norm(enuv,3);
+    vel = norm_rtk(enuv,3);
     if (vel >= 1.0)
     {
         dir = atan2(enuv[0],enuv[1])*R2D;
@@ -1249,7 +1251,7 @@ int outnmea_gga(unsigned char *buff, const sol_t *sol)
     if (time.sec >= 0.995) {time.time++; time.sec = 0.0;}
     time2epoch(time,ep);
     ecef2pos(sol->rr,pos);
-    h=geoidh(pos);
+    h = 0;//geoidh(pos);
     deg2dms(fabs(pos[0])*R2D,dms1);
     deg2dms(fabs(pos[1])*R2D,dms2);
     p += sprintf(p,"$GPGGA,%02.0f%02.0f%05.2f,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%d,%02d,%.1f,%.3f,M,%.3f,M,%.1f,",
@@ -1281,7 +1283,8 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
     }
 
     /* GPGSA: gps/sbas */
-    for (sat=1,nsat=0;sat<=MAXSAT&&nsat<12;sat++) {
+    for (sat = 1,nsat = 0;sat <= MAXSAT&&nsat<12;sat++)
+    {
         if (!ssat[sat-1].vs || ssat[sat-1].azel[1] <= 0.0) continue;
         sys = satsys(sat,prn+nsat);
         if (sys != SYS_GPS && sys != SYS_SBS) continue;
@@ -1633,7 +1636,7 @@ int outsols(unsigned char *buff, const sol_t *sol, const double *rb,
         if (opt->nmeaintv[0]<0.0) return 0;
         if (!screent(sol->time,ts,ts,opt->nmeaintv[0])) return 0;
     }
-    if (sol->stat<=SOLQ_NONE||(opt->posf==SOLF_ENU&&norm(rb,3)<=0.0))
+    if (sol->stat <= SOLQ_NONE || (opt->posf == SOLF_ENU && norm_rtk(rb,3) <= 0.0))
     {
         return 0;
     }

src/algorithms/libs/rtklib/rtklib_solution.h

@@ -55,6 +55,9 @@
 
 #include "rtklib.h"
 
+#define COMMENTH    "%"                 /* comment line indicator for solution */
+#define MSG_DISCONN "$_DISCONNECT\r\n"  /* disconnect message */
+
 const char *opt2sep(const solopt_t *opt);
 int tonum(char *buff, const char *sep, double *v);
 double sqvar(double covar);