change detection_tion.

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -188,7 +188,7 @@ std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
-                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState,lastVehState;
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
 
 
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
@@ -417,6 +417,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     secSpeed = realspeed / 3.6;
 
 
+    if(ServiceCarStatus.mnParkType==2)
+    {
+        if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
+        {
+            vehState = dReverTZD;
+        }
+    }
 
 
     //sidePark
@@ -438,6 +445,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             ServiceCarStatus.bocheEnable=2;
         }
 
+
+
         if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 && vehState!=dRever2
                 &&  vehState!=dRever3 && vehState!=dRever4 &&  vehState!=reverseArr )
         {
@@ -503,6 +512,41 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    if(ServiceCarStatus.mnParkType == 2)
+        ServiceCarStatus.bocheEnable=1;
+
+
+    if(vehState == dReverTZD)
+    {
+         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+         if(pt.y<0.5)
+         {
+             if(abs(realspeed)>0.1)
+             {
+                 vehState = reverseArr;
+             }
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             minDecelerate=min(minDecelerate,-0.5f);
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+         else
+         {
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             dSpeed = 2;
+             dSecSpeed = dSpeed / 3.6;
+             gps_decition->speed = dSpeed;
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+    }
+
     // ChuiZhiTingChe
     if (vehState == reverseCar)
     {