modify table look-up pro

src/decition/common/common/car_status.h

@@ -91,6 +91,7 @@ namespace iv {
 
               float mfttc = 0;
               bool txt_log_on=false;
+              bool table_look_up_on=false;
 
 
         std::vector <iv::platform::station> car_stations;

src/decition/decition_brain_sf/decition/adc_adapter/hapo_adapter.cpp

@@ -202,7 +202,7 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
 
 
 
-    if(IsINterpolationOK())
+    if((ServiceCarStatus.table_look_up_on==true)&&(IsINterpolationOK()))
     {
         double ftorque,fbrake;
         GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);

src/decition/decition_brain_sf/decition/adc_controller/pid_controller.cpp

@@ -99,8 +99,8 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
             break;
         }
     }
+    if(gpsIndex == 0)   gpsIndex = nsize -1;
 
-    if(gpsIndex == 0)gpsIndex = nsize -1;
     EPos = trace[gpsIndex].x;
 
     for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), trace.size()); i++) {

src/decition/decition_brain_sf/decition/brain.cpp

@@ -332,6 +332,13 @@ void iv::decition::BrainDecition::run() {
         //将数据写入文件结束
     }
 
+    std::string table_look_up_on= xp.GetParam("table","false");
+    if(table_look_up_on=="true"){
+        ServiceCarStatus.table_look_up_on=true;
+    }else{
+        ServiceCarStatus.table_look_up_on=false;
+    }
+
     std::string strparklat = xp.GetParam("parklat","39.0624557");
     std::string strparklng = xp.GetParam("parklng","117.3575493");
     std::string strparkheading = xp.GetParam("parkheading","112.5");

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -966,59 +966,59 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double acc_end = 0.1;
 
-    if(1)//(IsINterpolationOK())
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     {
                 if(!circleMode && distoend<50){
-                double nowspeed = realspeed/3.6;
-                if((distoend<10)||(distoend<(nowspeed*nowspeed)))
-                {
-                    if(distoend == 0.0)distoend = 0.1;
-                    acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
-                    if(acc_end<(-3.0))acc_end = -3.0;
-                }
-
-                if(distoend < 0.1)acc_end = -0.5;
+                        double nowspeed = realspeed/3.6;
+                        if((distoend<10)||(distoend<(nowspeed*nowspeed)))
+                        {
+                            if(distoend == 0.0)distoend = 0.1;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if(acc_end<(-3.0))acc_end = -3.0;
+                        }
+
+                        if(distoend < 0.1)acc_end = -0.5;
                 }
     }else{
-    if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
-            minDecelerate=-0.5;
-            std::cout<<"map finish brake"<<std::endl;
-    }
-
-    if(!circleMode){
-
-            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+forecast_final_point]->mode2==23 && realspeed>3){
-                                            final_brake=true;
-                                            if(BrakePoint==-1)
-                                                 BrakePoint=PathPoint-10;
+                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
+                        minDecelerate=-0.5;
+                        std::cout<<"map finish brake"<<std::endl;
                 }
-            }
-            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(PathPoint+50<gpsMapLine.size()){
-                                    if(gpsMapLine[PathPoint+50]->mode2==23){
-                                                minDecelerate=-0.5;
-                                    }
+
+                if(!circleMode){
+
+                        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+forecast_final_point]->mode2==23 && realspeed>3){
+                                                        final_brake=true;
+                                                        if(BrakePoint==-1)
+                                                             BrakePoint=PathPoint-10;
+                            }
+                        }
+                        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(PathPoint+50<gpsMapLine.size()){
+                                                if(gpsMapLine[PathPoint+50]->mode2==23){
+                                                            minDecelerate=-0.5;
+                                                }
+                                            }
                                 }
-                    }
-            }
-    }
+                        }
+                }
     }
 
     if(!ServiceCarStatus.inRoadAvoid){
@@ -1179,16 +1179,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-//    if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
-//        if(realspeed<0.5)
-//            controlAng=0;
-//    }
 
-    if(!circleMode && PathPoint>(gpsMapLine.size()-20)){
-        controlAng=0;
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK())){
+        if(!circleMode && PathPoint>(gpsMapLine.size()-20)){
+            controlAng=0;
+        }
+    }else{
+        if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
+           if(realspeed<0.5)
+                controlAng=0;
+        }
     }
 
-
     //1220
     if(ServiceCarStatus.daocheMode){
         controlAng=0-controlAng;
@@ -2011,12 +2013,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     gps_decition->wheel_angle = 0.0 - controlAng;
-    if(1)//(IsINterpolationOK())
-    {
-    if(acc_end<0)
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     {
-        if(minDecelerate > acc_end)minDecelerate = acc_end;
-    }
+        if(acc_end<0)
+        {
+            if(minDecelerate > acc_end) minDecelerate = acc_end;
+        }
     }
 
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
@@ -2080,7 +2082,7 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
 
     pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
 
-    if(1)//(IsINterpolationOK())
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     {
         if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
         {