rtkrcv.cpp
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------------
00002 * rtkrcv.cpp : based on rtk-gps/gnss receiver console app
00003 *
00004 *          Copyright (C) 2009-2013 by T.TAKASU, All rights reserved.
00005 *          Copyright (C) 2012      by M.STAHL, All rights reserved.
00006 *
00007 * notes   :
00008 *     current version does not support win32 without pthread library
00009 *
00010 * version : $Revision:$ $Date:$
00011 * history : 2009/12/13 1.0  new
00012 *           2010/07/18 1.1  add option -m
00013 *           2010/08/12 1.2  fix bug on ftp/http
00014 *           2011/01/22 1.3  add option misc-proxyaddr,misc-fswapmargin
00015 *           2011/08/19 1.4  fix bug on size of arg solopt arg for rtksvrstart()
00016 *           2012/11/25      use as template for ROS node
00017 *           2012/11/03 1.5  fix bug on setting output format
00018 *           2013/06/30 1.6  add "nvs" option for inpstr*-format
00019 *           2013/09/07      update template for ROS node
00020 *-----------------------------------------------------------------------------*/
00021 #ifndef WIN32
00022 #define _POSIX_C_SOURCE 2
00023 #endif
00024 #include <stdio.h>
00025 #include <stdlib.h>
00026 #include <string.h>
00027 #include <stdarg.h>
00028 #include <ctype.h>
00029 #ifdef WIN32
00030 #else
00031 #include <unistd.h>
00032 #include <errno.h>
00033 #include <pthread.h>
00034 #endif
00035 #include <rtklib/rtklib.h>
00036 
00037 #define MAXSTR      1024                /* max length of a string */
00038 #define MAXBUFF     1024                /* max input buffer */
00039 #define MAXRCVCMD   4096                /* max receiver command */
00040 #define NAVIFILE    "rtkrcv.nav"        /* navigation save file */
00041 #define STATFILE    "rtkrcv.stat"       /* solution status file */
00042 #define TRACEFILE   "rtkrcv.trace"      /* trace file */
00043 #define INTKEEPALIVE 1000               /* keep alive interval (ms) */
00044 
00045 #define MIN(x,y)    ((x)<(y)?(x):(y))
00046 #define MAX(x,y)    ((x)>(y)?(x):(y))
00047 #define SQRT(x)     ((x)<=0.0?0.0:sqrt(x))
00048 
00049 /* type definition -----------------------------------------------------------*/
00050 typedef struct {                        /* virtual terminal type */
00051     int type;                           /* type (0:stdio,1:remote,2:device) */
00052     int state;                          /* state(0:close,1:open) */
00053     int in,out;                         /* input/output file descriptor */
00054     int nbuff;                          /* number of data */
00055     char buff[MAXBUFF];                 /* input buffer */
00056     pthread_t svr;                      /* input server */
00057     pthread_t parent;                   /* parent thread */
00058     pthread_mutex_t lock;               /* lock flag */
00059     pthread_cond_t event;               /* event flag */
00060 } vt_t;
00061 
00062 /* global variables ----------------------------------------------------------*/
00063 static rtksvr_t svr;                    /* rtk server struct */
00064 static stream_t moni;                   /* monitor stream */
00065 
00066 static FILE *logfp      =NULL;          /* log file pointer */
00067 
00068 static char passwd[MAXSTR]="admin";     /* login password */
00069 static int timetype     =0;             /* time format (0:gpst,1:utc,2:jst,3:tow) */
00070 static int soltype      =0;             /* sol format (0:dms,1:deg,2:xyz,3:enu,4:pyl) */
00071 static int solflag      =2;             /* sol flag (1:std+2:age/ratio/ns) */
00072 static int strtype[]={                  /* stream types */
00073     STR_SERIAL,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE
00074 };
00075 static char strpath[8][MAXSTR]={""};    /* stream paths */
00076 static int strfmt[]={                   /* stream formats */
00077     STRFMT_UBX,STRFMT_RTCM3,STRFMT_SP3,SOLF_LLH,SOLF_NMEA
00078 };
00079 static int svrcycle     =10;            /* server cycle (ms) */
00080 static int timeout      =10000;         /* timeout time (ms) */
00081 static int reconnect    =10000;         /* reconnect interval (ms) */
00082 static int nmeacycle    =5000;          /* nmea request cycle (ms) */
00083 static int buffsize     =32768;         /* input buffer size (bytes) */
00084 static int navmsgsel    =0;             /* navigation mesaage select */
00085 static char proxyaddr[256]="";          /* http/ntrip proxy */
00086 static int nmeareq      =0;             /* nmea request type (0:off,1:lat/lon,2:single) */
00087 static double nmeapos[] ={0,0};         /* nmea position (lat/lon) (deg) */
00088 static char rcvcmds[3][MAXSTR]={""};    /* receiver commands files */
00089 static char startcmd[MAXSTR]="";        /* start command */
00090 static char stopcmd [MAXSTR]="";        /* stop command */
00091 static int modflgr[256] ={0};           /* modified flags of receiver options */
00092 static int modflgs[256] ={0};           /* modified flags of system options */
00093 static int moniport     =0;             /* monitor port */
00094 static int keepalive    =0;             /* keep alive flag */
00095 static int fswapmargin  =30;            /* file swap margin (s) */
00096 
00097 static prcopt_t prcopt;                 /* processing options */
00098 static solopt_t solopt[2]={{0}};        /* solution options */
00099 static filopt_t filopt  ={""};          /* file options */
00100 
00101 /* receiver options table ----------------------------------------------------*/
00102 #define TIMOPT  "0:gpst,1:utc,2:jst,3:tow"
00103 #define CONOPT  "0:dms,1:deg,2:xyz,3:enu,4:pyl"
00104 #define FLGOPT  "0:off,1:std+2:age/ratio/ns"
00105 #define ISTOPT  "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http"
00106 #define OSTOPT  "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr"
00107 #define FMTOPT  "0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,8:gw10,9:javad,10:nvs,15:sp3"
00108 #define NMEOPT  "0:off,1:latlon,2:single"
00109 #define SOLOPT  "0:llh,1:xyz,2:enu,3:nmea"
00110 #define MSGOPT  "0:all,1:rover,2:base,3:corr"
00111 
00112 static opt_t rcvopts[]={
00113     {"console-passwd",  2,  (void *)passwd,              ""     },
00114     {"console-timetype",3,  (void *)&timetype,           TIMOPT },
00115     {"console-soltype", 3,  (void *)&soltype,            CONOPT },
00116     {"console-solflag", 0,  (void *)&solflag,            FLGOPT },
00117     
00118     {"inpstr1-type",    3,  (void *)&strtype[0],         ISTOPT },
00119     {"inpstr2-type",    3,  (void *)&strtype[1],         ISTOPT },
00120     {"inpstr3-type",    3,  (void *)&strtype[2],         ISTOPT },
00121     {"inpstr1-path",    2,  (void *)strpath [0],         ""     },
00122     {"inpstr2-path",    2,  (void *)strpath [1],         ""     },
00123     {"inpstr3-path",    2,  (void *)strpath [2],         ""     },
00124     {"inpstr1-format",  3,  (void *)&strfmt [0],         FMTOPT },
00125     {"inpstr2-format",  3,  (void *)&strfmt [1],         FMTOPT },
00126     {"inpstr3-format",  3,  (void *)&strfmt [2],         FMTOPT },
00127     {"inpstr2-nmeareq", 3,  (void *)&nmeareq,            NMEOPT },
00128     {"inpstr2-nmealat", 1,  (void *)&nmeapos[0],         "deg"  },
00129     {"inpstr2-nmealon", 1,  (void *)&nmeapos[1],         "deg"  },
00130     {"outstr1-type",    3,  (void *)&strtype[3],         OSTOPT },
00131     {"outstr2-type",    3,  (void *)&strtype[4],         OSTOPT },
00132     {"outstr1-path",    2,  (void *)strpath [3],         ""     },
00133     {"outstr2-path",    2,  (void *)strpath [4],         ""     },
00134     {"outstr1-format",  3,  (void *)&strfmt [3],         SOLOPT },
00135     {"outstr2-format",  3,  (void *)&strfmt [4],         SOLOPT },
00136     {"logstr1-type",    3,  (void *)&strtype[5],         OSTOPT },
00137     {"logstr2-type",    3,  (void *)&strtype[6],         OSTOPT },
00138     {"logstr3-type",    3,  (void *)&strtype[7],         OSTOPT },
00139     {"logstr1-path",    2,  (void *)strpath [5],         ""     },
00140     {"logstr2-path",    2,  (void *)strpath [6],         ""     },
00141     {"logstr3-path",    2,  (void *)strpath [7],         ""     },
00142     
00143     {"misc-svrcycle",   0,  (void *)&svrcycle,           "ms"   },
00144     {"misc-timeout",    0,  (void *)&timeout,            "ms"   },
00145     {"misc-reconnect",  0,  (void *)&reconnect,          "ms"   },
00146     {"misc-nmeacycle",  0,  (void *)&nmeacycle,          "ms"   },
00147     {"misc-buffsize",   0,  (void *)&buffsize,           "bytes"},
00148     {"misc-navmsgsel",  3,  (void *)&navmsgsel,          MSGOPT },
00149     {"misc-proxyaddr",  2,  (void *)proxyaddr,           ""     },
00150     {"misc-fswapmargin",0,  (void *)&fswapmargin,        "s"    },
00151     
00152     {"misc-startcmd",   2,  (void *)startcmd,            ""     },
00153     {"misc-stopcmd",    2,  (void *)stopcmd,             ""     },
00154     
00155     {"file-cmdfile1",   2,  (void *)rcvcmds[0],          ""     },
00156     {"file-cmdfile2",   2,  (void *)rcvcmds[1],          ""     },
00157     {"file-cmdfile3",   2,  (void *)rcvcmds[2],          ""     },
00158     
00159     {"",0,NULL,""}
00160 };
00161 
00162 /* discard space characters at tail ------------------------------------------*/
00163 static void chop(char *str)
00164 {
00165     char *p;
00166     for (p=str+strlen(str)-1;p>=str&&!isgraph((int)*p);p--) *p='\0';
00167 }
00168 /* thread to send keep alive for monitor port --------------------------------*/
00169 static void *sendkeepalive(void *arg)
00170 {
00171     trace(3,"sendkeepalive: start\n");
00172     
00173     while (keepalive) {
00174         strwrite(&moni,(unsigned char *)"\r",1);
00175         sleepms(INTKEEPALIVE);
00176     }
00177     trace(3,"sendkeepalive: stop\n");
00178     return NULL;
00179 }
00180 /* open monitor port ---------------------------------------------------------*/
00181 static int openmoni(int port)
00182 {
00183     pthread_t thread;
00184     char path[64];
00185     
00186     trace(3,"openmomi: port=%d\n",port);
00187     
00188     sprintf(path,":%d",port);
00189     if (!stropen(&moni,STR_TCPSVR,STR_MODE_RW,path)) return 0;
00190     strsettimeout(&moni,timeout,reconnect);
00191     keepalive=1;
00192     pthread_create(&thread,NULL,sendkeepalive,NULL);
00193     return 1;
00194 }
00195 /* close monitor port --------------------------------------------------------*/
00196 static void closemoni(void)
00197 {
00198     trace(3,"closemoni:\n");
00199     keepalive=0;
00200     
00201     /* send disconnect message */
00202     strwrite(&moni,(unsigned char *)MSG_DISCONN,strlen(MSG_DISCONN));
00203     
00204     /* wait fin from clients */
00205     sleepms(1000);
00206     
00207     strclose(&moni);
00208 }
00209 /* read remote console -------------------------------------------------------*/
00210 static int readremote(vt_t *vt, char *buff, int nmax)
00211 {
00212     int n=0;
00213     
00214     trace(4,"readremote:\n");
00215     
00216     if (!vt->state) return 0;
00217     pthread_cond_wait(&vt->event,&vt->lock);
00218     if (vt->state) {
00219         n=MIN(nmax,vt->nbuff);
00220         memcpy(buff,vt->buff,n);
00221         vt->nbuff=0;
00222     }
00223     pthread_mutex_unlock(&vt->lock);
00224     return n;
00225 }
00226 /* write remote console ------------------------------------------------------*/
00227 static int writeremote(vt_t *vt, char *buff, int n)
00228 {
00229     char *p,*q,crlf[]="\r\n";;
00230     
00231     trace(4,"writeremote: n=%d\n",n);
00232     
00233     if (!vt->state) return 0;
00234     
00235     for (p=buff;p<buff+n;p=q+1) {
00236         if (!(q=strchr(p,'\n'))) {
00237             if (write(vt->out,p,buff+n-p)<buff+n-p) return 0;
00238             break;
00239         }
00240         if (write(vt->out,p,q-p)<q-p) return 0;
00241         if (write(vt->out,crlf,2)<2) return 0;
00242     }
00243     return 1;
00244 }
00245 /* output to console ---------------------------------------------------------*/
00246 static int outvt(vt_t *vt, char *buff, int n)
00247 {
00248     trace(4,"outvt: n=%d\n",n);
00249     
00250     if (logfp) fwrite(buff,n,1,logfp);
00251     if (vt->type==1) return writeremote(vt,buff,n);
00252     return write(vt->out,buff,n)==n;
00253 }
00254 /* input line from console ---------------------------------------------------*/
00255 static int inpvt(vt_t *vt, char *buff, int nmax)
00256 {
00257     int n;
00258     
00259     trace(4,"inpvt:\n");
00260     
00261     if (vt->type==1) n=readremote(vt,buff,nmax-1);
00262     else n=read(vt->in,buff,nmax-1);
00263     if (n<=0) {
00264         outvt(vt,"\n",1);
00265         return 0;
00266     }
00267     buff[n]='\0';
00268     if (logfp) fprintf(logfp,"%s",buff);
00269     chop(buff);
00270     return 1;
00271 }
00272 /* printf to console ---------------------------------------------------------*/
00273 static int printvt(vt_t *vt, const char *format, ...)
00274 {
00275     va_list ap;
00276     int n;
00277     char buff[MAXSTR];
00278     
00279     trace(4,"prvt:\n");
00280     
00281     if (!vt->state) {
00282         va_start(ap,format);
00283         vfprintf(stderr,format,ap);
00284         va_end(ap);
00285         return 1;
00286     }
00287     va_start(ap,format);
00288     n=vsprintf(buff,format,ap);
00289     va_end(ap);
00290     return outvt(vt,buff,n);
00291 }
00292 /* confirm overwrite ---------------------------------------------------------*/
00293 static int confwrite(vt_t *vt, const char *file)
00294 {
00295     FILE *fp;
00296     char buff[MAXSTR],*p;
00297     
00298     strcpy(buff,file);
00299     if ((p=strstr(buff,"::"))) *p='\0'; /* omit options in path */
00300     if (!vt->state||!(fp=fopen(buff,"r"))) return 1; /* no existing file */
00301     fclose(fp);
00302     printvt(vt,"overwrite %-16s ? (y/n): ",buff);
00303     if (!inpvt(vt,buff,sizeof(buff))) return 0;
00304     return toupper((int)buff[0])=='Y';   
00305 }
00306 /* read receiver commands ----------------------------------------------------*/
00307 static int readcmd(const char *file, char *cmd, int type)
00308 {
00309     FILE *fp;
00310     char buff[MAXSTR],*p=cmd;
00311     int i=0;
00312     
00313     trace(3,"readcmd: file=%s\n",file);
00314     
00315     if (!(fp=fopen(file,"r"))) return 0;
00316     
00317     while (fgets(buff,sizeof(buff),fp)) {
00318         if (*buff=='@') i=1;
00319         else if (i==type&&p+strlen(buff)+1<cmd+MAXRCVCMD) {
00320             p+=sprintf(p,"%s",buff);
00321         }
00322     }
00323     fclose(fp);
00324     return 1;
00325 }
00326 /* read antenna file ---------------------------------------------------------*/
00327 static void readant(vt_t *vt, prcopt_t *opt, nav_t *nav)
00328 {
00329     const pcv_t pcv0={0};
00330     pcvs_t pcvr={0},pcvs={0};
00331     pcv_t *pcv;
00332     gtime_t time=timeget();
00333     int i;
00334     
00335     trace(3,"readant:\n");
00336     
00337     opt->pcvr[0]=opt->pcvr[1]=pcv0;
00338     if (!*filopt.rcvantp) return;
00339     
00340     if (readpcv(filopt.rcvantp,&pcvr)) {
00341         for (i=0;i<2;i++) {
00342             if (!*opt->anttype[i]) continue;
00343             if (!(pcv=searchpcv(0,opt->anttype[i],time,&pcvr))) {
00344                 printvt(vt,"no antenna %s in %s",opt->anttype[i],filopt.rcvantp);
00345                 continue;
00346             }
00347             opt->pcvr[i]=*pcv;
00348         }
00349     }
00350     else printvt(vt,"antenna file open error %s",filopt.rcvantp);
00351     
00352     if (readpcv(filopt.satantp,&pcvs)) {
00353         for (i=0;i<MAXSAT;i++) {
00354             if (!(pcv=searchpcv(i+1,"",time,&pcvs))) continue;
00355             nav->pcvs[i]=*pcv;
00356         }
00357     }
00358     else printvt(vt,"antenna file open error %s",filopt.satantp);
00359     
00360     free(pcvr.pcv); free(pcvs.pcv);
00361 }
00362 /* start rtk server ----------------------------------------------------------*/
00363 static int startsvr(vt_t *vt)
00364 {
00365     double pos[3],npos[3];
00366     char s[3][MAXRCVCMD]={"","",""},*cmds[]={NULL,NULL,NULL};
00367     char *ropts[]={"","",""};
00368     char *paths[]={
00369         strpath[0],strpath[1],strpath[2],strpath[3],strpath[4],strpath[5],
00370         strpath[6],strpath[7]
00371     };
00372     int i,ret,stropt[8]={0};
00373     
00374     trace(3,"startsvr:\n");
00375     
00376     /* read start commads from command files */
00377     for (i=0;i<3;i++) {
00378         if (!*rcvcmds[i]) continue;
00379         if (!readcmd(rcvcmds[i],s[i],0)) {
00380             printvt(vt,"no command file: %s\n",rcvcmds[i]);
00381         }
00382         else cmds[i]=s[i];
00383     }
00384     /* confirm overwrite */
00385     for (i=3;i<8;i++) {
00386         if (strtype[i]==STR_FILE&&!confwrite(vt,strpath[i])) return 0;
00387     }
00388     if (prcopt.refpos==4) { /* rtcm */
00389         for (i=0;i<3;i++) prcopt.rb[i]=0.0;
00390     }
00391     pos[0]=nmeapos[0]*D2R;
00392     pos[1]=nmeapos[1]*D2R;
00393     pos[2]=0.0;
00394     pos2ecef(pos,npos);
00395     
00396     /* read antenna file */
00397     readant(vt,&prcopt,&svr.nav);
00398     
00399     /* open geoid data file */
00400     if (solopt[0].geoid>0&&!opengeoid(solopt[0].geoid,filopt.geoid)) {
00401         trace(2,"geoid data open error: %s\n",filopt.geoid);
00402         printvt(vt,"geoid data open error: %s\n",filopt.geoid);
00403     }
00404     for (i=0;*rcvopts[i].name;i++) modflgr[i]=0;
00405     for (i=0;*sysopts[i].name;i++) modflgs[i]=0;
00406     
00407     /* set stream options */
00408     stropt[0]=timeout;
00409     stropt[1]=reconnect;
00410     stropt[2]=1000;
00411     stropt[3]=buffsize;
00412     stropt[4]=fswapmargin;
00413     strsetopt(stropt);
00414     
00415     if (strfmt[2]==8) strfmt[2]=STRFMT_SP3;
00416     
00417     /* set ftp/http directory and proxy */
00418     strsetdir(filopt.tempdir);
00419     strsetproxy(proxyaddr);
00420     
00421     /* execute start command */
00422     if (*startcmd&&(ret=system(startcmd))) {
00423         trace(2,"command exec error: %s (%d)\n",startcmd,ret);
00424         printvt(vt,"command exec error: %s (%d)\n",startcmd,ret);
00425     }
00426     solopt[0].posf=strfmt[3];
00427     solopt[1].posf=strfmt[4];
00428     
00429     /* start rtk server */
00430     if (!rtksvrstart(&svr,svrcycle,buffsize,strtype,paths,strfmt,navmsgsel,
00431                      cmds,ropts,nmeacycle,nmeareq,npos,&prcopt,solopt,&moni)) {
00432         trace(2,"rtk server start error\n");
00433         printvt(vt,"rtk server start error\n");
00434         return 0;
00435     }
00436     return 1;
00437 }
00438 /* stop rtk server -----------------------------------------------------------*/
00439 static void stopsvr(vt_t *vt)
00440 {
00441     char s[3][MAXRCVCMD]={"","",""},*cmds[]={NULL,NULL,NULL};
00442     int i,ret;
00443     
00444     trace(3,"stopsvr:\n");
00445     
00446     if (!svr.state) return;
00447     
00448     /* read stop commads from command files */
00449     for (i=0;i<3;i++) {
00450         if (!*rcvcmds[i]) continue;
00451         if (!readcmd(rcvcmds[i],s[i],1)) {
00452             printvt(vt,"no command file: %s\n",rcvcmds[i]);
00453         }
00454         else cmds[i]=s[i];
00455     }
00456     /* stop rtk server */
00457     rtksvrstop(&svr,cmds);
00458     
00459     /* execute stop command */
00460     if (*stopcmd&&(ret=system(stopcmd))) {
00461         trace(2,"command exec error: %s (%d)\n",stopcmd,ret);
00462         printvt(vt,"command exec error: %s (%d)\n",stopcmd,ret);
00463     }
00464     if (solopt[0].geoid>0) closegeoid();
00465 }
00466 
00467 /* rtkrcv main -----------------------------------------------------------------
00468 * sysnopsis
00469 *     rtkrcv [-m port][-t level][-r level]
00470 *
00471 * description
00472 *     A ROS version of the real-time positioning AP by rtklib. To start
00473 *     or stop RTK server, to configure options or to print solution/status,
00474 *     login a console and input commands. As default, stdin/stdout are used for
00475 *     the console. Use -p option for network login with telnet protocol. To show
00476 *     the available commands, type ? or help on the console. The initial
00477 *     processing options are loaded from ROS parameters. To configure the
00478 *     processing options, create a launch file or use set, load or save command
00479 *     on the console.
00480 *
00481 * option
00482 *     -m port    port number for monitor stream
00483 *     -r level   output solution status file (0:off,1:states,2:residuals)
00484 *     -t level   debug trace level (0:off,1-5:on)
00485 *
00486 *-----------------------------------------------------------------------------*/
00487 
00488 static vt_t svr_vt={0};
00489 static int outstat=0,rcv_trace=0;
00490 
00491 int loadrosopts(opt_t *opts);
00492 
00493 int start_rtkrcv(int argc, char **argv)
00494 {
00495     int i;
00496 
00497     for (i=1;i<argc;i++) {
00498         if (!strcmp(argv[i],"-m")&&i+1<argc) moniport=atoi(argv[++i]);
00499         else if (!strcmp(argv[i],"-r")&&i+1<argc) outstat=atoi(argv[++i]);
00500         else if (!strcmp(argv[i],"-t")&&i+1<argc) rcv_trace=atoi(argv[++i]);
00501         else fprintf(stderr,"Unknown option: %s\n",argv[i]);
00502     }
00503     if (rcv_trace>0) {
00504         traceopen(TRACEFILE);
00505         tracelevel(rcv_trace);
00506     }
00507     /* initialize rtk server and monitor port */
00508     rtksvrinit(&svr);
00509     strinit(&moni);
00510 
00511     /* load options from ROS */
00512     resetsysopts();
00513     loadrosopts(rcvopts);
00514     loadrosopts(sysopts);
00515     getsysopts(&prcopt,solopt,&filopt);
00516 
00517     /* read navigation data */
00518     if (!readnav(NAVIFILE,&svr.nav)) {
00519         fprintf(stderr,"no navigation data: %s\n",NAVIFILE);
00520     }
00521     if (outstat>0) {
00522         rtkopenstat(STATFILE,outstat);
00523     }
00524     /* open monitor port */
00525     if (moniport>0&&!openmoni(moniport)) {
00526         fprintf(stderr,"monitor port open error: %d\n",moniport);
00527         return 0;
00528     }
00529     /* start rtk server */
00530     if (!startsvr(&svr_vt)) return 0;
00531     return 1;
00532 }
00533 
00534 void stop_rtkrcv() {
00535     /* stop rtk server */
00536     stopsvr(&svr_vt);
00537 
00538     if (moniport>0) closemoni();
00539 
00540     if (outstat>0) rtkclosestat();
00541 
00542     if (rcv_trace>0) traceclose();
00543 
00544     /* save navigation data */
00545     if (!savenav(NAVIFILE,&svr.nav)) {
00546         fprintf(stderr,"navigation data save error: %s\n",NAVIFILE);
00547     }
00548 }
00549 
00550 char get_solution(double &lat, double &lon, double &height)
00551 {
00552     rtksvrlock(&svr);
00553     const sol_t *sol = &svr.solbuf[svr.nsol-1];
00554     const double *rb = svr.rtk.rb;
00555     double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0};
00556     double enu[3]={0},pitch=0.0,yaw=0.0,len;
00557     int i;
00558     int status;
00559 
00560     if (sol->time.time == 0 || !sol->stat) {
00561         rtksvrunlock(&svr);
00562         return -1; // no fix
00563     }
00564     if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix
00565     if (sol->stat == 3)                   status = 1; // sbas fix
00566     else                                  status = 2; // differential fix
00567 
00568     if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) {
00569         for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i];
00570     }
00571     len=norm(bl,3);
00572     Qr[0]=sol->qr[0];
00573     Qr[4]=sol->qr[1];
00574     Qr[8]=sol->qr[2];
00575     Qr[1]=Qr[3]=sol->qr[3];
00576     Qr[5]=Qr[7]=sol->qr[4];
00577     Qr[2]=Qr[6]=sol->qr[5];
00578 
00579     if (norm(sol->rr,3) > 0.0) {
00580         ecef2pos(sol->rr,pos);
00581         covenu(pos,Qr,Qe);
00582         lat = pos[0]*R2D;
00583         lon = pos[1]*R2D;
00584         if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */
00585         height = pos[2];
00586     }
00587     svr.nsol=0;
00588     rtksvrunlock(&svr);
00589     return status;
00590 }


rtkrcv
Author(s): Manuel Stahl
autogenerated on Mon Oct 6 2014 00:14:21