星网宇达（惯导+IMU）设备实现自动采点

最新推荐文章于 2023-09-25 17:38:12 发布

范猛男

最新推荐文章于 2023-09-25 17:38:12 发布

阅读量1.2k

点赞数

分类专栏：自动驾驶文章标签：自动驾驶

本文链接：https://blog.csdn.net/weixin_48657767/article/details/123676799

版权

自动驾驶专栏收录该内容

21 篇文章 6 订阅

订阅专栏

一、创建和打开gps_Road.txt文件，准备往里写数据

FILE *p = NULL;
p = fopen("gps_Road.txt", "w");//定义一个指针也就是地址p存放gps_Road.txt里的数据，fopen():先查看gps_Road.txt这个文件有没有，没有就创建一个gps_Road.txt文件，然后打开准备往里写

二、创建GPFPD数据接收线程

pthread_t RecvFromIMU_tid;//pthread_t类型的参数
pthread_create(&RecvFromIMU_tid, NULL, RecvFromIMU_pthread, NULL);//四个参数分别是pthread_t类型的参数、线程属性、线程函数、线程函数的参数
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);
    /*将目前终端机的结构保存至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); 
}

void *RecvFromIMU_pthread(void *args)
{
    memset(g_RecvDataFromRTK,0,sizeof(g_RecvDataFromRTK));//初始化g_RecvDataFromRTK[]这个数组
    if(IsInit==false)//bool类型，反正都会是1
    {
        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));
	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));
		//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;
            for(i = 0;i < 200;i++)
            {
                if(g_RecvDataFromRTK_1[i]=='*')//遍历整个数组，遍历到GPFPD的尾部就是'*'的地方就停止遍历，跳出整个循环
                {
                    break;
                }                  
            }
            if(i > 70&&i!=200)
            {
                process_len = i;//整个GPFPD数据的长度是120字节，如果在以上区间就说明正确
                //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);//把整个GPFPD数据存放到g_ProcDataFromSCBuf[]数组中
				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;
	//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_len=process_len;
            num=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;
            for(int i=0;i<localproc_len;i++)
            {
                if(localproc_SCData[i]==0x2c)//0x2c是','表明的是GPFPD隔开的逗号点
                {
                    num++;//num++就是先使用num原本的数字然后再加一
                    if(num==3)//原本是第四位，因为num++的原因变成第三位
                    {
                        for(int j=i+1;;j++)
                        {
                            if(localproc_SCData[j]==0x2c)//遇到','就会跳出整个循环，表明转向角数据已经找到，在两个','之间
                            {
                                break;
                            }
                            if(localproc_SCData[j]!=0x2e)//遇到'.'就要排除它，比如说345.255就要把'.'去除
                            {
                                g_Heading_tmp*=10;
                                g_Heading_tmp+=(localproc_SCData[j]-48);//把每一位取出来的数字从字符串转换成数字，需要每一位减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;
            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)/*.c))

#指定.o文件的存放目录
OBJ_DIR=					../OBJ

#目标文件
OBJ:=						$(patsubst %.c,%.o,$(SRC))
#$(info "OBJ:$(OBJ)")#调试打印

#去.o文件地址，只保留文件名
OBJ_WITHOUT_DIR = $(notdir $(OBJ))
#$(info "OBJ_WITHOUT_DIR:$(OBJ_WITHOUT_DIR)")#调试打印

#将生成的.o文件与指定目录相绑定
OBJ_WITH_DIR = $(addprefix $(OBJ_DIR)/,$(OBJ_WITHOUT_DIR))
#$(info "OBJ_WITH_DIR:$(OBJ_WITH_DIR)")#调试打印

#必须先执行 OBJ_WITHOUT_DIR ，才能有OBJ_WITH_DIR依赖
all: clean $(OBJ_WITHOUT_DIR) $(OUTPUT)

$(OUTPUT):$(OBJ_WITH_DIR)
	$(CC) -o $(OUTPUT_DIR)/$@ $^ $(LIB_DIR) $(LIB)
%.o:%.c
	$(CC) $(INCLUDE_DIR) $(LIB_DIR) $(LIB) -c $< -o $(OBJ_DIR)/$@

clean:
	rm -rf $(OUTPUT) $(OBJ_DIR)/*.o

.PHONY: all clean

范猛男

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
星网宇达（惯导+IMU）设备实现自动采点

一、创建和打开gps_Road.txt文件，准备往里写数据FILE *p = NULL;p = fopen("gps_Road.txt", "w");//定义一个指针也就是地址p存放gps_Road.txt里的数据，fopen():先查看gps_Road.txt这个文件有没有，没有就创建一个gps_Road.txt文件，然后打开准备往里写二、创建GPFPD数据接收线程pthread_t RecvFromIMU_tid;//pthread_t类型的参数pthread_create(&Rec
复制链接

扫一扫