按照vv7的逻辑进行代码适配，加速度控车

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_adapter/jeely_xingyueL_adapter.cpp

@@ -23,9 +23,9 @@ iv::decition::Jeely_xingyueL_adapter::~Jeely_xingyueL_adapter(){
 
 iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                         float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
-    accAim=min(0.7f,accAim);
 
-    float controlSpeed=0;
+
+    float controlSpeed=accAim;
     float controlBrake=0;
     float u = 0- accAim;
     float realSpeed = now_gps_ins.speed;
@@ -71,175 +71,30 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
 
         //0110
         if(changingDangWei){
-            controlBrake=max(0.5f,controlBrake);
-        }
-
-        //斜坡刹车加大 lsn 0217
-        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
-            controlBrake=max(0.5f,controlBrake);
+            controlSpeed=min(-0.5f,controlSpeed);
         }
-        //斜坡刹车加大 end
-
-
-
-
-
+//        else if((obsDistance<0 ||obsDistance>50)&&(abs(dSpeed-realSpeed)<3)){
+//             controlSpeed=max(-0.2f,controlSpeed);
+//        }
     }
     else
     {
-        controlBrake = 0;
-
-        if(ServiceCarStatus.torque_st==0){
-            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
-            controlSpeed =min( controlSpeed,2.0f);
-
-        }else{
-            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
-        }
-
-
-
-        if(controlSpeed>ServiceCarStatus.torque_st){
-            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
-        }else if(controlSpeed<ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st-1.0;
-        }
-
-
-
-
-        //Seizing
-        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
-            seizingCount++;
-        }else{
-            seizingCount=0;
-        }
-        if(seizingCount>=10){
-            controlSpeed=ServiceCarStatus.torque_st+2;
-        }
-
-        // 斜坡加大油门   0217 lsn
-
-        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && (dSpeed-realSpeed)>3.0){
-            controlSpeed=max((float)30.0,controlSpeed);
-            controlSpeed=min((float)40.0,controlSpeed);
-        }
-        // 斜坡加大油门  end
-
-
-        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realSpeed)<10.0){
-            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        }
+        controlSpeed=min(1.0f,controlSpeed);
 
     }
 
-    if(controlSpeed>0){
-        controlSpeed=max(controlSpeed,3.2f);
-    }
-    static bool emergency_brake=false;
     //0227 10m nei xianzhi shache
-    if(obsDistance<8 &&obsDistance>0){
-        emergency_brake=true;
-    }
-    if(emergency_brake==true){
-        controlSpeed=0;
-        if(realSpeed<6)
-            controlBrake=max(controlBrake,0.5f);//0.8   zj-0.6
-        else
-            controlBrake=max(controlBrake,0.6f);
-        if(obsDistance>12 || (obsDistance==-1)){
-            emergency_brake=false;
-        }
+    if(obsDistance<10 &&obsDistance>0){
+        controlSpeed=min(controlSpeed,-0.8f);
     }
 
+
     if(DecideGps00::minDecelerate<0){
-        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
-        controlSpeed=0;
+        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
     }
-
-
-    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
-    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
-    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
-        controlBrake =0;
-        controlSpeed =0;
-        accAim=0;
-    }
-    if(obsDistance>60){
-        if(accAim<-0.5){
-            accAim=max(accAim,-0.5f);
-        }
-    }
-
-
-    (*decition)->brake = controlBrake*9;//9     zj-6
-    (*decition)->torque=controlSpeed;
-    lastBrake= (*decition)->brake;
-    lastTorque=(*decition)->torque;
-
-
-
-
-
-    if((ServiceCarStatus.table_look_up_on==true)&&(IsINterpolationOK()))
-    {
-        double ftorque,fbrake;
-        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
-        if(accAim>0){
-            ftorque=max(ftorque,3.2);
-        }
-        (*decition)->brake = fbrake;
-
-        (*decition)->torque= ftorque;
-    }
-//    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
-//                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
-
-//    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
-//                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
-
-    (*decition)->grade=1;
-    (*decition)->mode=1;
-    (*decition)->speak=0;
-    (*decition)->handBrake=1;  // 0: laqi 1: shifang
-    (*decition)->bright=false;
-    (*decition)->engine=0;
-
-    (*decition)->dangweiEnable=true;
-    (*decition)->angleEnable=true;
-    (*decition)->speedEnable=true;
-    (*decition)-> brakeEnable=true;
-    (*decition)->air_enable = false;
-    (*decition)->driveMode=1;
-    (*decition)->brakeType=0;
-    (*decition)->angSpeed=60;
-    (*decition)->topLight=0;
-
-
-    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
-        (*decition)->dangWei=2;
-        (*decition)->backLight=1;
-    }
-    //1220
-    else{
-        (*decition)->dangWei=4;
-        (*decition)->backLight=0;
-    }
-
-
-
-    if((*decition)->brake>0){
-        (*decition)->brakeLight=1;
-    }else{
-        (*decition)->brakeLight=0;
-    }
-
+    (*decition)->accelerator=controlSpeed;
 
     if((*decition)->leftlamp){
         (*decition)->directLight=1;
@@ -249,56 +104,28 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
         (*decition)->directLight=0;
     }
 
-    (*decition)->handBrake=false;
+
     (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
 
-    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
-    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
-    lastDangWei= (*decition)->dangWei;
-
-    (*decition)->topLight=0; //1116
-    (*decition)->nearLight=0;
-    (*decition)->farLight=0;
-    (*decition)->home_light=0;
-    (*decition)->roof_light=0;
-
-     static int64_t DoorTimeStart=-1;
-     static int32_t door=0;
-     if(ServiceCarStatus.has_mbdoor){
-         if(ServiceCarStatus.mbdoor){
-             door=2;
-             //(*decition)->door=2;
-             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
-             ServiceCarStatus.has_mbdoor=0;
-         }else{
-             door=3;
-            // (*decition)->door=3;
-             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
-             ServiceCarStatus.has_mbdoor=0;
-         }
-     }
-
-     if((QDateTime::currentSecsSinceEpoch()-DoorTimeStart)>10)
-             (*decition)->door=0;
-     else
-              (*decition)->door=door;
-
-
-     if(ServiceCarStatus.has_mbjinguang){
-         if(ServiceCarStatus.mbjinguang){
-             (*decition)->nearLight=1;
-         }else{
-             (*decition)->nearLight=0;
-         }
-     }
-
-//std::cout<<"==========================================="<<std::endl;
-//     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
-//             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
-//    <<std::endl;
-//std::cout<<"========================================="<<std::endl;
-
-   (*decition)->accelerator=  (*decition)->torque ;
+    (*decition)->wheel_angle=max((float)-450.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)450.0,(*decition)->wheel_angle);
+
+    if((*decition)->accelerator>=0){
+        (*decition)->torque= (*decition)->accelerator;
+        (*decition)->brake=0;
+    }else{
+        (*decition)->torque=0;
+        (*decition)->brake=0-(*decition)->accelerator;
+    }
+
+
+
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
     return *decition;
 }
 
@@ -322,8 +149,12 @@ float iv::decition::Jeely_xingyueL_adapter::limitBrake(float realSpeed, float co
         BrakeIntMax = 3;
     }else if(ttc>3){
         BrakeIntMax = 4;
-    }else {
+    }else if(ttc>1.6){
         BrakeIntMax = 5;
+    }else if(ttc>0.8){
+        BrakeIntMax = 8;
+    }else{
+        BrakeIntMax = 10;
     }
     if(obsDistance>0 && obsDistance<10){
         BrakeIntMax=max(BrakeIntMax,3);
@@ -334,16 +165,23 @@ float iv::decition::Jeely_xingyueL_adapter::limitBrake(float realSpeed, float co
 
 
 
-    controlBrake=min(5.0f,controlBrake);
+    controlBrake=min(10.0f,controlBrake);
     controlBrake=max(0.0f,controlBrake);
     return controlBrake;
 
 }
 
 float iv::decition::Jeely_xingyueL_adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
-
-    controlSpeed=min((float)100.0,controlSpeed);
-    controlSpeed=max((float)0.0,controlSpeed);
+//    controlSpeed=min((float)5.0,controlSpeed);
+//    controlSpeed=max((float)-7.0,controlSpeed);
+    if(realSpeed >= 0.0 && realSpeed <= 6.0){
+        controlSpeed=min((float)1.27,controlSpeed);
+        controlSpeed=max((float)-1.28,controlSpeed);
+    }
+    if(realSpeed > 6.0){
+        controlSpeed=min((float)12.0,controlSpeed);
+        controlSpeed=max((float)-12.0,controlSpeed);
+    }
     return controlSpeed;
 }