test vehicletable in hapo. end stop is ok , < 10cm. and change driver_map_xodrload for this test.

src/controller/controller_hapo/controller_hapo.pro

@@ -47,7 +47,7 @@ include($$PWD/control/control.pri)
 INCLUDEPATH += $$PWD/../controllercommon
 
 
-#DEFINES += TORQUEBRAKETEST
+DEFINES += TORQUEBRAKETEST
 
 #unix:!macx: LIBS += -L/home/yuchuli/qt/lib -lncomgnss -ladcmemshare
 

src/controller/controller_hapo/main.cpp

@@ -67,7 +67,7 @@ static QMutex gMutex;
 void executeDecition(const iv::brain::decition decition)
 {
 
-    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
+//    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
 
     gcontroller->control_acc_en(true);
     gcontroller->control_angle_enable(true);
@@ -110,7 +110,7 @@ void executeDecition(const iv::brain::decition decition)
         {
             gcontroller->control_torque(xtb.torque());
             gcontroller->control_brake(xtb.brake());
-//            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
+            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
         }
         else
         {
@@ -132,7 +132,7 @@ void executeDecition(const iv::brain::decition decition)
     }*/
 
 
-qDebug("door is %d",decition.door());
+//qDebug("door is %d",decition.door());
 
 
 

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

@@ -199,6 +199,7 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     lastTorque=(*decition)->torque;
 
 
+    givlog->debug("MIND","speed:%f accAim:%f",now_gps_ins.speed,accAim);
     if(IsINterpolationOK())
     {
         double ftorque,fbrake;

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -962,23 +962,38 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
+    int nmapsize = gpsMapLine.size();
+    double distoend = sqrt(pow(now_gps_ins.gps_x - gpsMapLine[nmapsize-1]->gps_x,2)
+                           +pow(now_gps_ins.gps_y - gpsMapLine[nmapsize-1]->gps_y,2));
 
+    double acc_end = 0.1;
     //2020-01-03, kkcai
     //if(!circleMode && PathPoint>gpsMapLine.size()-200){
-    if(!circleMode && PathPoint>gpsMapLine.size()-500){
-        minDecelerate=-1.0;
+//    if(!circleMode && PathPoint>gpsMapLine.size()-500){
+        if(!circleMode && distoend<50){
+//        minDecelerate=-1.0;
         std::cout<<"map finish brake"<<std::endl;
         double nowspeed = realspeed/3.6;
-        int nmapsize = gpsMapLine.size();
-        double distoend = sqrt(pow(now_gps_ins.gps_x - gpsMapLine[nmapsize-1]->gps_x,2)
-                               +pow(now_gps_ins.gps_y - gpsMapLine[nmapsize-1]->gps_y,2));
+
+//        double distoend = sqrt(pow(now_gps_ins.gps_x - gpsMapLine[nmapsize-1]->gps_x,2)
+//                               +pow(now_gps_ins.gps_y - gpsMapLine[nmapsize-1]->gps_y,2));
         if((distoend<10)||(distoend<(nowspeed*nowspeed)))
         {
-            minDecelerate = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+            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.5)minDecelerate = -3.0;
+        
+
+        if(distoend < 0.1)acc_end = -0.5;
+
+        givlog->debug("MIND","SPEED %f decele : %f",nowspeed,acc_end);
+  //      if(distoend<6.0)minDecelerate = -3.0;
     }
 
+        givlog->debug("MIND","distance to end  : %f",distoend);
+
+
 
 
 
@@ -1990,6 +2005,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     // givlog->debug("SPEED","dSpeed is %f",dSpeed);
     gps_decition->wheel_angle = 0.0 - controlAng;
+
+
+        if(acc_end<0)
+        {
+ //           if(gps_decition->accelerator > acc_end)gps_decition->accelerator = acc_end;
+            if(minDecelerate > acc_end)minDecelerate = acc_end;
+        }
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
@@ -2093,6 +2115,10 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
 
     pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
 
+    if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
+    {
+        decition->accelerator = minDecelerate;
+    }
     if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
         ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){

src/driver/driver_map_xodrload/main.cpp

@@ -493,7 +493,7 @@ void SetPlan(xodrobj xo)
         pp.x = pp.x + fdis * cos(pp.hdg);
         pp.y = pp.y + fdis * sin(pp.hdg);
         pp.nSignal = 23;
-        xPlan.push_back(pp);
+//        xPlan.push_back(pp);
 
     }
 

src/tool/controller_torquebrake_get/mainwindow.cpp

@@ -115,7 +115,7 @@ void MainWindow::UpdateGPSIMU(const char *strdata, const unsigned int nSize, con
         return;
     }
 
-    mfSpeedNow = xgpsimu.speed();
+    mfSpeedNow =  3.6 * sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
     mMutexGPSIMU.lock();
     mgpsimu.CopyFrom(xgpsimu);
     mbGPSIMUUpdate = true;
@@ -128,7 +128,7 @@ void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
 
     double fspeed = 3.6 * sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
     snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f",
-            fspeed,xgpsimu.acce_x() * 9.8,mfTorque,mfBrake);
+            fspeed,xgpsimu.acce_y() * 9.8,mfTorque,mfBrake);
     ui->plainTextEdit->appendPlainText(strout);
 
     double froundspeed = mfVelStep * std::round(fspeed/mfVelStep);
@@ -153,7 +153,7 @@ void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
     }
     else
     {
-        mfVectorAcc.push_back(xgpsimu.acce_x()*9.8);
+        mfVectorAcc.push_back(xgpsimu.acce_y()*9.8);
     }
 }