소스 검색

for three files add include file tracepointstation.h,in decide_gpss_00.h add V2X logic for 1+x

chenxiaowei 3 년 전
부모
커밋
21436cdc48

+ 1 - 0
src/decition/decition_brain_sf_1x/decition/adc_planner/lane_change_planner.cpp

@@ -4,6 +4,7 @@
 #include <common/constants.h>
 
 #include <math.h>
+#include "common/tracepointstation.h"
 
 iv::decition::LaneChangePlanner::LaneChangePlanner(){
     this->planner_id = 0;

+ 1 - 0
src/decition/decition_brain_sf_1x/decition/adc_tools/compute_00.cpp

@@ -8,6 +8,7 @@
 #include <iostream>
 #include <fstream>
 #include <control/can.h>
+#include "common/tracepointstation.h"
 
 #include "ivlog.h"
 extern iv::Ivlog * givlog;

+ 41 - 0
src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -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){

+ 1 - 0
src/decition/decition_brain_sf_1x/decition/decide_gps_00.h

@@ -19,6 +19,7 @@
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
+#include "common/tracepointstation.h"
 
 namespace iv {
 namespace decition {