|
@@ -1982,6 +1982,47 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
|
|
|
|
std::cout<<"juecesudu0="<<dSpeed<<std::endl;
|
|
|
|
|
|
+
|
|
|
+
|
|
|
+ //-----------------------------------------1+X采集车车路协同,add---------------------------------------------
|
|
|
+ unsigned char traffic_type=0;//路况信息
|
|
|
+ unsigned char warning_type=0;//预警信息,RSU获得
|
|
|
+ float distance_to_center=0;
|
|
|
+ float radiation_distance=0;//事件辐射范围,从RSU获得
|
|
|
+ float limit_spd=0; //从RSU获得的限速值
|
|
|
+ //以上变量信息都需要存储到log文件中
|
|
|
+// distance_to_center=GetDistance(now_gps_ins,gps_center);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
|
|
|
+ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方,施工,道路结冰,事件范围外10米缓慢减速,事件范围内停车
|
|
|
+ {
|
|
|
+ if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
|
|
|
+ {
|
|
|
+ dSpeed = min(1,realspeed-0.5);
|
|
|
+ }
|
|
|
+ else if(distance_to_center<radiation_distance)
|
|
|
+ {
|
|
|
+ dSpeed=0.0;
|
|
|
+ minDecelerate=-2.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+ }
|
|
|
+ else if(traffic_type==0x04)//如果事件类型是限速,则在辐射范围内限速,辐射范围外正常行驶
|
|
|
+ {
|
|
|
+ if(distance_to_center<radiation_distance)
|
|
|
+ {
|
|
|
+ dSpeed=min(limit_spd,realspeed-0.5);
|
|
|
+ }
|
|
|
+ }
|
|
|
+//碰撞预警,1减速,2 停车
|
|
|
+ if(warning_type==0x01)
|
|
|
+ {
|
|
|
+ dSpeed=limit_spd;
|
|
|
+ }
|
|
|
+ else if(warning_type==0x01)
|
|
|
+ {
|
|
|
+ dSpeed=0.0;
|
|
|
+ }
|
|
|
+ //-----------------------------------------1+X采集车车路协同,end---------------------------------------------
|
|
|
//-------------------------------traffic light----------------------------------------------------------------------------------------
|
|
|
|
|
|
if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
|