无人小巴自动循迹和雷达避障程序全解析（参加秋招用）
时间：2023-04-20 21:37:00
简介：无人小巴的整体功能分为自动跟踪、雷达避障和APP控制启停
一、自动跟踪
一是入口函数main.c创建了7个线程
void Start_Threads_Process() {
int ret; #ifndef NavTestMode ret = pthread_create(&RecvFromCAN_tid, NULL, RecvFromCAN_pthread,NULL); if(ret!=0) {
printf("Create thread to recv can msg failed\n"); } ret = pthread_create(&RecvFromPL_tid, NULL, RecvFromPL_pthread, NULL); if(ret!=0) {
printf("Create thread to recv PLdata msg failed\n"); } ret = pthread_create(&Process_tid,NULL,Process_pthread,NULL); if(ret!=0) {
printf("Create thread to track failed\n"); } ret = pthread_create(&SendToCAN_tid, NULL, SendToCAN_pthread, NULL); if (ret != 0) {
printf("Create thread to send can msg failed\n");
}
#endif
ret = pthread_create(&RecvFromIMU_tid, NULL, RecvFromIMU_pthread, NULL);
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");
}
ret = pthread_create(&SendCarState_tid, NULL, SendCarState_pthread, NULL);
if (ret != 0)
{
printf("Create thread SendCarState_pthread failed\n");
}
}
2.首先是RecvFromCAN_pthread（从CAN口接收数据线程）
这里面有车辆状态结构体vcu_vstate1_st，形式如下：
struct vcu_vstate1_st{
//1
unsigned char autopilot:2;
unsigned char door:1;
unsigned char daytime_driving_light:1;
unsigned char left_blinker:1;
unsigned char right_blinker:1;
unsigned char warning_light:1;
unsigned char clearance_light:1;
//2
unsigned char front_foglight:2;
unsigned char rear_foglight:1;
unsigned char left_foglight:1;
unsigned char right_foglight:1;
unsigned char interior_light:1;
unsigned char ambient_light:1;
unsigned char reserved1:1;
//3
unsigned char speed; //车速
//4
unsigned char SOC;
//5
unsigned char system_power:2;//智能系统电源指令
unsigned char charge_status:2;//充电状态
unsigned char charger_connection_status:2;//充电连接指示
unsigned char failure_level:2;//整车故障级别
//6~7
unsigned short reserved2;
//8
unsigned char heartbeat;
};
接下来就是提取从CAN口里面上传的车辆数据，存放在vcu_vstate1_st结构体中。
（1）首先定义一个结构体对象
struct vcu_vstate1_st g_vcu_vstate1;
（2）接下来经历CAN口初始化和CAN口接收
/*CAN通信初始化 */
int can_init()
{
struct sockaddr_can addr;//CAN地址对象
struct can_frame frame;//存放CAN口上传数据的结构体
struct ifreq ifr;
fd_set readSet;
const char *ifname = "can0";
struct can_filter filter[3];
int enable = 1;
int s = -1;
if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
{
perror("Error while opening socket");
return -1;
}
setsockopt(
s,
SOL_CAN_RAW,
CAN_RAW_FD_FRAMES,
&enable,
sizeof(enable)
);
strcpy(ifr.ifr_name, ifname);
ioctl(s, SIOCGIFINDEX, &ifr);
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0)
{
perror("Error in socket bind");
return -2;
}
return s;
}
当接收到id号为VCU_VEHICLE_STATE1的数据帧头的时候，就把frame也就是CAN口上传的数据存放在g_vcu_vstate1中
/*接收CAN数据 */
void *can_recv()
{
int s = can_init();//CAN口初始化，返回CAN口标志位
fd_set readSet;
int nbytes;
struct can_frame frame;
while (1)
{
FD_ZERO(&readSet);
FD_SET(s, &readSet);
if (select((s + 1), &readSet, NULL, NULL, NULL) > 0)
{
if (FD_ISSET(s, &readSet))
{
nbytes = read(s, &frame, sizeof(struct can_frame));//把s指向地址里面的数据放入frame
unsigned int id;
id = frame.can_id & CAN_EFF_MASK;
if (id == VCU_VEHICLE_STATE1)//当接收到id号为VCU_VEHICLE_STATE1的数据帧头的时候，就把frame也就是CAN口上传的数据存放在g_vcu_vstate1中
{
//printf("VCU_VEHICLE_STATE1收到了---------------%d\n", nbytes);
pthread_mutex_lock(&Mutex_CAN_DATA);
memcpy(&g_vcu_vstate1, frame.data, frame.can_dlc * sizeof(char));
pthread_cond_signal(&Cond_CAN_DATA);
pthread_mutex_unlock(&Mutex_CAN_DATA);
}
// else if(id == VCU_DRIVE_STATE)
// {
// printf("VCU_DRIVE_STATE收到了---------------%d\n",nbytes);
// memcpy(&g_vcu_dstate, frame.data, frame.can_dlc * sizeof(char));
// }
else
{
/*暂不解析 */
}
}
}
}
}
2.然后是RecvFromIMU_pthread（接收IMU数据线程）
因为我们这边采用的是全星SV2的惯导设备，里面集成GPS和IMU，里面传出来的数据格式中可以提取经纬度、速度以及转向角信息，比如：GPRMC和GNHDT
总体思路：先打开串口，然后把串口里面的数据读取出来，存放在一个数组中，最后解析这个数组中数据，把当中有用的信息提取出来。
void *RecvFromIMU_pthread(void *args)
{
memset(g_RecvDataFromRTK,0,sizeof(g_RecvDataFromRTK));//初始化g_RecvDataFromRTK数组里面的内容，将其清空
pfile=fopen("LOG.txt","a");
if(pfile==NULL){
printf("打开文件写LOG失败\n");
}
if(IsInit==false)//刚开始IsInit==false
{
if(initialize()<0)//初始化中主要功能是打开串口、设置波特率和设置标志位
{
printf("MC: 打开串口配置参数失败\n");
return 0;
}
IsInit=true;
printf("MC: 打开串口配置参数成功\n");
}
int nBytes_0 = 0;
int nBytes_1 = 0;
while(1)
{
usleep(60000);
memset(&g_RecvDataFromRTK_1, 0, sizeof(g_RecvDataFromRTK_1));
nBytes_1 = read(g_fd, g_RecvDataFromRTK_1, 200);//将g_fd指向地址里面的数据读到g_RecvDataFromRTK_1中去
//printf("g_RecvDataFromRTK_1:%s\n",g_RecvDataFromRTK_1);
//printf("nBytes0=%d,nBytes=%d\n", nBytes_0, nBytes_1);
if (g_RecvDataFromRTK_1[0] == '$')//分别判断gprmc和grhdt两个数据格式的数据位
{
char *ret;
int i;
int gps_num = 0;
for(i = 0;i < 200;i++)
{
if(g_RecvDataFromRTK_1[i] == '*')
{
gps_num++;
}
if(gps_num == 2)
{
break;
}
}
if(i != 200&&i > 80)
{
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);
}
}
}
return 0;
}
void *AnalysisIMU_pthread(void *args)
{
char localproc_SCData[200] = {
0};
int localproc_len=0;
int num_rmc = 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_len=process_len;
num_rmc = 0;
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_Speed_tmp = 0;
double g_Heading_tmp = 0;
char rtk_num[1] = {
0};
char *p_gprmc = strstr(localproc_SCData,"GPRMC");
char *p_gpdht = strstr(localproc_SCData,"GNHDT");
if(p_gprmc == NULL || p_gpdht == NULL)
continue;
int gprmc_len = 80;
int gpdht_len = 20;
for(int i = 0;i < gprmc_len;i++)
{
if(p_gprmc[i] == 0x2c)
{
num_rmc++;
if(num_rmc == 3)
{
for(int j=i+1;;j++)
{
if(p_gprmc[j]==0x2c)
{
break;
}
else if(p_gprmc[j]!=0x2e)
{
g_Lattitude_tmp*=10;
g_Lattitude_tmp+=(p_gprmc[j]-48);
}
}
//
}
else if(num_rmc == 5)
{
for(int j=i+1;;j++)
{
if(p_gprmc[j]==0x2c)
{
break;
}
else if(p_gprmc[j]!=0x2e)
{
g_Longitude_tmp*=10;
g_Longitude_tmp+=(p_gprmc[j]-48);
}
}
break;
//
}
/*else if(num_rmc == 7) { for(int j=i+1;;j++) { if(p_gprmc[j]==0x2c) { break; } else if(p_gprmc[j]!=0x2e) { g_Speed_tmp*=10; g_Speed_tmp+=(p_gprmc[j]-48); } } }*/
/*else if(num_rmc == 10) { rtk_num[0] = p_gprmc[i+1]; 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);
lon_fraction = modf(g_Longitude_tmp,&lon_int);
g_Lattitude = lat_int+lat_fraction/60*100;
g_Longitude = lon_int+lon_fraction/60*100;
g_Heading = g_Heading_tmp;
//printf("g_Lattitude: %lf \n",g_Lattitude);
//printf("g_Longitude: %lf \n",g_Longitude);
//printf("g_Heading: %lf \n",g_Heading);
//printf("g_Speed_tmp: %lf \n",g_Speed_tmp);
}
return 0;
}
3.接下来是关键信息发送线程（SendCarState_pthread），发送到PL模块去
把IMU接收过来的经纬度，转向角数据存放在导航信息结构体gMC_NAV，通过与路径规划PL模块建立UDP通信，广播状态信息
/*导航信息 */
typedef struct tagWRC_MC_NAV_INFO
{
UINT16 Head; //数据帧头 固定值 0x0824
UINT8 Group; //车辆组号
UINT8 ALV_ID; //车辆编号
UINT32 FrameID;
WRC_IP_TIME synTime; //时刻
UINT8 GPS_Status; //GPS状态
UINT8 NAV_Status; //惯导状态 见上“惯导组合状态 ”
UINT8 IS_NAV_Err; //导航数据是否有错: 0-无错,非零-有错
UINT32 navID; //导航信息编号(从0开始)
//位姿
double Latitude_degree; //单位:度
double Longitude_degree; //单位:度
double Altitude_m; //单位:米
double EarthRefCoord[2]; // 地面平面坐标系 单位:米 [0] 北向 +X [1] 东向 +Y 注意：暂时以校内智能楼差分站为参考基准
float Roll_rad; //单位:弧度
float Pitch_rad; //单位:弧度
float Yaw_rad; //单位:弧度
//速度
INT16 Speed_cm_ps; //单位:厘米/秒
INT16 SPD_LefWheel_cm_ps; //单位:厘米/秒
INT16 SPD_RightWhell_cm_ps; //单位:厘米/秒
INT16 IMU_R_xyz_mrad[3]; //角速率:毫弧度
INT16 IMU_A_xyz_mm_ps[3]; //IMU加速度:毫米
//GPS
UINT16 GpsSol_pDOP; //0.01,Postition DOP
UINT8 GpsSol_numSV; //Number of Svs used in navigation solution;
UINT16 GpsSol_ecefX_cm; //ECEF X: 单位厘米
UINT16 GpsSol_ecefY_cm; //ECEF Y: 单位厘米
UINT16 GpsSol_ecefZ_cm; //ECEF Z: 单位厘米
//安全
UINT16 nSize; //结构体大小
UINT8 checksum; //Checksum of above data.
}WRC_MC_NAV_INFO;
memset(&gMC_NAV, 0, sizeof(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);
int UDP_send_data()
{
if (initial == false) {
initial = true;
if ((brdcFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
{
printf("fail to create socket\n");
return -1;
}
