modify hapo speed control

src/controller/controller_hapo/main.cpp

@@ -90,7 +90,7 @@ void executeDecition(const iv::brain::decition decition)
     gcontroller->control_air_temp(decition.air_temp());
     gcontroller->control_air_mode(decition.air_mode());
     gcontroller->control_wind_level(decition.wind_level());
-    gcontroller->control_roof_light(decition.roof_light());
+    gcontroller->control_roof_light(0);
     gcontroller->control_home_light(decition.home_light());
     gcontroller->control_air_worktime(decition.air_worktime());
     gcontroller->control_air_offtime(decition.air_offtime());

src/decition/decition_brain/decition/adc_adapter/hapo_adapter.cpp

@@ -42,24 +42,24 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     {
         controlSpeed=0;
         controlBrake=u; //102
-        if(obsDistance>0 && obsDistance<10){
+        if(obsDistance>0 && obsDistance<6){
             controlBrake= u*1.5;
         }
-         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around
                  && dSpeed>0 && lastBrake==0){
             controlBrake=0;
             controlSpeed=0;
-        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around){
             controlBrake=min(controlBrake,0.5f);
             controlSpeed=0;
         }
 
-       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>5
                   && !turn_around ){
             controlBrake=min(controlBrake,0.3f);
             controlSpeed=0;
         }
-        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>5
                 && dSpeed>0 && !turn_around ){
             controlBrake=min(controlBrake,0.5f);
             controlSpeed=0;

src/decition/decition_brain/decition/adc_controller/pid_controller.cpp

@@ -166,9 +166,26 @@ float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float
 
     std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
     std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
-    if(obsDistance<10 && obsDistance>0){
+
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
+    {
+            static int obs_line=0;
+            if(obsDistance<6 && obsDistance>0){
+                dSpeed=0;
+                obs_line=1;
+            }
+            if(obs_line==1)
+            {
+                if(obsDistance<8 && obsDistance>0){
+                    dSpeed=0;
+                }else{
+                    obs_line=0;
+                }
+            }
+    }else if(obsDistance<10 && obsDistance>0){
         dSpeed=0;
     }
+
     float dSecSpeed = dSpeed / 3.6;
     float realSpeed=gpsIns.speed;
     float secSpeed =realSpeed/3.6;

src/decition/decition_brain/decition/brain.cpp

@@ -158,7 +158,7 @@ iv::decition::BrainDecition::BrainDecition()
  //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
 
     v2x = iv::modulecomm::RegisterRecv("v2x",iv::decition::Listenv2x);
-    ultra = iv::modulecomm::RegisterRecv("ultra",iv::decition::Listenv2x);
+    ultra = iv::modulecomm::RegisterRecv("ultra",iv::decition::Listenultra);
 
     mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
 

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -184,6 +184,7 @@ std::vector<double> lastDistanceVector(roadSum, -1);
 
 bool qiedianCount = false;
 //日常展示
+static int sensor_update=0;
 
 iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                    const std::vector<GPSData> gpsMapLine,
@@ -233,8 +234,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                                           DecideGps00::minDis,
                                                           DecideGps00::maxAngle);
         DecideGps00::lastGpsIndex = PathPoint;
-
-        adjuseGpsLidarPosition();
+        if(sensor_update==0)
+        {
+            adjuseGpsLidarPosition();
+            sensor_update=1;
+        }
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
@@ -1898,9 +1902,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
 
 
-    if(obsDistance>0 && obsDistance<10){
+    if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
+        if(obsDistance>0 && obsDistance<6){
+                dSpeed=0;
+            }
+    }else if(obsDistance>0 && obsDistance<10){
         dSpeed=0;
     }
+
     //  obsDistance=-1; //1227
 
     if(ServiceCarStatus.group_control){