modify adapter brake

src/decition/decition_brain_sf/decition/adc_adapter/sightseeing_adapter.cpp

@@ -31,6 +31,7 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
     float realSpeed = now_gps_ins.speed;
     float ttc = 0-(obsDistance/obsSpeed);
     bool turn_around=false;
+    bool final_handbrake=false;
     if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
         turn_around=true;
     }
@@ -96,30 +97,6 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
         }else{
             controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
         }
-
-        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
-        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
-        //            if(realSpeed<5  ){
-        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-        //            }else if(realSpeed<10){
-        //                controlSpeed=min((float)25.0,controlSpeed);  //40
-        //            }
-        //        }
-
-
-        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
-
-//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
-//            if(controlSpeed>0){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
-//            if(controlSpeed>0 ){
-//                controlSpeed = ServiceCarStatus.torque_st;
-//            }
-//        }
-
-
         if(controlSpeed>ServiceCarStatus.torque_st){
             controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
         }else if(controlSpeed<ServiceCarStatus.torque_st){
@@ -215,35 +192,53 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
     }
 
 
-    (*decition)->brake = controlBrake*9;//9     zj-6
-    (*decition)->torque=dSpeed;
-    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;
+    controlBrake=0;
+    if(obsDistance>10 && obsDistance<15){
+            dSpeed=2;
+    }else if(obsDistance>5 && obsDistance<=10){
+            dSpeed=1;
+    }else if(obsDistance>0 && obsDistance<=5){
+            dSpeed=0;
+    }
 
-        (*decition)->torque= ftorque;
+    if((dSpeed==0)&&(realSpeed<0.2)){
+            final_handbrake=true;
+    }else{
+            final_handbrake=false;
     }
+
+    (*decition)->brake = controlBrake;
+    (*decition)->torque= dSpeed;
+    //(*decition)->brake = controlBrake*9;//9     zj-6
 //    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
 //                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
 
+    //垃圾车，控制输出，手刹&档位
+    if(final_handbrake==true){
+        (*decition)->handBrake=1;  //手刹 0: shifang, 1: laqi
+    }else{
+        (*decition)->handBrake=0;  //手刹 0: shifang, 1: laqi
+    }
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=4;
+    }else{
+        (*decition)->dangWei=2;
+    }
+
+    //垃圾车，控制输出，角度&角速度
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+    (*decition)->wheel_angle=max((float)-540.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)540.0,(*decition)->wheel_angle);
+    (*decition)->angSpeed=600;
 
+    //日志输出
+    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f,accAim: %f,handBrake: %d,dangWei: %d",
+                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque,accAim,(*decition)->handBrake,(*decition)->dangWei);
 
+    //垃圾车，控制输出，其它
     (*decition)->grade=1;
     (*decition)->mode=1;
     (*decition)->speak=0;
-
     (*decition)->bright=false;
     (*decition)->engine=0;
     (*decition)->dangweiEnable=true;
@@ -253,88 +248,14 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
     (*decition)->air_enable = false;
     (*decition)->driveMode=1;
     (*decition)->brakeType=0;
-    (*decition)->angSpeed=60;
     (*decition)->topLight=0;
 
-    (*decition)->handBrake=0;  // 0: shifang 1: laqi
-    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
-        (*decition)->dangWei=4;
-        (*decition)->backLight=1;
-    }
-    //1220
-    else{
-        (*decition)->dangWei=2;
-        (*decition)->backLight=0;
-    }
-
-    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f,accAim: %f,handBrake: %d,dangWei: %d",
-                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque,accAim,(*decition)->handBrake,(*decition)->dangWei);
-
-    if((*decition)->brake>0){
-        (*decition)->brakeLight=1;
-    }else{
-        (*decition)->brakeLight=0;
-    }
-
-
-    if((*decition)->leftlamp){
-        (*decition)->directLight=1;
-    }else if((*decition)->rightlamp){
-         (*decition)->directLight=2;
-    }else{
-        (*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;
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
 
-    (*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)->accelerator=  (*decition)->torque ;
     return *decition;
 }
 

src/decition/decition_brain_sf/decition/brain.cpp

@@ -511,7 +511,8 @@ void iv::decition::BrainDecition::run() {
         if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
         {
             ServiceCarStatus.mbBrainCtring = false;
-            decition_gps->brake = 6;
+            decition_gps->brake = 0;
+            decition_gps->handBrake=1;  //手刹 0: shifang 1: laqi
             decition_gps->accelerator = -0.5;
             decition_gps->wheel_angle = 0;
             decition_gps->torque = 0;
@@ -903,7 +904,8 @@ void iv::decition::BrainDecition::run() {
 
         if (handPark && decition_gps != NULL)
         {
-            decition_gps->brake = 20;
+            decition_gps->brake = 0;
+            decition_gps->handBrake=1;  //手刹 0: shifang 1: laqi
             decition_gps->accelerator = 0;
             //			decitionExecuter->executeDecition(decition_gps);
             ShareDecition(decition_gps);

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -376,7 +376,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         gps_decition->speed = dSpeed;
 
         gps_decition->accelerator = -0.5;
-        gps_decition->brake=10;
+        gps_decition->brake=0;
         return gps_decition;
     }
 
@@ -1165,10 +1165,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if(gpsTraceOri.empty()){
         gps_decition->wheel_angle = 0;
-        gps_decition->speed = dSpeed;
+        gps_decition->speed = 0;
 
         gps_decition->accelerator = -0.5;
-        gps_decition->brake=10;
+        gps_decition->brake=0;
         return gps_decition;
     }
 
@@ -2273,9 +2273,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(obsDistance>0 && obsDistance<8){
                 dSpeed=0;
             }
-    }else if(obsDistance>0 && obsDistance<15){
-        dSpeed=0;
     }
+//    else if(obsDistance>0 && obsDistance<15){
+//        dSpeed=0;
+//    }
 
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;