简介：无人小巴总体功能分为自动循迹、雷达避障以及APP控制启停
一、自动循迹
1.首先是入口函数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;
}
int optval = 1;
setsockopt(brdcFd, SOL_SOCKET, SO_BROADCAST | SO_REUSEADDR, &optval, sizeof(int));
memset(&Addr, 0, sizeof(struct sockaddr_in));
Addr.sin_family = AF_INET;
Addr.sin_port = htons(port_out); //50001
//Addr.sin_addr.s_addr = htonl(INADDR_ANY);
Addr.sin_addr.s_addr = htonl(INADDR_BROADCAST);
}
int sendbytes = 0;
//sendbytes = sendto(brdcFd, &gMC_state, sizeof(gMC_state), 0, (struct sockaddr *)&Addr, sizeof(struct sockaddr));
// printf("发送了MC_state--------%d\n",sendbytes);
//if(sendbytes>=0)
//{
// printf("NAV**********************************************************************************************%d\n",gMC_NAV.navID);
//}
sendbytes = sendto(brdcFd, &gMC_NAV, sizeof(gMC_NAV), 0, (struct sockaddr *)&Addr, sizeof(struct sockaddr));//将gMC_NAV中的导航信息发送到brdFD指向的地址中
//printf("发送了MC_NAV--------%d\n",sendbytes);
//printf("yaw--------%f",gMC_NAV.Yaw_rad);
//sendbytes = sendto(brdcFd, &gMC_DRIVE, sizeof(gMC_DRIVE), 0, (struct sockaddr *)&Addr, sizeof(struct sockaddr));
// printf("发送了MC_DRIVE--------%d\n",sendbytes);
}
4.路径规划模块开始接收底盘UDP过来的数据，显示建立SOCKET通信接收，然后把接收到的车辆状态信息存放在线程参数Param中
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);//接收来自底盘IMU的数据串
//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);
}
//}
//printf("rec!\n");
break;
}
}
return 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));
}
5.开始路径规划线程（pl_thread）
（1）首先从规划的gps_load.txt文件里面提取一行行数据，然后解析，将经纬度数据，目标速度以及路径点放进g_pMoRoadToPL这个结构体中
void *pl_thread(void *param)
{
/**/
//memset(&g_MoCmdToPL,0,sizeof(WRC_MO_Command_To_PL));
memset(&g_pMoRoadToPL,0,sizeof(WRC_MO_Road_To_PL));//初始化g_pMoRoadToPL数组，清空里面的内容
//g_MoRoadToPL
FILE *fp_gps = fopen("gps_load.txt","r");//打开gps_load.txt文件，然后从中读取数据
if(NULL == fp_gps)
{
printf("open file error");
return 0;
}
char mystring [200];
memset(mystring,'\0',200);
int point_num = 0;
while(fgets(mystring , 200 , fp_gps))//从指定数据流中读取一行，从fp_gps数据流读取前200个字节数据存放到mystring
{
//printf("%s\n",mystring);
string line_string(mystring);
//printf("%s\n",line_string.c_str());
int position = 0;
string lat_s,lon_s,sp_s;
position = line_string.find(",");
lat_s = line_string.substr(0,position);
line_string.erase(0,position+1);
position = line_string.find(",");
lon_s = line_string.substr(0,position);
line_string.erase(0,position+1);
position = line_string.find(",");
sp_s = line_string.substr(0,position);
double d_lat = atof(lat_s.c_str());
double d_lon = atof(lon_s.c_str());
double d_sp = atof(sp_s.c_str());
g_pMoRoadToPL.GPSPointQueue[point_num].LatitudeInfo = d_lat;
g_pMoRoadToPL.GPSPointQueue[point_num].LongitudeInfo = d_lon;
g_pMoRoadToPL.GPSPointQueue[point_num].idealSpeed = (int)(d_sp*0.5144*100); printf("%lf,%lf,%d\n",g_pMoRoadToPL.GPSPointQueue[point_num].LatitudeInfo,g_pMoRoadToPL.GPSPointQueue[point_num].LongitudeInfo,
g_pMoRoadToPL.GPSPointQueue[point_num].idealSpeed);//至此取出一行行分别由经纬度、理想速度组成的数据串
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;
}
point_num++;
}
printf("point_num:%d\n",point_num);//这就是传输过来的统计多少个路径点，以5个点为一组
//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;
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);
//return 0;
while(drive_mode == 0)
{
sleep(1);
}
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;
}
(2)启动PL处理线程（PL_ProcThread）
思路：把IMU实时传过来的点与自动采点进行距离解算，从自动采点的很多点钟找到当前点最近点和目标点，最后判断是否为最后五个点，若是，则将速度置0 ，若是中间点，赋予它目标点的x,y坐标和实时速度。
这里面有两个关键算法：寻找最近点和寻找目标点
void * PL_ProcThread(void* param)
{
WRC_Parameter* pParam=(WRC_Parameter*)param;
memset(&g_PLResultToMC,0,sizeof(WRC_PL_TO_MC));
//m_OneFramePoint[];
/*pParam->g_NavInfo.Longitude_degree = 120.9152374;
pParam->g_NavInfo.Latitude_degree = 32.0311360;
pParam->g_NavInfo.Yaw_rad = 347.446;*/
//radar_car_x_cm = 0;
//radar_car_y_cm = 0;
int road_pos = 0;
int des_pos = 5;
front_hindrance = 0;
g_PLResultToMC.pPoint[0].x_cm =0;
g_PLResultToMC.pPoint[0].y_cm =10;
sleep(3);
while(1)
{
usleep(50000);
printf("front_hindrance_dis: %d\n",front_hindrance_dis);
if(front_hindrance_dis <= 2 || drive_mode == 2)
{
g_PLResultToMC.speed_cm_ps = 0;
g_PLResultToMC.drvCmd = WRC_PL_COMMAND_DRV_WAIT;
printf("stop!\n");
sleep(3);
//usleep(50000);
continue;
}
//printf("pParam->g_NavInfo.Longitude_degree: %lf\n",pParam->g_NavInfo.Longitude_degree);
//printf("pParam->g_NavInfo.Latitude_degree: %lf\n",pParam->g_NavInfo.Latitude_degree);
//printf("pParam->g_NavInfo.Yaw_rad: %lf\n",pParam->g_NavInfo.Yaw_rad);
//printf("g_pMoRoadToPL.GpsPointNum:%d\n",g_pMoRoadToPL.GpsPointNum);
double head_raw = pParam->g_NavInfo.Yaw_rad;
//printf("head_raw:%lf\n",head_raw);
road_pos = Road_Planning_Find_Nearest_Point(des_pos-5,pParam->g_NavInfo,g_pMoRoadToPL.GPSPointQueue);
des_pos = Road_Planning_Find_Aim_Point(road_pos,5,pParam->g_NavInfo,g_pMoRoadToPL.GPSPointQueue);
//printf("road_pos: %d\n",road_pos);
//printf("des_pos: %d\n",des_pos);
//if()
if(road_pos == g_pMoRoadToPL.GpsPointNum-5)
{
/*float end_distance = ntzx_GPS_length(pParam->g_NavInfo.Longitude_degree,
pParam->g_NavInfo.Latitude_degree,
g_pMoRoadToPL.GPSPointQueue[g_pMoRoadToPL.GpsPointNum-1].LongitudeInfo,
g_pMoRoadToPL.GPSPointQueue[g_pMoRoadToPL.GpsPointNum-1].LatitudeInfo);
if(end_distance<0.5)
{*/
g_PLResultToMC.pPoint[0].x_cm =0;
g_PLResultToMC.pPoint[0].y_cm =10;
g_PLResultToMC.speed_cm_ps = 0;
g_PLResultToMC.drvCmd = WRC_PL_COMMAND_DRV_WAIT;
//}
break;
}
else if(des_pos!=-1)
{
double x_cm_tmp=0;
double y_cm_tmp=0;
ntzx_GPS_posit(head_raw,pParam->g_NavInfo.Longitude_degree,pParam->g_NavInfo.Latitude_degree,
g_pMoRoadToPL.GPSPointQueue[des_pos].LongitudeInfo,
g_pMoRoadToPL.GPSPointQueue[des_pos].LatitudeInfo,
&x_cm_tmp,&y_cm_tmp);
g_PLResultToMC.pPoint[0].x_cm =(int)(x_cm_tmp*100);
g_PLResultToMC.pPoint[0].y_cm =(int)(y_cm_tmp*100);
g_PLResultToMC.speed_cm_ps = g_pMoRoadToPL.GPSPointQueue[des_pos].idealSpeed;
//g_PLResultToMC.speed_cm_ps = 70;
//printf("front_hindrance_dis: %d\n",front_hindrance_dis);
}
}
}
return 0;
}
（3）发送给底盘线程（ThreadToMC）
void *ThreadToMC(void *param)
{
void *pIPDataToMC = NULL; //Ip数据包的地址
int nIPDataSize_MC=0;
int nSendToMCSize=0;
WRC_PL_TO_MC PLResultToMC_temp;
while(1)
{
usleep(50000);
pthread_mutex_lock(&g_MutexSendToMC);
memset(&PLResultToMC_temp,0,sizeof(WRC_PL_TO_MC)); memcpy(&PLResultToMC_temp,&g_PLResultToMC,sizeof(WRC_PL_TO_MC));
pthread_mutex_unlock(&g_MutexSendToMC);
//printf("PL : x_cm是%d!\n",PLResultToMC_temp.pPoint[0].x_cm);
//printf("PL : y_cm是%d!\n",PLResultToMC_temp.pPoint[0].y_cm);
//printf("PL : a是%d!\n",PLResultToMC_temp.speed_cm_ps);
// LOG("PL: 发往MC的指令是：%d\n",PLResultToMC_temp.IO_Cmd);
//step1.3、编码
nIPDataSize_MC = WRC_PL_Encode_IP_Data(&PLResultToMC_temp, &pIPDataToMC);//把pIPData给PLRes,返回结构体大小
//step1.4、网络发送往MC
//printf("PL : -----------------------------------****************************>>>>>>>>>>>>Now,send to MC!\n");
//printf("PL : Now,send to MC!\n");
bool isSendMcFalse = false;
nSendToMCSize = gTcp.WRC_IP_send_to("MC", pIPDataToMC, nIPDataSize_MC);//返回发出去的字节数
//printf("PL : ----------------------------------****************************>>>>>>>>>>>send to MC %d Bytes Success!\n",nSendToMCSize);
pthread_mutex_unlock(&g_MutexSendToMC);
}
}
6.PL规划信息发送给底盘（RecvFromPL_pthread），底盘接收后进行速度和角度PID调节
（1）PL模块规划后的数据通过UDP通信发送给底盘，底盘开始接收PL发送数据存放在gWRC_PL_TO_MC中
void *WRC_RecvFromPL()
{
int g_UDPPLSocket = -1; //Socket描述符
//初始化
UDPDataSize = 0;
memset(g_RecvDataBuf, 0, sizeof(g_RecvDataBuf));//初始化g_RecvDataBuf这个数组，清空里面的内容
//创建套接字
g_UDPPLSocket = socket(AF_INET, SOCK_DGRAM, 0);//建立SOCKET通信，第一个参数是AF_INET局域网域，第二个参数是UDP通信的标志，第三个默认是0
if (g_UDPPLSocket < 0)
{
printf("MC : 创建与PL通信的套接字失败!\n");
return NULL;
}
else
{
printf("MC : 创建与PL通信的套接字成功!\n");
}
//定义sockaddr_in
struct sockaddr_in server_sockaddr;
memset(&server_sockaddr, 0, sizeof(sockaddr_in));
server_sockaddr.sin_family = AF_INET;
server_sockaddr.sin_port = htons(SERVER_PORT);//服务器端口号
server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY); //服务器IP地址 //htonl(INADDR_ANY)
socklen_t Len = sizeof(server_sockaddr);
//客户端
struct sockaddr_in fromaddr;
bzero(&fromaddr, sizeof(fromaddr));
socklen_t fromaddrlen = sizeof(struct sockaddr);
//printf("建立连接\n");
//建立连接
if (bind(g_UDPPLSocket, (struct sockaddr*)&(server_sockaddr), Len) == -1)
{
printf("g_UDPPLSocket:bind error..");
return NULL;
}
else
{
printf("g_UDPPLSocket:bind success..");
}
//接收数据
fd_set Readfd;
struct timeval Time_out;
//static int StartTiming = 0;
//static int SendZero = 0;
while (1)
{
Time_out.tv_sec = 2;//超时时间10s
Time_out.tv_usec = 500;
//套节字集合清空
FD_ZERO(&Readfd);
//将套接字描述符加入到文件描述符集合
FD_SET(g_UDPPLSocket, &Readfd);
int ret = select(g_UDPPLSocket + 1, &Readfd, NULL, NULL, &Time_out); //侦听是否可读
switch (ret)
{
case -1: //发生错误
//printf("UDPCLENT：SOCKET select error:\n");
break;
case 0:
//printf("UDPCLENT:UDP recv time out!\n");
pthread_mutex_lock(&gMutex_PL_DATA);
memset(&gWRC_PL_TO_MC, 0, sizeof(WRC_PL_TO_MC));
pthread_cond_signal(&gCond_PL_DATA);
pthread_mutex_unlock(&gMutex_PL_DATA);
break;
default:
//printf("rec!\n");
if (FD_ISSET(g_UDPPLSocket, &Readfd))//检查套接字sock是否在集合readfd当中
{
int nBytes;
memset(&g_RecvDataBuf, 0, sizeof(g_RecvDataBuf));
nBytes = recvfrom(g_UDPPLSocket, g_RecvDataBuf, sizeof(g_RecvDataBuf), 0, (struct sockaddr*)&fromaddr, &fromaddrlen);
//printf("nBytes: %d\n",nBytes);
if (nBytes <= 0)
{
//printf("UDPRECV:recv bytes error..\n");
}
else
{
UDPDataSize = nBytes;
//锁住
pthread_mutex_lock(&gMutex_PL_DATA);
memset(&gWRC_PL_TO_MC, 0, sizeof(WRC_PL_TO_MC));
memcpy(&gWRC_PL_TO_MC, g_RecvDataBuf, sizeof(WRC_PL_TO_MC));
//激活处理线程
pthread_cond_signal(&gCond_PL_DATA);
//释放锁
pthread_mutex_unlock(&gMutex_PL_DATA);
}
}
break;
}
//}
// else
// {
// continue;
// }
}
return 0;
}
（2）PL规划数据接收过来之后，经过一个线程信号触发条件，跳转到角度和速度pid控制函数，然后经过g_ecu_eps_cmd结构体里面的角度控制和加速度控制控制底盘
关键点：速度和角度pid控制
主要思想：
//方向控制
int direction_control(FILE *pfile)
{
signed short tx, ty; //目标点车体坐标
//step.1---------------------获得目标点的车体坐标-----------------------------//
pthread_mutex_lock(&gMutex_PL_DATA);
pthread_cond_wait(&gCond_PL_DATA, &gMutex_PL_DATA);//等待PL信号触发
tx = gWRC_PL_TO_MC.pPoint[0].x_cm; //选取车前5米作为目标点
ty = gWRC_PL_TO_MC.pPoint[0].y_cm;
//printf("tx:%d",tx);
//printf("ty:%d`",ty);
//tx = 4;
//ty = 117;
pthread_mutex_unlock(&gMutex_PL_DATA);
//step.2-----------计算偏差以及其积分项和微分项,偏差单位：度------------------//
if (isFirstDir)
{
curDirErr = atan2(tx, ty) * 180 / PI;
//printf("isFirstDir%lf\n",atan2(tx, ty));
//curDirErr = atan(1.0*tx / ty) * 180 / PI;
preDirErr = curDirErr;
isFirstDir = false;
}
else
{
preDirErr = curDirErr;
curDirErr = atan2(tx, ty) * 180 / PI;
//printf("noFirstDir%lf\n",curDirErr);
//curDirErr = atan(1.0*tx / ty) * 180 / PI;
}
//printf("%d\n",isFirstDir);
//printf("%lf\n",curDirErr);
DirIntegral += curDirErr; //计算偏差积分项
DirDifferential = curDirErr - preDirErr; //计算偏差微分项
//step.4------------------------对方向进行PID控制-----------------------------//
double Angle;
int SpdLevel;
pthread_mutex_lock(&Mutex_EPS_DATA);
//速度分段控制
if (g_vcu_vstate1.speed < 60)
SpdLevel = 0;
else
SpdLevel = 1;
//根据挡位进行方向控制
//if (g_ecu_speed_cmd.gear_state == GEAR_D)
//{
Angle = (DirD_Kp[SpdLevel] * curDirErr + DirD_Ki[SpdLevel] * DirIntegral + DirD_Kd[SpdLevel] * DirDifferential + 1080) * 10;
//}
//if (g_ecu_speed_cmd.gear_state == GEAR_R)
//{
//待测试
//Angle = (-(DirR_Kp[SpdLevel] * curDirErr + DirR_Ki[SpdLevel] * DirIntegral + DirR_Kd[SpdLevel] * DirDifferential) + 1080) * 10;
//}
if (Angle > 16200) //21600
Angle = 16200; //21600
if (Angle < 5400) //0
Angle = 5400; //0
#ifdef SpeedMode //模式切换
g_ecu_eps_cmd.steering_wheel_angle = 10800;//不转弯模式
#else
g_ecu_eps_cmd.steering_wheel_angle = (unsigned short)Angle; //方向控制模式
#endif
//停车置零
/*if ((AimSpeed == 0) && (CarSpeed <= 52))
{
g_ecu_eps_cmd.steering_wheel_angle = 10800;
DirIntegral = 0;
DirDifferential = 0;
}*/
pthread_mutex_unlock(&Mutex_EPS_DATA);
}
//速度控制
int speed_control(FILE *pfile)
{
//step.1--------------------获得目标速度,单位：cm/s---------------------------//
pthread_mutex_lock(&gMutex_PL_DATA);
pthread_cond_wait(&gCond_PL_DATA, &gMutex_PL_DATA);
double speed = gWRC_PL_TO_MC.speed_cm_ps;
pthread_mutex_unlock(&gMutex_PL_DATA);
//printf("speed_control\n");
if (speed > 10/ 3.6 * 100)
speed = 10 / 3.6 * 100;
if (speed < 0)
speed = 0;
if (SpeedNum < MAX_SPEED_NUM)
{
SpeedArray[SpeedNum] = speed;
SpeedSum += speed;
AimSpeed = 1.0*SpeedSum / (SpeedNum + 1);
}
else
{
SpeedSum += speed;
SpeedSum -= SpeedArray[SpeedNum%MAX_SPEED_NUM];
SpeedArray[SpeedNum%MAX_SPEED_NUM] = speed;
AimSpeed = 1.0*SpeedSum / MAX_SPEED_NUM;
}
SpeedNum++;
//printf("AimSpeed:%f\n",AimSpeed);
if (speed != 0) //如果目标速度不为零,则释放手刹
{
g_ecu_speed_cmd.epb_state = 0b10;
}
//step.2--------------------获得车体速度,单位：km/h---------------------------//
pthread_mutex_lock(&Mutex_CAN_DATA);
pthread_cond_wait(&Cond_CAN_DATA, &Mutex_CAN_DATA);
double tempCarSpeed = g_vcu_vstate1.speed;//待优化
printf("AimSpeed: %lf ... CarSpeed: %lf\n",AimSpeed,tempCarSpeed);
pthread_mutex_unlock(&Mutex_CAN_DATA);
//待优化----车体速度平滑
CarSpeed = (tempCarSpeed + LastCarSpeed + LLastCarSpeed + LLLastCarSpeed) / 4;
LLLastCarSpeed = LLastCarSpeed;
LLastCarSpeed = LastCarSpeed;
LastCarSpeed = tempCarSpeed;
//挡位控制--------------待测试
//if (gWRC_PL_TO_MC.pPoint[1].y_cm > 0) //路径点在车体前
//{
// if (g_ecu_speed_cmd.gear_state != GEAR_D) //若当前不为前进挡
// {
// if (tempCarSpeed != 50) //将车速减为零
// AimSpeed = 0;
// else
// g_ecu_speed_cmd.gear_state = GEAR_D; //挂前进挡
// }
//}
//if (gWRC_PL_TO_MC.pPoint[1].y_cm < 0) //路径点在车体后
//{
// if (g_ecu_speed_cmd.gear_state != GEAR_R) //若当前不为后退挡
// {
// if (tempCarSpeed != 50) //将车速减为零
// AimSpeed = 0;
// else
// g_ecu_speed_cmd.gear_state = GEAR_R; //挂后退挡
// }
//}
//step.3-----------计算偏差以及其积分项和微分项,偏差单位：m/s-----------------//
if (isFirstSpd)
{
curSpdErr = AimSpeed * 0.01 - (CarSpeed - 50) / 3.6;
preSpdErr = curSpdErr;
isFirstSpd = false;
}
else
{
preSpdErr = curSpdErr;
curSpdErr = AimSpeed * 0.01 - (CarSpeed - 50) / 3.6;
}
SpdIntegral += curSpdErr; //计算偏差积分项
SpdDifferential = curSpdErr - preSpdErr; //计算偏差微分项
//step.4------------------------对速度进行PID控制-----------------------------//
pthread_mutex_lock(&Mutex_SPEED_DATA);
g_ecu_speed_cmd.gear_state = GEAR_D; //挂前进挡//测试需注释！！！！！！！！！！！！！！！！！！！
double Acceleration = (spd_Kp * curSpdErr + spd_Ki * SpdIntegral + spd_Kd * SpdDifferential + 12.8) * 10 + 0.5;
//printf("Acceleration:%lf\n",Acceleration);
if (Acceleration > 138) //255
Acceleration = 138; //255
if (Acceleration < 108) //0
Acceleration = 108; //0
#ifdef DirectMode //模式切换
g_ecu_speed_cmd.acceleration = 128; //零速度模式
#else
g_ecu_speed_cmd.acceleration = (unsigned char)Acceleration; //速度控制模式
#endif
//停车置零
if ((AimSpeed == 0) && (tempCarSpeed == 50))
{
g_ecu_speed_cmd.acceleration = 128;
g_ecu_speed_cmd.epb_state = 0b01; //拉起手刹,停车驻车
SpdIntegral = 0; //清零积分量
SpdDifferential = 0; //清零微分量
}
else if(AimSpeed == 0)
{
g_ecu_speed_cmd.acceleration = 118;
}
//if ((AimSpeed == 0) && (CarSpeed <= 52))
//g_ecu_speed_cmd.epb_state = 0b01; //拉起手刹,停车驻车
pthread_mutex_unlock(&Mutex_SPEED_DATA);
//step.4------------------------------返回------------------------------------//
return 0;
}
二、APP控制小巴启停
主要思想：APP发送信息过来，若发送的信息为‘0’，则drive_mode = 0表示制动，车辆停止，若发送信息为‘1’，则drive_mode = 1表示放开制动，车辆继续前进。
pthread_t g_ThreadMOUdpRec;
pthread_create(&g_ThreadMOUdpRec,NULL,mo_udp_rec,NULL);
pthread_join(g_ThreadMOUdpRec, NULL);
void *mo_udp_rec(void *p)
{
int sockfd;//SOCKET通信套接字
char buffer[MAXLINE];
char sendbuffer[MAXLINE];
struct sockaddr_in servaddr, cliaddr;
// Creating socket file descriptor
if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
perror("socket creation failed");
exit(EXIT_FAILURE);
}
memset(&servaddr, 0, sizeof(servaddr));
memset(&cliaddr, 0, sizeof(cliaddr));
// Filling server information
servaddr.sin_family = AF_INET; // IPv4
servaddr.sin_addr.s_addr = INADDR_ANY;
servaddr.sin_port = htons(PORT);
// Bind the socket with the server address
if ( bind(sockfd, (const struct sockaddr *)&servaddr,
sizeof(servaddr)) < 0 )
{
perror("bind failed");
exit(EXIT_FAILURE);
}
int n;
socklen_t len = (socklen_t)sizeof(cliaddr); //len is value/resuslt
for (;;) {
n = recvfrom(sockfd, (char *)buffer, MAXLINE,
MSG_WAITALL, ( struct sockaddr *) &cliaddr,
&len);
buffer[n] = '\0';
printf("Client : %s\n", buffer);
if(buffer[0] == '0')
{
drive_mode = 0;
}
else if(buffer[0] == '1')
{
drive_mode = 1;
}
//sprintf(sendbuffer, "have recieve %ld bytes", strlen(buffer));
//sendto(sockfd, (const char *)sendbuffer, strlen(sendbuffer), 0,
//(const struct sockaddr *) &cliaddr, len);
//fprintf(stdout, "%s\n", sendbuffer);
}
return 0;
}
这种情况是drive_mode==0属于即将停车状态，，会将speed赋0，动作命令赋wait
if(front_hindrance_dis <= 2 || drive_mode == 0)
{
g_PLResultToMC.speed_cm_ps = 0;
g_PLResultToMC.drvCmd = WRC_PL_COMMAND_DRV_WAIT;
printf("stop!\n");
sleep(3);
//usleep(50000);
continue;
}
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
