change controller_chery_sterra_es_fcm. test pid.

src/controller/controller_chery_sterra_es_fcm/dbcsigpacker.cpp

@@ -30,7 +30,7 @@ void dbcsigpacker::SetMsgSignal(std::string  strmsgname,std::string  strsigname,
     for(i=0;i<nsize;i++){
         if(xMsgSig.mvectorSPV[i].name == strsigname){
             xMsgSig.mvectorSPV[i].value = fvalue;
-            break;
+            return;
         }
     }
 

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -85,9 +85,9 @@ double gfBrakeReq = 0.0;
 static bool gbSendBrake = false;
 
 
-static double kp = 0.1;
+static double kp = 0.5;
 static double ki = 0.0;
-static double kd = 0.0;
+static double kd =  0.0;
 static double MAXACC = 3.0;
 static int64_t gnLastAccUptime = 0;
 static bool gbUsePID = true;
@@ -121,6 +121,7 @@ double  PIDTorque(double torque)
     if((nnow - gnLastAccUptime)>1e9)return torque;
 
     static double error_previous = 0.0;
+    static double integral = 0.0;
     double fVehWeight = 1800;
     double fRollForce = 50;
     const double fRatio = 2.5;
@@ -131,7 +132,7 @@ double  PIDTorque(double torque)
     double error = accAim - gfVehAcc;
     double dt = 0.01; //10ms
     double diff = (error - error_previous)/dt;
-    double integral = integral + error * dt * ki;
+    integral = integral + error * dt * ki;
     double output = error * kp + integral + diff * kd;
 
     double fAccAjust = accAim + output;
@@ -140,13 +141,15 @@ double  PIDTorque(double torque)
 
     double ftorqueAjust = acctotorque(fAccAjust);
 
+    error_previous = error;
+
     if(gbPrintPIDOut)
     {
         int64_t timesecond = nnow/1e9;
         int64_t timepart = nnow - timesecond * 1e9;
         double fnow = timepart; fnow = fnow/1e9;
         fnow = fnow + timesecond;
-        std::cout<<fnow<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
+        std::cout<<fnow<<"\t"<<gfVehSpd<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
     }
 
     return ftorqueAjust;
@@ -166,7 +169,6 @@ double fpidtestmaxvel = 15.0;
 
 void executeDecition(const iv::brain::decition &decition)
 {
-
     double fwheel = decition.wheelangle()*0.9;
     if(fwheel<-430)fwheel = 430;
     if(fwheel>380)fwheel = 380;
@@ -189,9 +191,12 @@ void executeDecition(const iv::brain::decition &decition)
     if(pidtestnstep == 0)
     {
         ftorque = PIDTorque(acctotorque(fpidtestacc));
+        fbrake = 0;
     }
 #endif
 
+
+
     SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",fwheel);
     SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
     SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
@@ -327,13 +332,29 @@ void Activate()
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",gfWheelReq);
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",1.0);
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",2.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
+        SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
+        SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);
+
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);
 
+        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
+        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
         SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
 
-        SetMsgSignal("ADS_COM_3","FCM_2_SysStatus",0.0);
+        SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",0.0);
 
         SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
-        SetMsgSignal("ADS_COM_2","ADS_3_ACCSts",3.0);
+        SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",3.0);
 
 
 
@@ -360,13 +381,31 @@ void UnAcitvate()
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",0.0);
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",0.0);
         SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",0.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
+        SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
+        SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
+        SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);
+
+
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
+        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);
+
+        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
+        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
 
         SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
 
-        SetMsgSignal("ADS_COM_3","FCM_2_SysStatus",1.0);
+        SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",1.0);
 
         SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
-        SetMsgSignal("ADS_COM_2","ADS_3_ACCSts",0.0);
+        SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",0.0);
 
 
         ExecSend();
@@ -458,6 +497,7 @@ void PrepareMsg()
     static int rollcouter = 0;
     //    std::cout<<" roll count:: "<<rollcouter<<std::endl;
     SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr1",rollcouter);
+    SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr2",rollcouter);
     // set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
     // set_EPS1_signal("CutOutMAC_2CB_S",1.0);
     gpsterraes->GetEPS1Data(ADS_EPS_1);

src/controller/controller_chery_sterra_es_fcm/sterraes.cpp

@@ -315,7 +315,7 @@ void sterraes::GetONEBOX3Data(unsigned char * pdata){
 void sterraes::GetADSCOM3Data(unsigned char * pdata){
 //    std::vector<uint8_t> xpack =  mpPacker->pack(0x31A,mvectorADSCOM3);
 
-    std::vector<uint8_t> xpack = mpdbcsigpacker->GetPack("ADS_COM_2");
+    std::vector<uint8_t> xpack = mpdbcsigpacker->GetPack("ADS_COM_3");
 
     if(xpack.size()<24){
         std::cout<<"GetADSCOM3Data Fail. pack size: "<<xpack.size()<<std::endl;
@@ -331,7 +331,7 @@ void sterraes::GetADSCOM3Data(unsigned char * pdata){
 
 void sterraes::GetADSCOM2Data(unsigned char * pdata){
 //    std::vector<uint8_t> xpack =  mpPacker->pack(0x314,mvectorADSCOM2);
-    std::vector<uint8_t> xpack = mpdbcsigpacker->GetPack("ADS_COM_3");
+    std::vector<uint8_t> xpack = mpdbcsigpacker->GetPack("ADS_COM_2");
 
     if(xpack.size()<8){
         std::cout<<"GetADSCOM2Data Fail. pack size: "<<xpack.size()<<std::endl;