modify cross pro

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -916,6 +916,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     obsDistanceAvoid = -1;
     esrIndexAvoid = -1;
     roadPre = -1;
+    dSpeed = 80;
 
     gpsTraceOri.clear();
     gpsTraceNow.clear();
@@ -956,31 +957,61 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     if(!circleMode){
-            if(PathPoint+500<gpsMapLine.size())
+
+            double forecast_final=secSpeed*secSpeed+5;
+            int forecast_final_point=((int)forecast_final)*10;
+            static int BrakePoint=-1;
+            static bool final_brake=false,final_brake_lock=false;
+            if(PathPoint+forecast_final_point<gpsMapLine.size())
             {
-                if(gpsMapLine[PathPoint+500]->mode2==23 && realspeed>20)
-                                        minDecelerate=-0.5;
-                else if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
-                                        minDecelerate=-0.5;
-                else if(gpsMapLine[PathPoint+100]->mode2==23)
-                                        minDecelerate=-0.6;
-            }
-            else if(PathPoint+300<gpsMapLine.size()){
-                if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
-                                        minDecelerate=-0.5;
-                else if(gpsMapLine[PathPoint+100]->mode2==23)
-                                        minDecelerate=-0.6;
-            }
-            else if(PathPoint+150<gpsMapLine.size()){
-                if(gpsMapLine[PathPoint+150]->mode2==23 && realspeed>10)
-                                        minDecelerate=-0.5;
-                else if(gpsMapLine[PathPoint+100]->mode2==23)
-                                        minDecelerate=-0.6;
+                if(gpsMapLine[PathPoint+forecast_final_point]->mode2==23 && realspeed>3){
+                                            final_brake=true;
+                                            if(BrakePoint==-1)
+                                                 BrakePoint=PathPoint-10;
+                }
             }
-            else if(PathPoint+100<gpsMapLine.size()){
-                    if(gpsMapLine[PathPoint+100]->mode2==23)
-                                        minDecelerate=-0.6;
+            if(PathPoint<BrakePoint)
+            {
+                final_brake=false;
+                final_brake_lock=false;
+                BrakePoint=-1;
+            }
+            if(final_brake==true){
+                    if((realspeed>3)&&(final_brake_lock==false)){
+                                minDecelerate=-0.5;
+                    }else{
+                                dSpeed=min(dSpeed, 3.0);
+                                final_brake_lock=true;
+                                if(gpsMapLine[PathPoint]->mode2==23){
+                                            minDecelerate=-0.7;
+                                }
+                    }
             }
+//            if(PathPoint+500<gpsMapLine.size())
+//            {
+//                if(gpsMapLine[PathPoint+500]->mode2==23 && realspeed>20)
+//                                        minDecelerate=-0.5;
+//                else if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
+//                                        minDecelerate=-0.5;
+//                else if(gpsMapLine[PathPoint+100]->mode2==23)
+//                                        minDecelerate=-0.6;
+//            }
+//            else if(PathPoint+300<gpsMapLine.size()){
+//                if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
+//                                        minDecelerate=-0.5;
+//                else if(gpsMapLine[PathPoint+100]->mode2==23)
+//                                        minDecelerate=-0.6;
+//            }
+//            else if(PathPoint+150<gpsMapLine.size()){
+//                if(gpsMapLine[PathPoint+150]->mode2==23 && realspeed>10)
+//                                        minDecelerate=-0.5;
+//                else if(gpsMapLine[PathPoint+100]->mode2==23)
+//                                        minDecelerate=-0.6;
+//            }
+//            else if(PathPoint+100<gpsMapLine.size()){
+//                    if(gpsMapLine[PathPoint+100]->mode2==23)
+//                                        minDecelerate=-0.6;
+//            }
     }
 
     if(!ServiceCarStatus.inRoadAvoid){
@@ -1110,7 +1141,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //  dSpeed = getSpeed(gpsTraceNow);
-    dSpeed = 80;
 
     planTrace.clear();//Add By YuChuli, 2020.11.26
     for(int i=0;i<gpsTraceNow.size();i++){
@@ -3724,6 +3754,17 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
     static int obstacle_disable=0;
     static int speed_slowdown_flag=0;
     static bool lock_flag=false;
+    double forecast_distance=0;
+    int forecast_point_num;
+
+    double secLowSpeed=ServiceCarStatus.mroadmode_vel.mfmode18/3.6;   //m/s
+    if(secSpeed>secLowSpeed)
+    {
+        forecast_distance=secSpeed*secSpeed-secLowSpeed*secLowSpeed+5;
+        forecast_point_num=((int)forecast_distance)*10;
+        if((PathPoint+forecast_point_num+2)>gpsMapLine.size())
+                                                forecast_point_num=0;
+    }
 
     if(  gpsMapLine[PathPoint]->mode2==3000){
         if(obsDistance>4){       //7   zj-5
@@ -3744,7 +3785,7 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
              obstacle_disable=0;
     }else if(gpsMapLine[PathPoint]->mode2==4000){
         ServiceCarStatus.msysparam.vehWidth=5.6;
-    }else if(gpsMapLine[PathPoint]->mode2==5000){
+    }else if((gpsMapLine[PathPoint+forecast_point_num]->mode2==5000)||(gpsMapLine[PathPoint+forecast_point_num+1]->mode2==5000)||(gpsMapLine[PathPoint+forecast_point_num-1]->mode2==5000)){
             speed_slowdown_flag=1;
             lock_flag=false;
     }else if(gpsMapLine[PathPoint]->mode2==5001){