本文介绍了一个车辆自动踩点系统，该系统通过获取千寻差分数据，利用GPGGA和GRHDT两种格式的信息进行车辆定位和转向角计算。系统采用多线程实现，包括数据接收线程和解析线程，从串口读取RTK数据，解析经纬度和转向角，并将有效数据写入gps_road.txt文件。在解析过程中，针对GPGGA和GRHDT数据格式进行解析，提取关键地理位置信息并进行坐标转换。
一、整体思路：
车辆自动踩点需要获取千寻差分数据，差分数据有两种格式：分别是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;
}
扫一扫
2065
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
