change controller_changan_shenlan_v2.change torque calc use pid.

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -59,6 +59,15 @@ static QMutex gMutex;
 
 static std::thread * gpsendthread = NULL;
 
+static double kp = 0.1;
+static double ki = 0.0;
+static double kd = 0.0;
+static double MAXACC = 3.0;
+static double gfVehAcc = 0.0;
+static int64_t gnLastAccUptime = 0;
+static bool gbUsePID = true;
+static bool gbPrintPIDOut = true;
+
 
 // signal: @ACC_LatAngReq    //更改CANid
 #define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
@@ -135,6 +144,47 @@ ECU_25E_t _m25E = {0,0,0,0};
 
 void ExecSend();
 
+double  PIDTorque(double torque)
+{
+    if(gbUsePID == false)return torque;
+
+    int nnow = std::chrono::system_clock::now().time_since_epoch().count();
+
+    if((nnow - gnLastAccUptime)>1e9)return torque;
+
+    static double error_previous = 0.0;
+    double fVehWeight = 1800;
+    double fRollForce = 50;
+    const double fRatio = 2.5;
+    double accAim = (torque * fRatio - fRollForce)/fVehWeight;
+
+
+
+    double error = accAim - gfVehAcc;
+    double dt = 0.01; //10ms
+    double diff = (error - error_previous)/dt;
+    double integral = integral + error * dt * ki;
+    double output = error * kp + integral + diff * kd;
+
+    double fAccAjust = accAim + output;
+    if(fAccAjust>MAXACC)fAccAjust = MAXACC;
+    if(fAccAjust<=0)fAccAjust = 0.0;
+
+    double fNeed = fRollForce + fVehWeight*fAccAjust;
+    double ftorqueAjust = fNeed/fRatio;
+
+    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;
+    }
+
+    return ftorqueAjust;
+}
+
 void executeDecition(const iv::brain::decition &decition)
 {
 
@@ -188,6 +238,8 @@ void executeDecition(const iv::brain::decition &decition)
 
 
     /*制动过程用的减速度，加速用扭矩*/
+    double ftorque = decition.torque();
+    ftorque = PIDTorque(ftorque);
     _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(decition.torque());
     _m24E.ACC_AccTrqReqActive = decition.acc_active();
     if(decition.brake()<(-5.0))
@@ -658,6 +710,46 @@ void sig_int(int signo)
 }
 #endif
 
+void Listencanrecv0()
+{
+    void * pblambda = iv::modulecomm::RegisterRecv("canrecv0",[](const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
+        (void)strdata;(void)nSize;(void)index;(void)dt;(void)strmemname;
+        iv::can::canmsg xmsg;
+        if(false == xmsg.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"controller Listencan0 fail."<<std::endl;
+            return;
+        }
+
+        int i;
+        for(i=0;i<xmsg.rawmsg_size();i++)
+        {
+            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
+
+            if(praw->id() == 0x1CC)
+            {
+                unsigned char xdata[64];
+                if(praw->len() == 64)
+                {
+                    memcpy(xdata,praw->data().data(),64);
+                    unsigned int value;
+                    value = xdata[10]&0x3F;value = value<<7;
+                    value =value + (xdata[11]&0xFE);
+                    double facc = value;
+                    facc = facc * 0.01 - 32.0;
+                    gfVehAcc = facc;
+                    gnLastAccUptime =  std::chrono::system_clock::now().time_since_epoch().count();
+                    //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
+                }
+            }
+        }
+    });
+
+    (void)pblambda;
+
+
+}
+
 int main(int argc, char *argv[])
 {
     RegisterIVBackTrace();
@@ -701,6 +793,12 @@ int main(int argc, char *argv[])
     gstrmemcansend = xp.GetParam("cansend","cansend0");
     gstrmemdecition = xp.GetParam("dection","deciton");
     gstrmemchassis = xp.GetParam("chassismsgname","chassis");
+    kp = xp.GetParam("kp",0.1);
+    ki = xp.GetParam("ki",0.0);
+    kd = xp.GetParam("kd",0.0);
+    MAXACC = xp.GetParam("MAXACC",3.0);
+    gbUsePID =  xp.GetParam("UsePID",true);
+    gbPrintPIDOut = xp.GetParam("PrintPIDOut",true);
 
     gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);