输出一些调试信息及V2R逻辑修改

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -611,6 +611,10 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
             xbs.set_mbtraficlightstart(ServiceCarStatus.mLightStartSensorBtn);
+            //20211011,cxw,增加1x车辆行驶状态和目标限速，begin
+//            xbs.set_vehicle_state(ServiceCarStatus.vehicle_state_1x);
+//            xbs.set_vehicle_spd(ServiceCarStatus.target_spd_1x);
+            //20211011,cxw,增加1x车辆行驶状态和目标限速，end
             ShareBrainstate(xbs);
             ShareDecition(decition_gps);
 
@@ -1178,19 +1182,23 @@ void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasi
        ServiceCarStatus.mbRunPause = group_message.mbpause();
 
 //      std::cout<<"enter  for butn %d  "<<ServiceCarStatus.mbRunPause <<std::endl;
+       //givlog->debug("decition_brain","mbRunPause is : %d",group_message.mbpause());
     }
     if(group_message.has_speedlimit())
     {
          ServiceCarStatus.ui_limit_spd = group_message.speedlimit();
+//         givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
     }
 
     if(group_message.has_radiobroadcastgpslat())
     {
         ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslat();
+//        givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
     }
     if(group_message.has_radiobroadcastgpslon())
     {
         ServiceCarStatus.rsu_gps_lng= group_message.radiobroadcastgpslon();
+//        givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
     }
     if(group_message.has_radiolighttype()) //1:绿灯 2：红灯 3：黄灯
     {

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -366,6 +366,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
            gps_decition->brake=10;
         }
+        givlog->debug("decition_brain","gps_lat is error,f%",now_gps_ins.gps_lat); //20230315
         return gps_decition;
     }
 
@@ -909,6 +910,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         minDecelerate=-1.0;
         phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
+
+        givlog->debug("decition_brain","PathPoint is smaller than 0, it is : %d",PathPoint);
         return gps_decition;
     }
 
@@ -1113,6 +1116,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
            gps_decition->brake=10;
         }
+        givlog->warn("decition_brain","gpsTraceOri is empty");
         return gps_decition;
     }
 
@@ -2194,6 +2198,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
     float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
     //以上变量信息都需要存储到log文件中
+    GaussProjCal(traffic_center_gps.gps_lng, traffic_center_gps.gps_lat,  &traffic_center_gps.gps_x, &traffic_center_gps.gps_y);
    distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
 if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
         ||(warning_type==0x01)||(warning_type==0x02))
@@ -2215,23 +2220,39 @@ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_ty
        }
        else
        {}
+//       givlog->debug("decition_brain","traffic_center_gps.gps_lat is : %f",traffic_center_gps.gps_lat);
+//       givlog->debug("decition_brain","traffic_center_gps.gps_lng is : %f",traffic_center_gps.gps_lng);
+//       givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+//       givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+//       givlog->debug("decition_brain","dSpeedd is : %f",dSpeed);
+//       givlog->debug("decition_brain","minDecelerate is : %f",minDecelerate);
+//       givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
     }
    else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
     {
        if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
        {
-          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+//          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+          if(realspeed-2>trafficlimit_spd)
+           {
+                dSpeed=realspeed-2;//先让速度稍微减少一点
+           }
            ServiceCarStatus.vehicle_state_1x = 1;
            ServiceCarStatus.target_spd_1x = trafficlimit_spd;
        }
        else if(distance_to_center<radiation_distance)
        {
-          dSpeed=min((double)trafficlimit_spd,realspeed);
+          //dSpeed=min((double)trafficlimit_spd,realspeed);
+           dSpeed=(double)trafficlimit_spd;
            ServiceCarStatus.vehicle_state_1x = 1;
            ServiceCarStatus.target_spd_1x = trafficlimit_spd;
        }
        else
        {}
+//       givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+//       givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+//       givlog->debug("decition_brain","dSpeed is : %f",dSpeed);
+//       givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
     }
     else
     {}
@@ -2280,6 +2301,12 @@ else
         {
             minDecelerate=-2.0;
         }
+       // givlog->debug("decition_brain","traffic_light_pathpoint is : %d",traffic_light_pathpoint);
+   //        givlog->debug("decition_brain","traffic_speed is : %f",traffic_speed);
+   //        givlog->debug("decition_brain","traffic_light_gps.gps_lat is : %f",traffic_light_gps.gps_lat);
+   //        givlog->debug("decition_brain","traffic_light_gps.gps_lng is : %f",traffic_light_gps.gps_lng);
+   //        givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightColor is : %d",ServiceCarStatus.iTrafficeLightColor);
+   //        givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightTime is : %d",ServiceCarStatus.iTrafficeLightTime);
     }
     //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
 
@@ -4113,13 +4140,24 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
 //    }
 
     if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
-        traffic_speed=0;
+//        traffic_speed=0;
+//        return  traffic_speed;
+        if(traffic_dis>3.0)
+        {
+            traffic_speed=sqrt(traffic_dis*2.0*9.8*0.5);
+        }
+        else
+        {
+           minDecelerate=-0.7;
+           traffic_speed=0;
+        }
+        return  traffic_speed;
     }
     //    else   //20200108
     //    {
     //        traffic_speed=10;
     //    }
-    return  traffic_speed;
+//    return  traffic_speed;
 
     passSpeed = min((float)(dSpeed/3.6),secSpeed);
     passTime = traffic_dis/(dSpeed/3.6);