decition_brain_sf.

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -933,32 +933,68 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double acc_end = 0.1;
     static double distoend=1000;
-    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    {
+//        if(PathPoint+1000>gpsMapLine.size())
+//        {
+//            distoend=computeDistToEnd(gpsMapLine,PathPoint);
+//        }
+//        else
+//        {
+//            distoend=1000;
+//        }
+//        if(!circleMode && distoend<50)
+//        {
+//            double nowspeed = realspeed/3.6;
+//            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
+//            {
+//                if(distoend == 0.0)
+//                    distoend = 0.09;
+//                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+//                if(acc_end<(-3.0))
+//                    acc_end = -3.0;
+//            }
+
+//            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
+//                acc_end = -0.5;
+//        }
+//    }
+    int useaccend = 1;
+    double mstopbrake=0.3;
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
     {
-        if(PathPoint+1000>gpsMapLine.size())
-        {
-            distoend=computeDistToEnd(gpsMapLine,PathPoint);
-        }
-        else
-        {
-            distoend=1000;
-        }
-        if(!circleMode && distoend<50)
-        {
-            double nowspeed = realspeed/3.6;
-            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
-            {
-                if(distoend == 0.0)
-                    distoend = 0.09;
-                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
-                if(acc_end<(-3.0))
-                    acc_end = -3.0;
-            }
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<100){
+                        double nowspeed = realspeed/3.6;
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                        }
 
-            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
-                acc_end = -0.5;
-        }
-    }else{
+                        if((distoend < 0.5)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
+                }
+    }
+    else{
         //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
         //                        minDecelerate=-0.5;
         //                        std::cout<<"map finish brake"<<std::endl;
@@ -1008,7 +1044,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 {
                     if(enterTofinal==false)  //20230417,shenlan)
                     {
-                        enterTofinal_speed=realspeed-0.5;
+                        enterTofinal_speed=realspeed;
                         enterTofinal=true;
                     }
                     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
@@ -1021,9 +1057,20 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                     }
                     final_brake_lock=true;
                     brake_mode=true;
-                    if(distance_to_end<0.8)
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
                     {
-                        minDecelerate=-0.7;
+                        if(distance_to_end<2.0)
+                        {
+                            minDecelerate=-0.7;
+                        }
+
+                    }
+                    else
+                    {
+                        if(distance_to_end<0.8)
+                        {
+                            minDecelerate=-0.7;
+                        }
                     }
                 }
             }