add park

src/decition/decition_brain_sf/decition/adc_adapter/sightseeing_adapter.cpp

@@ -221,6 +221,10 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
             dSpeed=0;
     }
 
+    if((dSpeed==0)&&(ServiceCarStatus.daocheMode)){
+        controlBrake=2000;
+    }
+
     if((dSpeed==0)&&(realSpeed<0.1)){
             final_handbrake=true;
             controlBrake=0;

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -1926,8 +1926,8 @@ double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
     double angle_1 = getQieXianAngle(nowGps,aimGps);
     double x_2 = 0.0, y_2 = 0.0;
     double steering_angle;
-    double l = 2.950;
-    double r =6;
+    double l = 4.0;
+    double r =8;
     double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
     double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
     double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
@@ -2058,9 +2058,9 @@ double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
     }
     steering_angle = atan2(l, r);
 
-    if (x_t_n < 0)
+    if (x_t_f < 0)
     {
-        steering_angle = 0 - steering_angle;
+        steering_angle =  -steering_angle;
     }
 
     nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -509,7 +509,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }else{
             ServiceCarStatus.bocheEnable=2;
         }
-
+//ServiceCarStatus.bocheMode=1;
         if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){
 
             if(abs(realspeed)>0.1){
@@ -544,7 +544,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == reversing)
     {
         double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
-        if (dis<2.0)//0.5
+        if (dis<4.0)//0.5
         {
             vehState = reverseCircle;
             qiedianCount = true;
@@ -557,8 +557,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             gps_decition->wheel_angle = 0;
             gps_decition->speed = dSpeed;
             obsDistance=-1;
-            givlog->debug("decition_brainR","vehState: %d,dSpeed: %f",
-                            vehState,dSpeed);
+            givlog->debug("decition_brainR","vehState: %d,dSpeed: %f,dis: %f",
+                            vehState,dSpeed,dis);
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             //	gps_decition->accelerator = 0;
             return gps_decition;
@@ -569,7 +569,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
         double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+        if((((angdis<10)||(angdis>350)))&&(dis<3.0))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         {
             vehState = reverseDirect;
@@ -578,7 +578,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         }
         else {
-            if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<2000)
                 //         if (qiedianCount && trumpet()<1500)
             {
 
@@ -601,11 +601,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
 
             }
-            controlAng = Compute00().bocheAngle*16.5;
+            controlAng = Compute00().bocheAngle*19.5;
             gps_decition->wheel_angle =controlAng*1.05;
 
-            givlog->debug("decition_brainR","vehState: %d,controlAng: %f,dSpeed: %f",
-                            vehState,controlAng,dSpeed);
+            givlog->debug("decition_brainR","vehState: %d,controlAng: %f,dSpeed: %f,dis: %f,angdis: %f",
+                            vehState,controlAng,dSpeed,dis,angdis);
             cout<<"farTpointDistance================"<<dis<<endl;
 
             return gps_decition;
@@ -616,8 +616,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
-
-        if ((pt.y)<0.5)
+        float pty=pt.y;
+        if ((pt.y)<1.5)
         {
             qiedianCount=true;
             vehState=reverseArr;
@@ -650,8 +650,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             controlAng = 0;
             gps_decition->wheel_angle = 0;
 
-            givlog->debug("decition_brainR","vehState: %d,controlAng: %f",
-                            vehState,controlAng);
+            givlog->debug("decition_brainR","vehState: %d,controlAng: %f,pty: %f",
+                            vehState,controlAng,pty);
             return gps_decition;
         }
     }
@@ -663,7 +663,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         //
 
         ServiceCarStatus.bocheMode=false;
-        if (qiedianCount && trumpet()<1500)
+        if (qiedianCount && trumpet()<3000)
         {
             dSpeed=0;
             minDecelerate=min(minDecelerate,-0.5f);