1.decition 2.controller

src/common/modulecomm/testmodulecomm.pro

@@ -9,7 +9,7 @@ unix:system("./../../../include/linuxsystemtest.sh ")
 unix:include(./../../../include/systemdef.pri)
 win32: DEFINES += SYSTEM_WIN
 
-DEFINES += USE_GROUPUDP
+#DEFINES += USE_GROUPUDP
 
 if(contains(DEFINES,USE_GROUPUDP)){
 QT += network

src/controller/controller_Geely_xingyueL/main.cpp

@@ -30,6 +30,10 @@ GearLevelIndicate ggearRealVal;
 ChassisErrCode gchassErr;
 StsMach gstsMach;
 float gsteerDeg, gspeed;
+double lastspeedSetVal = 0;
+double lastEpsSetVal = 0;
+
+GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;
 
 static void ShareChassis(void * pa,iv::chassis  * pchassis)
 {
@@ -49,33 +53,75 @@ static void ShareChassis(void * pa,iv::chassis  * pchassis)
 
 void executeDecition(const iv::brain::decition decition)
 {
-//    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
-
-    car_control_module.sm_task_20ms();  // 线控状态机函数
-    switch (decition.gear()) {
-    case 1:
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
-        break;
-    case 2:
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
-        break;
-    case 3:
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
-        break;
-    default:
-        car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
-        break;
+    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;
+
+    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
+    double speedSetVal = 0;
+    double EpsSetVal = 0;
+
+    if(decition.has_gear()){
+        switch (decition.gear()) {
+        case 1:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+            gearSetVal=GearPrkgAssistReq::kTargetGearP;
+            break;
+        case 2:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
+            gearSetVal=GearPrkgAssistReq::kTargetGearR;
+            break;
+        case 3:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
+            gearSetVal=GearPrkgAssistReq::kTargetGearD;
+            break;
+        default:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
+            gearSetVal=GearPrkgAssistReq::kNoRequest;;
+            break;
+        }
+        lastgearSetVal=gearSetVal;
+    }
+    else
+    {
+        gearSetVal=lastgearSetVal;
     }
 
-    car_control_module.set_target_acc_mps2(decition.accelerator());
-    car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
-    if(decition.leftlamp()==true)
+    if(decition.has_accelerator()){
+        speedSetVal=decition.accelerator();
+        lastspeedSetVal=speedSetVal;
+        //speedSetVal=0.1;
+       // car_control_module.set_target_acc_mps2(decition.accelerator());
+//        car_control_module.set_target_acc_mps2(0.1);
+    }
+    else
+    {
+        speedSetVal=lastspeedSetVal;
+    }
+
+    if(decition.has_wheelangle())
+    {
+       // car_control_module.set_target_pinion_ag_in_deg(0.0);
+        EpsSetVal=decition.wheelangle();
+        lastEpsSetVal=EpsSetVal;
+       // EpsSetVal=0.0;//
+  // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
+    }
+    else
+    {
+        EpsSetVal=lastEpsSetVal;
+    }
+    // gearSetVal=GearPrkgAssistReq::kTargetGearD;
+    car_control_module.set_target_gear(gearSetVal);
+    car_control_module.set_target_acc_mps2(speedSetVal);
+    car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
+
+    if(decition.has_leftlamp() && decition.leftlamp()==true)
         car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
-    else if(decition.rightlamp()==true)
+    else if(decition.has_rightlamp() && decition.rightlamp()==true)
         car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
     else
         car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
-/*
+
+    /*
     void set_lat_lgt_ctrl_req(bool req);  // 握手请求, true:请求握手， false:退出握手
     void set_target_gear(GearPrkgAssistReq tar);  // 目标档位请求
     bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
@@ -104,18 +150,22 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
         qDebug("not D");
     }
     if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
-        qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-    oldtime = QDateTime::currentMSecsSinceEpoch();
+        // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        oldtime = QDateTime::currentMSecsSinceEpoch();
     gMutex.lock();
     gdecition.CopyFrom(xdecition);
     gMutex.unlock();
     gnDecitionNum = gnDecitionNumMax;
+    std::cout<<"update decision. "<<std::endl;
 }
 
 
 void sendthread()
 {
     iv::brain::decition xdecition;
+//    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
+//    car_control_module.sm_task_20ms();  // 线控状态机函数
+
     while(1)
     {
         if(gnDecitionNum <= 0)
@@ -130,10 +180,22 @@ void sendthread()
             gnDecitionNum--;
         }
 
-        gstatus = car_control_module.is_lat_lgt_ctrl_active();
-        executeDecition(xdecition);
+        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
+
+        if(bstatus == true)
+        {
+            //            std::cout<<"di pan ke kong "<<std::endl;
+            executeDecition(xdecition);
+        }
+        else
+        {
+            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
+                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
+            car_control_module.set_lat_lgt_ctrl_req(true);
+        }
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        car_control_module.sm_task_20ms();  // 线控状态机函数
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
     }
 }
 
@@ -142,6 +204,7 @@ void * gpa;
 void recvthread()
 {
     iv::chassis xchassis;
+    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
     while(1)
     {
         gstatus = car_control_module.is_lat_lgt_ctrl_active();
@@ -152,6 +215,9 @@ void recvthread()
         gspeed = car_control_module.get_current_vehicle_spd_in_ms();
         gsteerDeg = car_control_module.get_current_steer_ang_in_deg();
 
+        std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;
+
+
         xchassis.set_angle_feedback(gsteerDeg);
 
         ShareChassis(gpa,&xchassis);
@@ -182,8 +248,10 @@ int main(int argc, char *argv[])
         strpath = argv[1];
     std::cout<<strpath.toStdString()<<std::endl;
 
-    car_control_module.set_lat_lgt_ctrl_req(true);
-    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+    //    car_control_module.set_lat_lgt_ctrl_req(true);
+    //    car_control_module.sm_task_20ms();  // 线控状态机函数
+
+    //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
 
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
     std::string gstrmemdecition = xp.GetParam("dection","deciton");
@@ -193,7 +261,7 @@ int main(int argc, char *argv[])
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
 
     std::thread xthread(sendthread);
-    std::thread myxthread(recvthread);
+//    std::thread myxthread(recvthread);
 
     return a.exec();
 }

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

@@ -40,33 +40,33 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
     }
     if (accAim < 0)
     {
-        controlSpeed=0;
-        controlBrake=u; //102
-        if(obsDistance>0 && obsDistance<10){
-            controlBrake= u*1.0;     //1.5    zj-1.2
-        }
-        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
-            controlBrake=0;
-            controlSpeed=max(0.0,ServiceCarStatus.torque_st-2.0);
-        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && 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){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=0;
-        }
+//        controlSpeed=0;
+//        controlBrake=u; //102
+//        if(obsDistance>0 && obsDistance<10){
+//            controlBrake= u*1.0;     //1.5    zj-1.2
+//        }
+//        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+//            controlBrake=0;
+//            controlSpeed=max(0.0,ServiceCarStatus.torque_st-2.0);
+//        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && 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){
+//            controlBrake=min(controlBrake,0.5f);
+//            controlSpeed=0;
+//        }
 
-       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
-                  && !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
-                && 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
+//                  && !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
+//                && dSpeed>0 && !turn_around ){
+//            controlBrake=min(controlBrake,0.5f);
+//            controlSpeed=0;
+//        }
 
 
         //0110
@@ -94,6 +94,13 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
     }
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
+    if(realSpeed<6.0)//xw
+    {
+        controlSpeed=min(controlSpeed,0.1f);
+    }
+
+    //controlSpeed=0.1;
+
     (*decition)->accelerator=controlSpeed;
 
     if((*decition)->leftlamp){
@@ -119,11 +126,16 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
     }
 
 
-
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=3;
+    }
 
 std::cout<<"==========================================="<<std::endl;
-     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
-    <<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<std::endl;
 std::cout<<"========================================="<<std::endl;
 
     return *decition;

src/driver/driver_gps_hcp2/hcp2.cpp

@@ -206,7 +206,7 @@ void hcp2::ins550dDecode()
     data2 = CalcValue(pdata,ins550DecodeType::i16,48,1.0);
     data3 = CalcValue(pdata,ins550DecodeType::i16,50,1.0);
  //   std::cout<<" data1="<<data1<<"  data2="<<data2<<"  data3="<<data3<<std::endl;
-//    std::cout<<"type is "<<(int)type<<std::endl;
+    std::cout<<"type is "<<(int)type<<std::endl;
     switch (type) {
     case 0:
         latstd = pow(2.718281,data1/100.0);

src/driver/driver_gps_hcp2/main.cpp

@@ -24,7 +24,7 @@ int main(int argc, char *argv[])
     std::cout<<strpath.toStdString()<<std::endl;
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
-    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB0");
+    std::string strdevname = xp.GetParam("devname","/dev/ttysWK0");
     std::string strgpsimumemname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
     std::string strgpsmemname = xp.GetParam("msg_gps","hcp2_gps");
     std::string strimumemname = xp.GetParam("msg_imu","hcp2_imu");

src/tool/carControlTester/carControlTester.pro

@@ -11,6 +11,8 @@ greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
 TARGET = carControlTester
 TEMPLATE = app
 
+QMAKE_LFLAGS += -no-pie
+
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which has been marked as deprecated (the exact warnings
 # depend on your compiler). Please consult the documentation of the

src/tool/carControlTester/mainwindow.cpp

@@ -16,7 +16,7 @@ MainWindow::MainWindow(QWidget *parent) :
     QTimer * timer = new QTimer();
     connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
     timer->setTimerType(Qt::PreciseTimer);
-    timer->start(200);
+    timer->start(50);
 }
 
 MainWindow::~MainWindow()
@@ -171,7 +171,12 @@ void MainWindow::onTimerTask()
     else if(car_control_module.is_lat_lgt_ctrl_active() && bctrlAct){
         car_control_module.set_target_gear( gearSetVal);
         car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
-        car_control_module.set_target_acc_mps2(speedSetVal);
+
+        double speed = car_control_module.get_current_vehicle_spd_in_ms();
+        if(speed < 10)
+            car_control_module.set_target_acc_mps2(speedSetVal);
+        else
+            car_control_module.set_target_acc_mps2(0);
         car_control_module.set_turn_light_status(turnLightSetVal);
     }
     car_control_module.sm_task_20ms();

src/tool/carControlTester/mainwindow.ui

@@ -48,8 +48,14 @@
           <property name="decimals">
            <number>1</number>
           </property>
+          <property name="minimum">
+           <double>-1.000000000000000</double>
+          </property>
           <property name="maximum">
-           <double>30.000000000000000</double>
+           <double>1.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.100000000000000</double>
           </property>
          </widget>
          <widget class="QSlider" name="vSliderSpeedSet">
@@ -64,11 +70,14 @@
             <height>201</height>
            </rect>
           </property>
+          <property name="minimum">
+           <number>-1</number>
+          </property>
           <property name="maximum">
-           <number>30</number>
+           <number>1</number>
           </property>
           <property name="pageStep">
-           <number>2</number>
+           <number>0</number>
           </property>
           <property name="orientation">
            <enum>Qt::Vertical</enum>
@@ -119,13 +128,13 @@
            <number>1</number>
           </property>
           <property name="minimum">
-           <double>-400.000000000000000</double>
+           <double>-45.000000000000000</double>
           </property>
           <property name="maximum">
-           <double>400.000000000000000</double>
+           <double>45.000000000000000</double>
           </property>
           <property name="singleStep">
-           <double>10.000000000000000</double>
+           <double>5.000000000000000</double>
           </property>
          </widget>
         </widget>