自动驾驶之小巴自动循迹全过程详解

一、小巴底盘惯导数据接收、解析、上传
1.在serial.cpp中，实现惯导差分数据接收和解析
（1）惯导差分数据从哪里来？
惯导差分数据可以参考之前串口收发和星网宇达官方sdk融合，融合之后就可以正常接收差分GPFPD数据，GPFPD数据举例如下所示：

$GPFPD,2199,118326.980,264.364,0.635,1.458,32.0303810,120.9144455,16.85,-0.007,0.000,0.001,4.135,5,26,45*56

以上为差分定位GPFPD数据，里面包含经度，纬度、转向角。
（2）惯导接收线程分析
惯导接收线程在底盘上，在serial.c文件中，惯导数据是通过串口收发的，可以用于读取硬件设备的串口数据，并对于数据进行判断。

//初始化星网宇达惯导
int initialize()
{
    struct termios oldtio,newtio; 
    g_fd=open("/dev/ttyUSB0", O_RDWR|O_NOCTTY); //打开串口/dev/ttyUSB0,可读可写|非阻塞，返回一个标志位g_fd
    if(g_fd<0)  { //如果标志位g_fd小于零，就说明没有打开串口
        printf("MC: open port fail!\n");        
        return -1;
    } 
    set_speed(g_fd,115200);//设置串口波特率

    if (set_parity(g_fd,8,1,'N') == false)  {//设置串口停止位
		printf("Set Parity Error\n");
		return -1;
	}
    return 0; 
    fcntl(g_fd,F_SETFL,0);
    /*将目前终端机的结构保存至oldtio结构*/  
    tcgetattr(g_fd,&oldtio);  
    /*清除newtio结构，重新设置通信协议*/  
    bzero(&newtio,sizeof(newtio));  
    /*通信协议设为8N1,8位数据位,N没有效验,1位结束位*/  
    newtio.c_cflag = BAUDRATE | CS8 | CLOCAL| CREAD;  
    newtio.c_iflag = IGNPAR;  
    newtio.c_oflag = 0;  
    /*设置为正规模式*/  
    //newtio.c_lflag=ICANON;
    newtio.c_lflag=0;
    newtio.c_cc[VTIME]=0;
    newtio.c_cc[VMIN]=69; 
    /*清除所有队列在串口的输入*/  
    tcflush(g_fd,TCIFLUSH);  
     /*新的termios的结构作为通信端口的参数*/  
    tcsetattr(g_fd,TCSANOW,&newtio); 
}


int g_fd=-1;
bool IsInit=false
unsigned char g_RecvDataFromRTK_1[300];
//进入惯导接收线程
void *RecvFromIMU_pthread(void *args)
{
    memset(g_RecvDataFromRTK,0,sizeof(g_RecvDataFromRTK));//初始化惯导接收数组
    pfile=fopen("LOG.txt","a");//包含了要打开的文件名称,打开文件模式。a代表的意思是以附加的方式打开只写文件。若文件不存在，则会创建该文件；如果文件存在，则写入的数据会被加到文件尾后，即文件原先的内容会被保留（EOF 符保留）
    //返回值：如果正确就返回指向该流的文件指针，错误就返回NULL
    if(pfile==NULL){
        printf("打开文件写LOG失败\n");
    }
    if(IsInit==false)
    {
        if(initialize()<0)
        {
            printf("MC: 打开串口配置参数失败\n");
            return 0;
        }
        IsInit=true;
        printf("MC: 打开串口配置参数成功\n");
    }
    //return 0;
	memset(&g_RecvDataFromRTK_0, 0, sizeof(g_RecvDataFromRTK_0));
	memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));//初始化g_RecvDataFromRTK_1数组
	int nBytes_0 = 0;
	int nBytes_1 = 0;//读取的字节数
    while(1)
    {
		memset(&g_RecvDataFromRTK_0, 0, sizeof(g_RecvDataFromRTK_0));//车顶左右有两个天线，分别接收不同数据
        memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));
		//memcpy(g_RecvDataFromRTK_0, g_RecvDataFromRTK_1, sizeof(g_RecvDataFromRTK_1));
		nBytes_0 = nBytes_1;
		nBytes_1 = read(g_fd, g_RecvDataFromRTK_1, 200);//将g_fd指向的地址里面前200字节内容读到g_RecvDataFromRTK_1[]中
		//printf("%s\n",g_RecvDataFromRTK_0);
        //printf("g_RecvDataFromRTK_1:%s\n",g_RecvDataFromRTK_1);
        //printf("%d\n",strlen(g_RecvDataFromRTK_1));
        usleep(10000);
		//printf("nBytes0=%d,nBytes=%d\n", nBytes_0, nBytes_1);
        
		if (g_RecvDataFromRTK_1[0] == '$')
		{
			char *ret;
            int i;
            //相当于确定一下数据全不全，从符号'$'到符号'*'表示接收的GPFPD数据有效
            //示例：$GPFPD,2199,118326.980,264.364,0.635,1.458,32.0303810,120.9144455,16.85,-0.007,0.000,0.001,4.135,5,26,45*56
            for(i = 0;i < 200;i++)
            {
                if(g_RecvDataFromRTK_1[i]=='*')
                {
                    break;
                }                  
            }
            if(i > 70&&i!=200)//说明这串GPFPD数据长度为70-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);//至此就接收到从/dev/ttyUSB0串口传送过来的GPFPD差分数据
				pthread_cond_signal(&gCond_SC_data);//线程信号触发
				pthread_mutex_unlock(&gMutex_SC_data);
            }							
		}
    }
    return 0;
}

(3)惯导解析线程分析
其中从惯导接收线程中有一个信号量跳转到惯导解析线程，主要作用就是解析一串惯导数据，将其中关键的经纬度和转向角信息提取出来。

void *AnalysisIMU_pthread(void *args)
{
  unsigned char localproc_SCData[200];
  int localproclen = 0;
  int num = 0;
  memset(&g_IMUdata, 0, sizeof(IMUdata));
  while(1)
    {
        #ifdef Starneto
            pthread_mutex_lock(&gMutex_SC_data); 
            pthread_cond_wait(&gCond_SC_data,&gMutex_SC_data);
            memcpy(localproc_SCData,g_ProcDataFromSCBuf,process_len) ;
            localproc_len=process_len;
            num=0;
            //printf("接收到%d个字节\n",localproc_len);
            pthread_mutex_unlock(&gMutex_SC_data);
            //以上完成差分GPFPD惯导数据g_ProcDataFromSCBuf[]复制到localproc_SCData[]中
            double g_Lattitude_tmp =0;
            double g_Longitude_tmp =0;
            double g_Heading_tmp=0;
            for(int i=0;i<localproc_len;i++)
            {
                if(localproc_SCData[i]==0x2c)
                {
                    num++;
                    if(num==3)
                    {
                        for(int j=i+1;;j++)
                        {
                            if(localproc_SCData[j]==0x2c)
                            {
                                break;
                            }
                            if(localproc_SCData[j]!=0x2e)
                            {
                                g_Heading_tmp*=10;                                g_Heading_tmp+=(localproc_SCData[j]-48);
                            }
                       
                        }
                        g_Heading_tmp/=1000;                       //printf("g_Heading----------%f\n",g_Heading);
                    }
                    if(num==6)
                    {
                        for(int j=i+1;;j++)
                        {                            if(localproc_SCData[j]==0x2c)
                            {
                                break;
                            }                            if(localproc_SCData[j]!=0x2e)
                            {                                g_Lattitude_tmp*=10;                                g_Lattitude_tmp+=(localproc_SCData[j]-48);
                            }
                        }                        g_Lattitude_tmp/=10000000;                        //printf("g_Lattitude----------%f\n",g_Lattitude);
                    }
                    if(num==7)
                    {
                        for(int j=i+1;;j++)
                        {                    if(localproc_SCData[j]==0x2c)
                            {
                                break;
                            }                           if(localproc_SCData[j]!=0x2e)
                            {                              g_Longitude_tmp*=10;                              g_Longitude_tmp+=(localproc_SCData[j]-48);
                            }
                        }                  g_Longitude_tmp/=10000000;                       //printf("g_Longitude----------%f\n",g_Longitude);
                    }
                }
            }
            g_Lattitude = g_Lattitude_tmp;
            g_Longitude = g_Longitude_tmp;
            g_Heading = g_Heading_tmp;
}
return 0;
}

g_Lattitude、g_Longitude、g_Heading这三个是全部变量，如下：

float g_Heading = 0;
double g_Lattitude = 0;
double g_Longitude = 0;

2.在UDP_broadcasst.cpp中，实现惯导差分数据通过UDP广播
（1）发送惯导差分定位信息线程

void *SendCarState_pthread(void *args)
{
    memset(&gMC_state, 0, sizeof(gMC_state));//车辆状态信息结构体初始化
	memset(&gMC_NAV, 0, sizeof(gMC_NAV));//惯导差分定位信息结构体初始化
	memset(&gMC_DRIVE, 0, sizeof(gMC_DRIVE));//导航信息结构体初始化
    while (1)
	{
	unsigned char sum1 = 0;
		unsigned char *pBuf1 = (unsigned char *)&gMC_state;
		for (int i = 0; i < gMC_state.nSize - 1; i++)
		{
			sum1 += pBuf1[i];
		}
		gMC_state.checksum = sum1;
		//填写gMC_NAV结构体
		gMC_NAV.Head = 0x0824;
		gMC_NAV.navID = NAV;
		gMC_NAV.Latitude_degree = g_Lattitude;
		gMC_NAV.Longitude_degree = g_Longitude;
		gMC_NAV.Yaw_rad = g_Heading;
		gMC_NAV.Speed_cm_ps = (g_vcu_vstate1.speed - 50) / 3.6 * 100;
		gMC_NAV.nSize = sizeof(gMC_NAV);
		UDP_send_data();
		usleep(20000);
	}
}

unsigned int NAV = 0;
bool initial = false;//布尔类型
int brdcFd;//设置标志位
struct sockaddr_in Addr;//定义一个sockaddr_in对象
int UDP_send_data()
{
  int sendbytes = 0;
  if(initial == false)
  {
     initial = true;
     if((brdcFd = socket(AF_INET,SOCK_DGRAM,0)) == -1)
     {
        printf(:fail to create socket\n");
        return -1;
     }
     int optval = 1;
     setsockopt(brdcFd,SOL_SOCKET, SO_BROADCAST | SO_REUSEADDR, &optval, sizeof(int));//参数依次为：标识一个套接口的套接字；选项定义的层次，支持SOL_SOCKET、IPPROTO_TCP、IPPROTO_IP和IPPROTO_IPV6；需设置的选项；指针，指向存放选项待设置的新值得缓冲区；optval缓冲区长度。
     memset(&Addr, 0, sizeof(struct sockaddr_in));
     Addr.sin_family = AF_INET;//IPV4因特网域
     Addr.sin_port = htons(port_out);//端口号：50001
     Addr.sin_addr.s_addr = htonl(INADDR_BROADCAST);//IP地址  
  }
 sendbytes = sendto(brdcFd, &gMC_NAV, sizeof(gMC_NAV), 0, (struct sockaddr *)&Addr, sizeof(struct sockaddr));
//SendTo是一个计算机函数，指向一指定目的地发送数据，SendTo()适用于发送未建立连接的UDP数据包 
//s：套接字
//buff：待发送数据的缓冲区
//size：缓冲区长度
//Flags：调用方式标志位, 一般为0, 改变Flags，将会改变Sendto发送的形式
//addr：（可选）指针，指向目的套接字的地址
//len_addr所指地址的长度
//返回值：整型。如果成功，则返回发送的字节数，失败则返回SOCKET_ERROR。 
}

二、PL模块接收底盘广播的三大数据分别是经度、纬度、转向角。
1.在WRC_main.cpp中，有底盘惯导数据接收线程，UDP惯导数据处理线程和PL路径规划线程
（1）底盘惯导数据接收线程

pthread_t g_ThreadUDPRecvHandle;
pthread_create(&g_ThreadUDPRecvHandle, NULL, WRC_UDPRECV, NULL);
pthread_join(g_ThreadUDPRecvHandle, NULL);

void *WRC_UDPRECV(void *args)
{
   char buffTEMP[UDP_BUFFERSIZE]={0};
    struct sockaddr_in addrto;
    bzero(&addrto,sizeof(struct sockaddr_in));
    addrto.sin_family=AF_INET;
    addrto.sin_addr.s_addr=htonl(INADDR_ANY);	//addrto.sin_addr.s_addr=htonl(0C0A804BC);
    addrto.sin_port=htons(50001);
    socklen_t len=sizeof(addrto);
    WRC_MC_NAV_INFO navInfo;
    WRC_MC_DRIVE_INFO driveInfo;
    int nWrite;
    int sock=-1;

    if((sock=socket(AF_INET,SOCK_DGRAM,0))==-1)
    {
        printf("                    socket error...");
        return NULL;
    }
    int flag=1;
    setsockopt(sock,SOL_SOCKET,SO_REUSEADDR,(char*)&flag,sizeof(flag));
    if(bind(sock,(struct sockaddr*)&(addrto),len)==-1)
    {
        printf("           UDPRECV:bind error..");
        return NULL;
    }
    fd_set readfd;
    struct timeval timeout;
    while(1)
    {
        timeout.tv_sec = 10;//超时时间10s
		timeout.tv_usec = 0;

		//套节字集合清空
		FD_ZERO(&readfd);
		//将套接字描述符加入到文件描述符集合
		FD_SET(sock, &readfd);
		int ret = select(sock + 1, &readfd, NULL, NULL, &timeout); //侦听是否可读
        switch (ret)
		{
		case -1: //发生错误
			printf("                UDPCLENT：SOCKET select error:");
			break;
        case 0:
            printf("             UDPCLENT:UDP recv time out!\n");
            break;
        default:
            //printf("                        broad recv............\n");
            //if (FD_ISSET(sock, &readfd))//检查套接字sock是否在集合readfd当中
			//{
               // printf("             1 2345................\n");
                int nBytes=recvfrom(sock,buffTEMP,UDP_BUFFERSIZE,0,(struct sockaddr*)&addrto,&len);
				//printf("nBytes: %d\n",nBytes);
                if(nBytes<=0)
                {
                    printf("                   UDPRECV:recv bytes error..");
                }
                else
                {
                    pthread_mutex_lock(&gMutex_MC_INFO);
                    memcpy(g_temp_NAV_info.pRecvBuff,buffTEMP,UDP_BUFFERSIZE);                g_temp_NAV_info.buffersize=nBytes;
					//printf("%d\n",g_temp_NAV_info.buffersize);
					pthread_cond_signal(&gCond_MC_INFO); 
					pthread_mutex_unlock(&gMutex_MC_INFO);                                           
                }
            break;
        }
    }
    return NULL;
}

（2）底盘惯导数据处理线程

pthread_t g_ThreadUDPProcHandle;
pthread_create(&g_ThreadUDPProcHandle, NULL, WRC_UDPPROC, &gParm);
pthread_join(g_ThreadUDPProcHandle, NULL);

void *WRC_UDPPROC(void *lParam )
{
	//
	//return 0;
    WRC_Parameter *pParam = (WRC_Parameter*)lParam;
    unsigned char buffTEMP[UDP_BUFFERSIZE]={0};
    int buffersize;
    WRC_MC_NAV_INFO tmpNav;
    memset(&tmpNav,0,sizeof(WRC_MC_NAV_INFO));
	/**/
	struct timeval now;
	WRC_IP_TIME nowTime;
    

    while(1)
    {
		
        WRC_MC_STATE_INFO *pState = NULL;
        WRC_MC_NAV_INFO *pNav = NULL;
        WRC_MC_DRIVE_INFO *pDrive = NULL;
        pthread_mutex_lock(&gMutex_MC_INFO);
        pthread_cond_wait(&gCond_MC_INFO,&gMutex_MC_INFO);
        memcpy(buffTEMP,g_temp_NAV_info.pRecvBuff,UDP_BUFFERSIZE);
        buffersize=g_temp_NAV_info.buffersize;
        pthread_mutex_unlock(&gMutex_MC_INFO);
        //解码
        WRC_MC_Decode_IP_Data(buffTEMP,buffersize,&pState,&pNav,&pDrive);
        //放入队列
        if(pNav!=NULL)
        {


			gettimeofday(&now,NULL);
			nowTime.ws=now.tv_sec/10000;
			nowTime.ms=(now.tv_sec%10000)*1000+now.tv_usec/1000;
            pthread_mutex_lock(&(pParam->g_MutMCNav));
            memcpy(&(pParam->g_NavInfo),pNav,sizeof(WRC_MC_NAV_INFO));
            pParam->g_iNavID=pNav->navID;
			//printf("WRC_UDPPROC\n");
			pthread_cond_signal(&(pParam->g_CondMcToFUNav));//TO FU
            pthread_mutex_unlock(&(pParam->g_MutMCNav));
			
			/*outNavFile<<pNav->navID<<" ";
			outNavFile<<"GPS状态："<<(unsigned short)pNav->GPS_Status<<" ";
			outNavFile<<"经度："<<pNav->Longitude_degree<<" ";
			outNavFile<<"纬度："<<pNav->Latitude_degree<<" ";
			outNavFile<<pNav->Yaw_rad<<" ";
			outNavFile<<pNav->EarthRefCoord[0]<<" ";
			outNavFile<<pNav->EarthRefCoord[1]<<" ";
			//outNavFile<<pNav->navID<<"--";
			
			outNavFile<<nowTime.ws<<"_"<<nowTime.ms<<" ";
			outNavFile<<"　卫星数："<<(unsigned short)pNav->GpsSol_numSV<<" ";
			outNavFile<<"　方差_X："<<pNav->GpsSol_ecefX_cm<<" ";
			outNavFile<<"　方差_Y："<<pNav->GpsSol_ecefY_cm<<" ";
			outNavFile<<"　方差_Z："<<pNav->GpsSol_ecefZ_cm<<endl;*/

			// outNavFile<<pNav->Yaw_rad<<" ";
			// outNavFile<<now.tv_sec<<" ";
			// outNavFile<<now.tv_usec<<" ";
			// outNavFile<<pNav->GpsSol_ecefX_cm<<" ";
			// outNavFile<<pNav->GpsSol_ecefY_cm<<" ";
			// outNavFile<<pNav->GpsSol_ecefZ_cm<<endl;
            //printf("WRC_UDPPROC:%d %d %f %f %f %f %f\n",pNav->navID,pNav->GPS_Status,pNav->Longitude_degree,
                  //pNav->Latitude_degree,pNav->EarthRefCoord[0],pNav->EarthRefCoord[1],pNav->Yaw_rad); 
	
		
		}
        //
        if(pDrive!=NULL)
        {
            pthread_mutex_lock(&(pParam->g_MutMCDrive));
            memcpy(&(pParam->g_DriInfo),pDrive,sizeof(WRC_MC_DRIVE_INFO));
            pthread_mutex_unlock(&(pParam->g_MutMCDrive));
			
			
            // printf("%f %d\n",pDrive->Real_angle_degree,pDrive->Real_Speed_cm_ps);
        }

		if(pState!=NULL)
        {
			/*2018-12-4 tianxuemu 增加MC的state结构体拷贝 */
			pthread_mutex_lock(&(pParam->g_MutMCState));
            memcpy(&(pParam->g_StaInfo),pState,sizeof(WRC_MC_STATE_INFO));
            pthread_mutex_unlock(&(pParam->g_MutMCState));

			
        }


    }
	
    return NULL;
}

（3）PL线程

pthread_t g_ThreadPLHandle;
pthread_create(&g_ThreadPLHandle, NULL, ThreadPL, &gParm);
pthread_join(g_ThreadPLHandle, NULL);

在C_WRC_PL.cpp中

void *ThreadPL(void *param)
{ memset(&g_pMoRoadToPL,0,sizeof(WRC_MO_Road_To_PL);
FILE *fp_gps = fopen("gps_road.txt","r");
if(fp_gps == NULL)
{
  printf("open file error");
  return 0;
}
char mystring [200];
    memset(mystring,'\0',200);//初始化mystring
    int point_num = 0;
    while(fgets(mystring , 200 , fp_gps))//把fp_gps里面的数据每一行取出来给mystring
    {
        //printf("%s\n",mystring);
        string line_string(mystring);//一行一行GPFPD数据提取出来
        //printf("%s\n",line_string.c_str());

        
        int position = 0;
        string lat_s,lon_s;
        for(int i = 0;i < 7; i++)
        {
            
            position = line_string.find(",");//找出这条数据中出现，的位置
            lat_s = line_string.substr(i,position);//取0到长度为position的数据
            line_string.erase(0,position+1);//删除从0到position+1之间的字符
        }
        //printf("%s\n",lat_s.c_str());
        
        position = line_string.find(",");
        lon_s = line_string.substr(0,position);//到i=7时，正好拿到经度数据

        //printf("%s\n",lon_s.c_str());
        
        double d_lat = atof(lat_s.c_str());
        double d_lon = atof(lon_s.c_str());
        

        g_pMoRoadToPL.GPSPointQueue[point_num].LatitudeInfo = d_lat;
        g_pMoRoadToPL.GPSPointQueue[point_num].LongitudeInfo = d_lon;
        printf("%f,%f\n",g_pMoRoadToPL.GPSPointQueue[point_num].LatitudeInfo,g_pMoRoadToPL.GPSPointQueue[point_num].LongitudeInfo);

        if(0 == point_num)
        {
            g_pMoRoadToPL.GPSPointQueue[point_num].type = StartPoint;
        }
        else if(5 == point_num)
        {
            g_pMoRoadToPL.GPSPointQueue[point_num].type = FinalPoint;
        }
        else
        {
            g_pMoRoadToPL.GPSPointQueue[point_num].type = NormalPoint;
        }
        
        g_pMoRoadToPL.GPSPointQueue[point_num].idealSpeedKMH = 5;

        point_num++;
    }
    printf("point_num:%d\n",point_num);
    //return 0;
    
    string line = "$GPFPD,1451,368123.30,124.245,0.128,1.579,34.1966004,108.8551924,394.98,-0.157,0.019,-0.345,3.898,7,8,1*7B\r\n"; 
                 /*$gpfpd,week,time     ,heading,pitch,roll ,latitude  ,longitude  ,9     ,10    ,11   ,12    ,13   ,14,15,16*/
    



    WRC_Parameter* pParam=(WRC_Parameter*)param;
    pthread_t g_RecvFromFuHandle;
    pthread_t g_PLProcHandle;
    pthread_t g_SendTOMCHandle;
    
 

    //WRC_RecvFromSC(pParam);

    // YJ_V2X_UDP V2X_UDP;
    // V2X_UDP.UDPRecvFromMo();

    //创建接收MO的线程
    //gTcp.TcpRecvFromMo("MO");
    //WRC_UDPRecvFromMo();
    //创建接收FU数据的线程
    //LOG("PL : Now create a thread that accepts FU data\n");
    // printf("%s","PL : Now create a thread that accepts FU data\n");
    //pthread_create(&g_RecvFromFuHandle,NULL,RecvFromFU,pParam);
    //创建PL的处理线程
    //LOG("PL : Now create a thread that processes the PL\n");
    // printf("%s","PL : Now create a thread that processes the PL\n");
    
    g_pMoRoadToPL.GpsPointNum = point_num;
    g_MoRoadToPL.GpsPointNum = point_num;
    printf("g_MoRoadToPL.GpsPointNum: %d\n",g_pMoRoadToPL.GpsPointNum);
    printf("g_pMoRoadToPL.size: %d\n",g_pMoRoadToPL.size);
    g_pMoRoadToPL.size = sizeof(WRC_MO_Road_To_PL);
    printf("g_pMoRoadToPL.size: %d\n",g_pMoRoadToPL.size);

   
    
    pthread_create(&g_PLProcHandle,NULL,PL_ProcThread,pParam);
    
    //return 0;
    //创建发往MC的线程
    //LOG("PL : Now create a thread that send the result to MO");
    pthread_create(&g_SendTOMCHandle,NULL,ThreadToMC,NULL);
pthread_join(g_RecvFromFuHandle,NULL);
pthread_join(g_PLProcHandle,NULL);
    //sleep(2);
    return 0;
}

（4）正式进入PL算法线程，比对路径点循迹

pthread_t g_PLProcHandle;
pthread_create(&g_PLProcHandle,NULL,PL_ProcThread,pParam);//pParam是线程传入的参数
pthread_join(g_PLProcHandle,NULL);

进入PL_ProcThread线程中

//
//  规划发送至底层的数据
//


typedef struct tagWRC_PL_TO_MC
{
	UINT16 Head; // 数据帧头  固定值 0x0624
	UINT32 FrameID;//规划的帧号(从0开始)
	//NJUST_IP_TIME synTime; //时刻	//规划时刻
	UINT8 mode; // 0:轨迹预瞄 1：轨迹 2: 编队模式 3: 直控模式
	//UINT
	INT16 Keep_dist_cm; // 跟随模式下 期望距离     直控模式下   方向盘角度-8000--8000 （0.1°） 
	INT16 Real_dist_cm; //跟随模式下 当前距离      直控模式下   刹车0-1000
	INT16 Obj_speed_cm_s; //跟随模式下 前车速度     直控模式下   油门0-1000
	
	
	UINT8 resv; // 预留
	unsigned int navID;  //与图像获取时间最接近的导航数据编号(从0开始)
	UINT8 drvCmd; //动作命令 见"动作"
	UINT16  IO_Cmd; // IO控制命令  
	UINT8 nPoint; //有效点个数
	INT16 speed_cm_ps; //规划速度,单位:厘米/秒
	WRC_PL_PATH_POINT pPoint[WRC_MAX_PL_PATH_POINTS_NUM]; //车体坐标系下的路径点
	INT32 courseAngleRad; //航向角,单位:0.001,千分之一弧度
	INT32 earthCoordX_cm;     //  北向  +X 
	INT32 earthCoordY_cm;     //  东向  +Y
	UINT16 nSize; //结构体大小 
	UINT8 checksum;  //Checksum of above data.
}WRC_PL_TO_MC;

static WRC_PL_TO_MC g_PLResultToMC;
void * PL_ProcThread(void* param)
{
  WRC_Parameter* pParam = (WRC_Paramter*)param;//惯导线程处理函数参数
 memset(&g_PLResultToMC,0,sizeof(WRC_PL_TO_MC));//初始化g_PLResultToMC数组 
  
}