车辆自动采点

一、整体思路：
车辆自动踩点需要获取千寻差分数据，差分数据有两种格式：分别是GPGGA和GRHDT，GPGGA内存在车辆经纬度定位信息，GRHDT内存在转向角信息，分别进行关键数据提取，然后将提取的数据存放在创建的gps_road.txt中
1.main.c中

/*int main(int argc,char *argv[])中的argc是输入数组argv[]的数的个数，输入的数都打印在argv[]中*/
int main(int argc, char *argv[])
{
    
    p = fopen("gps_load.txt", "w");//第一是打开的文件，第二是打开模式。w是指打开只写文件，若文件存在则文件长度清为零，即该文件内容会消失；若文件不存在则创建该文件
    //memset(tmp,0,sizeof(tmp));

    pthread_t RecvFromIMU_tid;
    pthread_t AnalysisIMU_tid;

    int ret;

    ret = pthread_create(&RecvFromIMU_tid, NULL, RecvFromIMU_pthread, NULL);
    /*pthread_create()线程的四个参数分别为：pthread_t类型的参数，线程属性，线程新创建函数，线程函数参数，无参数时设为NULL即可。有参数时输入参数的地址。当多于一个参数时应当使用结构体传入。（以下举例）
    返回值：成功返回0，不成功返回错误码*/
	if (ret != 0)//
	{
		printf("Create thread RecvFromIMU_pthread failed\n");
	}
	ret = pthread_create(&AnalysisIMU_tid, NULL, AnalysisIMU_pthread, NULL);
	if (ret != 0)
	{
		printf("Create thread AnalysisIMU_pthread failed\n");
	}
    pthread_join(RecvFromIMU_tid, NULL);
    pthread_join(AnalysisIMU_tid, NULL);

    return 0;
}

2.创建IMU数据接收线程RecvFromIMU_pthread

void *RecvFromIMU_pthread(void *args)//线程函数
{
    memset(g_RecvDataFromRTK,0,sizeof(g_RecvDataFromRTK));//接收RTK数据数组初始化
    /*pfile=fopen("LOG.txt","a");
    if(pfile==NULL){
        printf("打开文件写LOG失败\n");
    }*/
    if(IsInit==false)//bool类型，IsInit的初始值是false
    {
        if(initialize()<0)
        {
            printf("MC: 打开串口配置参数失败\n");
            return 0;
        }
        IsInit=true;//执行到这一步说明initialize()不小于0，说明打开串口配置参数成功，使bool值为true
        printf("MC: 打开串口配置参数成功\n");
    }
    //return 0;
	int nBytes_0 = 0;
	int nBytes_1 = 0;
    while(1)
    {
        usleep(100000);
        memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));//准备接收串口数据，串口接收数据数组初始化
		//memcpy(g_RecvDataFromRTK_0, g_RecvDataFromRTK_1, sizeof(g_RecvDataFromRTK_1));
		nBytes_1 = read(g_fd, g_RecvDataFromRTK_1, 200);//从串口读数据，从g_fd标志位指向的串口中读数据前200位到g_RecvDataFromRTK_1中
        //printf("g_RecvDataFromRTK_1:%s\n",g_RecvDataFromRTK_1);
        //continue;
		//printf("nBytes0=%d,nBytes=%d\n", nBytes_0, nBytes_1);
		if (g_RecvDataFromRTK_1[0] == '$')//从这判断数据格式对不对
		{
            //printf("%s\n",g_RecvDataFromRTK_0);
			char *ret;
            int i;
            int gps_num = 0;
            for(i = 0;i < 200;i++)//一共可能有200个字节的数据
            {
                if(g_RecvDataFromRTK_1[i]=='*')//因为有两段数据，分别是GPGGA和GRHDT，两段数据分别有*
                {
                    gps_num++;//取出两个*之后说明有两段数据，数据段+1
                }
                if(gps_num==2)//如果有两段数据，退出循环
                {
                    break;
                }                  
            }
            if(i!=200)
            {
                process_len = i;
                //printf("g_RecvDataFromRTK_1:%s\n",g_RecvDataFromRTK_1);
                for(int j=0;j<i;j++)
                {
                    g_DataCombine[j] = g_RecvDataFromRTK_1[j];
                }
                pthread_mutex_lock(&gMutex_SC_data);//线程锁
				memcpy(g_ProcDataFromSCBuf, g_DataCombine, DATALEN);
				pthread_cond_signal(&gCond_SC_data);//线程信号触发
				pthread_mutex_unlock(&gMutex_SC_data);
            }							
		}
            //break;
    }
    return 0;
}

3.差分数据解析线程AnalysisIMU_pthread

void *AnalysisIMU_pthread(void *args)
{
    char localproc_SCData[200] = {0};
    int localproc_len=0;
    int num_gga=0;
    int num_dht=0;
	//memset(&g_IMUdata, 0, sizeof(IMUdata));
    while(1)
    {
        
        pthread_mutex_lock(&gMutex_SC_data); 
        pthread_cond_wait(&gCond_SC_data,&gMutex_SC_data);//线程等待
        memcpy(localproc_SCData,g_ProcDataFromSCBuf,process_len) ;//接收的差分数据存在localproc_SCData数组中
        localproc_len=process_len;//接收差分数据的长度
        num_gga=0;
        num_dht=0;
        //printf("接收到%d个字节\n",localproc_len);
        pthread_mutex_unlock(&gMutex_SC_data);

        double g_Lattitude_tmp =0;
        double g_Longitude_tmp =0;
        double g_Heading_tmp=0;
        int rtk_num = 0;

        char *p_gpgga = strstr(localproc_SCData,"GPGGA");
        char *p_gpdht = strstr(localproc_SCData,"GNHDT");
        int gpgga_len = 60;
        int gpdht_len = 20;
        //printf("p_gpgga:%s\n",p_gpgga);
        //printf("p_gpdht:%s\n",p_gpdht);
        //printf("gpgga_len:%d\n",gpgga_len);
        //printf("gpdht_len:%d\n",gpdht_len);
        //continue;
        for(int i=0;i<gpgga_len;i++)
        {
            if(p_gpgga[i]==0x2c)//0x2c代表','；遍历数组，如果遍历到','的时候num_gga+1
            {
                num_gga++;
                if(num_gga == 2)//num_gga到达2的时候说明在经度位上，现在就是要把它取出来
                {
                    for(int j=i+1;;j++)
                    {
                        if(p_gpgga[j]==0x2c)//","后一位开始遍历，到下一个","为止退出循环
                        {
                            break;
                        }
                        else if(p_gpgga[j]!=0x2e)//如果不等于"."时候说明不是最后两位，说明可以把数据段取出来
                        {
                            g_Lattitude_tmp*=10;
                            g_Lattitude_tmp+=(p_gpgga[j]-48);
                        }
                    }
                    //
                }
                else if(num_gga == 4)
                {
                    for(int j=i+1;;j++)
                    {
                        if(p_gpgga[j]==0x2c)
                        {
                            break;
                        }
                        else if(p_gpgga[j]!=0x2e)
                        {
                            g_Longitude_tmp*=10;
                            g_Longitude_tmp+=(p_gpgga[j]-48);
                        }
                    }
                    //
                    //break;
                }
                else if(num_gga == 6)
                {
                    rtk_num = (p_gpgga[i+1]-48);
                    
                    break;
                }
            }
        }

        for(int i=0;i<gpdht_len;i++)
        {
            if(p_gpdht[i]==0x2c)
            {
                num_dht++;
                if(num_dht == 1)
                {
                    for(int j=i+1;;j++)
                    {
                        if(p_gpdht[j]==0x2c)
                        {
                            break;
                        }
                        else if(p_gpdht[j]!=0x2e)
                        {
                            g_Heading_tmp*=10;
                            g_Heading_tmp+=(p_gpdht[j]-48);
                        }
                    }
                    //
                    break;
                }                    
            }
        }
        g_Lattitude_tmp/=10000000000;
        g_Longitude_tmp/=10000000000;
        g_Heading_tmp/=10000;

        double lat_int,lat_fraction;
        double lon_int,lon_fraction;
        lat_fraction = modf(g_Lattitude_tmp,&lat_int)*100;
        lon_fraction = modf(g_Longitude_tmp,&lon_int)*100;
        g_Lattitude = lat_int+lat_fraction/60;
        g_Longitude = lon_int+lon_fraction/60;
        g_Heading = g_Heading_tmp;
        //printf("g_Lattitude:%lf  ",g_Lattitude);
        //printf("g_Longitude:%lf  ",g_Longitude);
        //printf("g_Heading:%lf\n",g_Heading);

        if(file_point_num == 0)//把第一次数据赋给下一次，但是只有一次
        {
            last_g_Lattitude = g_Lattitude;
            last_g_Longitude = g_Longitude;
            last_g_Heading = g_Heading;
            file_point_num++;
        }
        else
        {
            double distance = ntzx_GPS_length(last_g_Longitude,last_g_Lattitude,
            g_Longitude,g_Lattitude);
            if(distance >= 0.8)
            {
                //$GPFPD,2196,439607.060,266.245,0.522,0.414,32.0304055,120.9151016,16.56,-0.156,-0.004,0.002,5.136,12,27,45*4F
                fprintf(p,"$GPFPD,2196,439607.060,%lf,0.522,0.414,%lf,%lf,16.56,-0.156,-0.004,0.002,5.136,12,27,%d5*4F\n",
                g_Heading,g_Lattitude,g_Longitude,rtk_num);
                //fprintf(p,"$GPFPD,1451,368123.30,%lf,0.128,1.579,%lf,%lf,394.98,-0.157,0.019,-0.345,3.898,7,8,1*7B\n",
                //g_Heading,g_Lattitude,g_Longitude);
                fflush(p);
                last_g_Lattitude = g_Lattitude;
                last_g_Longitude = g_Longitude;
                last_g_Heading = g_Heading;
                file_point_num++;
                printf("hit!\n");
            }
        }
    }
    return 0;
}