一、创建和打开gps_Road.txt文件,准备往里写数据
FILE *p = NULL;
p = fopen("gps_Road.txt", "w");
二、创建GPFPD数据接收线程
pthread_t RecvFromIMU_tid;
pthread_create(&RecvFromIMU_tid, NULL, RecvFromIMU_pthread, NULL);
pthread_join(RecvFromIMU_tid, NULL);
int initialize()
{
struct termios oldtio,newtio;
g_fd=open(UART_NAME, O_RDWR|O_NOCTTY);
if(g_fd<0) {
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);
tcgetattr(g_fd,&oldtio);
bzero(&newtio,sizeof(newtio));
newtio.c_cflag = BAUDRATE | CS8 | CLOCAL| CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
newtio.c_lflag=0;
newtio.c_cc[VTIME]=0;
newtio.c_cc[VMIN]=69;
tcflush(g_fd,TCIFLUSH);
tcsetattr(g_fd,TCSANOW,&newtio);
}
void *RecvFromIMU_pthread(void *args)
{
memset(g_RecvDataFromRTK,0,sizeof(g_RecvDataFromRTK));
if(IsInit==false)
{
if(initialize()<0)
{
printf("MC: 打开串口配置参数失败\n");
return 0;
}
IsInit=true;
printf("MC: 打开串口配置参数成功\n");
}
memset(&g_RecvDataFromRTK_0, 0, sizeof(g_RecvDataFromRTK_0));
memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));
int nBytes_0 = 0;
int nBytes_1 = 0;
while(1)
{
usleep(10000*50);
memset(&g_RecvDataFromRTK_0, 0, sizeof(g_RecvDataFromRTK_0));
memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));
nBytes_0 = nBytes_1;
nBytes_1 = read(g_fd, g_RecvDataFromRTK_1, 200);
usleep(10000);
if (g_RecvDataFromRTK_1[0] == '$')
{
char *ret;
int i;
for(i = 0;i < 200;i++)
{
if(g_RecvDataFromRTK_1[i]=='*')
{
break;
}
}
if(i > 70&&i!=200)
{
process_len = i;
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);
}
}
}
return 0;
}
三、数据处理线程
pthread_t AnalysisIMU_tid;
pthread_create(&AnalysisIMU_tid, NULL, AnalysisIMU_pthread, NULL);
pthread_join(AnalysisIMU_tid, NULL);
void *AnalysisIMU_pthread(void *args)
{
unsigned char localproc_SCData[200];
int localproc_len=0;
int num=0;
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_len=process_len;
num=0;
pthread_mutex_unlock(&gMutex_SC_data);
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;
}
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;
}
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;
}
}
}
g_Lattitude = g_Lattitude_tmp;
g_Longitude = g_Longitude_tmp;
g_Heading = g_Heading_tmp;
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)
{
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;
}
四、Makefile
#指定项目.c所存在的文件夹
SRC_DIR= ./
vpath %.c ./:./MC:./CAN
#输出文件名
OUTPUT:= route_get
#输出目录
OUTPUT_DIR= ../RELEASE
#交叉编译1
#CC:= arm-linux-gcc
#CXX:= arm-linux-g++
#LIB_DIR:= -L$(PROJ_DIR)/lib/arm-linux-gcc
#交叉编译2
#CC:= arm-linux-gnueabihf-gcc
#CXX:= arm-linux-gnueabihf-g++
#LIB_DIR:= -L$(PROJ_DIR)/lib/arm-linux-gnueabihf-gcc
#交叉编译3
#CC:= aarch64-himix100-linux-gcc
#CXX:= aarch64-himix100-linux-g++
#LIB_DIR:= -L../../LIB/aarch64-himix100-linux-gcc
#标准编译
CC:= gcc -g
CXX:= g++
LIB_DIR:= -L../LIB/gcc
#包含路径
INCLUDE_DIR+= -I./MC
INCLUDE_DIR+= -I./CAN
#依赖项
LIB:= -lrt -lpthread -lm#-ludev #-nostdinc
#遍历所有子目录中的.c文件
SRC = $(foreach dir,$(SRC_DIR),$(wildcard $(dir)
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)