VS中读取NMEA数据进行定位精度分析

叁歲伎倆 2022-11-20 08:07 130阅读 0赞

> NMEA数据为Ucenter采集，通过读取NMEA数据，求出定位精度！
> 
> 实现代码如下:
> 
> ![20201027165032289.png][]
> 
> 其中，rtklib.h 和 rtkcmn.c 直接从SRC文件夹中添加不需要修改！
> 
> main.c 和 solution.c 代码如下：

main.c 代码如下：

// main.c
    #define _CRT_SECURE_NO_WARNINGS
    #include "rtklib.h"
    #define MAX 1024
     
    int main() {
    	//文件路径
    	FILE *fp;
    	char buff[MAX] = { 0 };
    	 ;
    	//char path[] = "E:/专业资料/网友交流/解析Ucenter，NMEA数据/u-cent数据/01.ubx";
    	char path[] = "E:/专业资料/网友交流/解析Ucenter，NMEA数据/u-cent数据/02.ubx";
    	if ((fp = fopen(path, "r")) == NULL) {
    		printf("读取失败！");
    		exit(EXIT_FAILURE);
    	}
    	// 处理函数
    	pro_nmea(fp, buff);
    	fclose(fp);
    	return 0;
    }

solution.c 代码如下：

#include <ctype.h>
    #include "rtklib.h"
    
    /* constants and macros ------------------------------------------------------*/
    
    #define SQR(x)     ((x)<0.0?-(x)*(x):(x)*(x))
    #define SQRT(x)    ((x)<0.0||(x)!=(x)?0.0:sqrt(x))
    
    #define MAXFIELD   64           /* max number of fields in a record */
    #define MAXNMEA    256          /* max length of nmea sentence */
    #define MAX 1024 //nmea 语句一行最大字符数
    #define 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
    };
    /* solution option to field separator ----------------------------------------*/
    static const char *opt2sep(const solopt_t *opt)
    {
        if (!*opt->sep) return " ";
        else if (!strcmp(opt->sep,"\\t")) return "\t";
        return opt->sep;
    }
    /* separate fields -----------------------------------------------------------*/
    static int tonum(char *buff, const char *sep, double *v)
    {
        int n,len=(int)strlen(sep);
        char *p,*q;
        
        for (p=buff,n=0;n<MAXFIELD;p=q+len) {
            if ((q=strstr(p,sep))) *q='\0'; 
            if (*p) v[n++]=atof(p);
            if (!q) break;
        }
        return n;
    }
     
    /* convert ddmm.mm in nmea format to deg -------------------------------------*/
    static double dmm2deg(double dmm)
    {
        return floor(dmm/100.0)+fmod(dmm,100.0)/60.0;
    }
    /* convert time in nmea format to time ---------------------------------------*/
    static void septime(double t, double *t1, double *t2, double *t3)
    {
        *t1=floor(t/10000.0);
        t-=*t1*10000.0;
        *t2=floor(t/100.0);
        *t3=t-*t2*100.0;
    }
     
    /* decode nmea gxrmc: recommended minumum data for gps -----------------------*/
    static int decode_nmearmc(char **val, int n, sol_t *sol)
    {
        double tod=0.0,lat=0.0,lon=0.0,vel=0.0,dir=0.0,date=0.0,ang=0.0,ep[6];
        double pos[3]={0};
        char act=' ',ns='N',ew='E',mew='E',mode='A';
        int i;
        
        trace(4,"decode_nmearmc: n=%d\n",n);
        
        for (i=0;i<n;i++) {
            switch (i) {
                case  0: tod =atof(val[i]); break; /* time in utc (hhmmss) */
                case  1: act =*val[i];      break; /* A=active,V=void */
                case  2: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */
                case  3: ns  =*val[i];      break; /* N=north,S=south */
                case  4: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */
                case  5: ew  =*val[i];      break; /* E=east,W=west */
                case  6: vel =atof(val[i]); break; /* speed (knots) */
                case  7: dir =atof(val[i]); break; /* track angle (deg) */
                case  8: date=atof(val[i]); break; /* date (ddmmyy) */
                case  9: ang =atof(val[i]); break; /* magnetic variation */
                case 10: mew =*val[i];      break; /* E=east,W=west */
                case 11: mode=*val[i];      break; /* mode indicator (>nmea 2) */
                                          /* A=autonomous,D=differential */
                                          /* E=estimated,N=not valid,S=simulator */
            }
        }
        if ((act!='A'&&act!='V')||(ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) {
            trace(2,"invalid nmea gprmc format\n");
            return 0;
        }
        pos[0]=(ns=='S'?-1.0:1.0)*dmm2deg(lat)*D2R;
        pos[1]=(ew=='W'?-1.0:1.0)*dmm2deg(lon)*D2R;
        septime(date,ep+2,ep+1,ep);
        septime(tod,ep+3,ep+4,ep+5);
        ep[0]+=ep[0]<80.0?2000.0:1900.0;
        sol->time=utc2gpst(epoch2time(ep));
        pos2ecef(pos,sol->rr);
        sol->stat=mode=='D'?SOLQ_DGPS:SOLQ_SINGLE;
        sol->ns=0;
        
        sol->type=0; /* postion type = xyz */
        
        trace(5,"decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n",
              time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns,
              vel,dir,ang,mew,mode);
        
        return 2; /* update time */
    }
    /* decode nmea gxzda: utc day,month,year and local time zone offset ----------*/
    static int decode_nmeazda(char **val, int n, sol_t *sol)
    {
        double tod=0.0,ep[6]={0};
        int i;
        
        trace(4,"decode_nmeazda: n=%d\n",n);
        
        for (i=0;i<n;i++) {
            switch (i) {
                case  0: tod  =atof(val[i]); break; /* time in utc (hhmmss) */
                case  1: ep[2]=atof(val[i]); break; /* day (0-31) */
                case  2: ep[1]=atof(val[i]); break; /* mon (1-12) */
                case  3: ep[0]=atof(val[i]); break; /* year */
            }
        }
        septime(tod,ep+3,ep+4,ep+5);
        sol->time=utc2gpst(epoch2time(ep));
        sol->ns=0;
        
        trace(5,"decode_nmeazda: %s\n",time_str(sol->time,0));
        
        return 2; /* update time */
    }
    /* decode nmea gxgga: fix information ----------------------------------------*/
    static int decode_nmeagga(char **val, int n, sol_t *sol)
    {
        gtime_t time;
        double tod=0.0,lat=0.0,lon=0.0,hdop=0.0,alt=0.0,msl=0.0,ep[6],tt;
        double pos[3]={0};
        char ns='N',ew='E',ua=' ',um=' ';
        int i,solq=0,nrcv=0;
     
        trace(4,"decode_nmeagga: n=%d\n",n);
        
        for (i=0;i<n;i++) {
            switch (i) {
                case  0: tod =atof(val[i]); break; /* time in utc (hhmmss) */
                case  1: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */
                case  2: ns  =*val[i];      break; /* N=north,S=south */
                case  3: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */
                case  4: ew  =*val[i];      break; /* E=east,W=west */
                case  5: solq=atoi(val[i]); break; /* fix quality */
                case  6: nrcv=atoi(val[i]); break; /* # of satellite tracked */
                case  7: hdop=atof(val[i]); break; /* hdop */
                case  8: alt =atof(val[i]); break; /* altitude in msl */
                case  9: ua  =*val[i];      break; /* unit (M) */
                case 10: msl =atof(val[i]); break; /* height of geoid */
                case 11: um  =*val[i];      break; /* unit (M) */
            }
        }
        if ((ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) {
            trace(2,"invalid nmea gpgga format\n");
            return 0;
        }
        if (sol->time.time==0.0) {
            trace(2,"no date info for nmea gpgga\n");
            return 0;
        }
        pos[0]=(ns=='N'?1.0:-1.0)*dmm2deg(lat)*D2R;
        pos[1]=(ew=='E'?1.0:-1.0)*dmm2deg(lon)*D2R;
        pos[2]=alt+msl;
        
        time2epoch(sol->time,ep);
        septime(tod,ep+3,ep+4,ep+5);
        time=utc2gpst(epoch2time(ep));
        tt=timediff(time,sol->time);
        if      (tt<-43200.0) sol->time=timeadd(time, 86400.0);
        else if (tt> 43200.0) sol->time=timeadd(time,-86400.0);
        else sol->time=time;
        pos2ecef(pos,sol->rr);
        sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE;
        sol->ns=nrcv;
        
        sol->type=0; /* postion type = xyz */
        
        trace(5,"decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n",
              time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns,
              hdop,ua,um);
        //打印出来：时间、坐标、定位状态、定位卫星数、hdop、
    	//printf("%s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n", time_str(sol->time, 0), sol->rr[0], sol->rr[1], 
    		//sol->rr[2], sol->stat, sol->ns,hdop, ua, um);
    	printf("%s rr=%.7f %.7f %.7f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n", time_str(sol->time, 1), pos[0]*R2D, pos[1]*R2D,
    		pos[2], sol->stat, sol->ns, hdop, ua, um);
     
        return 1;
    }
     
    /* decode nmea ---------------------------------------------------------------*/
    static int decode_nmea(char *buff, sol_t *sol)
    {
        char *p,*q,*val[MAXFIELD]; //指针数组，每一个元素为指针
        int n=0;
        
        trace(4,"decode_nmea: buff=%s\n",buff);
        
        /* parse fields */
        for (p=buff;*p&&n<MAXFIELD;p=q+1) {
            if ((q=strchr(p,','))||(q=strchr(p,'*'))) {
                val[n++]=p; *q='\0';
            }
            else break;
        }
        if (!strcmp(val[0]+3,"RMC")) { /* $xxRMC */
            return decode_nmearmc(val+1,n-1,sol);
        }
        else if (!strcmp(val[0]+3,"ZDA")) { /* $xxZDA */
            return decode_nmeazda(val+1,n-1,sol);
        }
        else if (!strcmp(val[0]+3,"GGA")) { /* $xxGGA */
            return decode_nmeagga(val+1,n-1,sol);
        }
        return 0;
    }
    // 处理函数
    //FILE *fp;
    //extern buff[MAX];
    sol_t sol = {0};
    extern pro_nmea(fp,buff) {
    
    	while (fgets(buff, MAX, fp))
    	{
    		decode_nmea(buff, &sol);
    	}
     
    }

运行结果如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

### 参考： ###

1、[GPS - NMEA sentence information][]

2、[http://www.leapsecond.com/java/gpsclock.htm][http_www.leapsecond.com_java_gpsclock.htm]

[20201027165032289.png]: /images/20221120/ed173bfcd40443da8660c2a7c5b4cf06.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20221120/7915964a9f394c6980bc5e4977d5f33c.png
[GPS - NMEA sentence information]: http://aprs.gids.nl/nmea/
[http_www.leapsecond.com_java_gpsclock.htm]: http://www.leapsecond.com/java/gpsclock.htm