change controller_changan_shenlan_v2. test can acc, not ok.

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -67,6 +67,7 @@ static double gfVehAcc = 0.0;
 static int64_t gnLastAccUptime = 0;
 static bool gbUsePID = true;
 static bool gbPrintPIDOut = true;
+static bool gbAccValid = false;
 
 
 // signal: @ACC_LatAngReq    //更改CANid
@@ -239,7 +240,7 @@ void executeDecition(const iv::brain::decition &decition)
 
     /*制动过程用的减速度，加速用扭矩*/
     double ftorque = decition.torque();
-    ftorque = PIDTorque(ftorque);
+    if(ftorque>0.1)ftorque = PIDTorque(ftorque);
     _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(decition.torque());
     _m24E.ACC_AccTrqReqActive = decition.acc_active();
     if(decition.brake()<(-5.0))
@@ -720,26 +721,64 @@ void Listencanrecv0()
             std::cout<<"controller Listencan0 fail."<<std::endl;
             return;
         }
+        static double  fLastVehSpeed = 0;
+        static bool baccnotokcount = 0;
+
 
         int i;
         for(i=0;i<xmsg.rawmsg_size();i++)
         {
-            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
 
+            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
             if(praw->id() == 0x1CC)
             {
-                unsigned char xdata[64];
                 if(praw->len() == 64)
                 {
+                    unsigned char xdata[64];
                     memcpy(xdata,praw->data().data(),64);
-                    unsigned int value;
-                    value = xdata[10]&0x3F;value = value<<7;
-                    value =value + (xdata[11]&0xFE);
+                    unsigned int value = xdata[10]; value = value<<7;
+                    value = value +((xdata[11]&0xFE)>>1);
                     double facc = value;
                     facc = facc * 0.01 - 32.0;
-                    gfVehAcc = facc;
+   //                 qDebug(" %02X %02X",xdata[10],xdata[11]);
+    //                gfVehAcc = facc;   //ESP Acc not ok.
+                    value = xdata[12]&0x1f;value = value<<8;
+                    value = value + xdata[13];
+                    double fvehspeed = value;fvehspeed = fvehspeed * 0.05625;
+                    int64_t timediff = (std::chrono::system_clock::now().time_since_epoch().count() - gnLastAccUptime)/1e6;
+                    if(timediff<16)
+                    {
+                        double facccalc = ((fvehspeed - fLastVehSpeed)/3.6)/0.01;
+                        gfVehAcc = facccalc;
+                        gbAccValid = true;
+                        baccnotokcount = 0;
+                    }
+                    else
+                    {
+                        baccnotokcount++;
+                        if(baccnotokcount>3)gbAccValid = false;
+                    }
+
+                    fLastVehSpeed = fvehspeed;
+  //                  std::cout<<" diff : "<<(std::chrono::system_clock::now().time_since_epoch().count() - gnLastAccUptime)/1e6<<std::endl;
                     gnLastAccUptime =  std::chrono::system_clock::now().time_since_epoch().count();
-                    //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
+                                      std::cout<<" acc : "<<gfVehAcc<<std::endl;
+
+                }
+            }
+            if(praw->id() == 0x3)
+            if(praw->id() == 0x322)
+            {
+                if(praw->len() == 8)
+                {
+                    unsigned char xdata[8];
+                    memcpy(xdata,praw->data().data(),8);
+                    unsigned int value = xdata[1]; value = value<<12;
+                    value = value +((xdata[2]&0xFF)<<4);
+                    value = value + xdata[3];
+                    double fmotorrpm = value - 20000;
+                    (void)fmotorrpm;
+   //                 std::cout<<" motor speed : "<<fmotorrpm<<std::endl;
                 }
             }
         }
@@ -797,8 +836,8 @@ int main(int argc, char *argv[])
     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);
+    gbUsePID =  xp.GetParam("UsePID",false);
+    gbPrintPIDOut = xp.GetParam("PrintPIDOut",false);
 
     gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
@@ -810,6 +849,8 @@ int main(int argc, char *argv[])
 
     std::thread xthread(sendthread);
 
+    Listencanrecv0();
+
     gpsendthread = &xthread;
 
 #ifdef Q_OS_LINUX

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -577,6 +577,7 @@ void PrepareMsg()
 
 void ExecSend()
 {
+    static int nrd = 0;
     PrepareMsg();
 //    gpsterraes->GetEPS1Data(ADS_EPS_1);
     static int nCount = 0;
@@ -623,6 +624,8 @@ void ExecSend()
     iv::can::canraw * pxraw145 = xmsg.add_rawmsg();
     pxraw145->CopyFrom(xraw);
 
+    if(nrd == 0)
+    {
     xraw.set_id(0x31A);
     xraw.set_data(ADS_COM3,24);
     xraw.set_bext(false);
@@ -638,6 +641,9 @@ void ExecSend()
     xraw.set_len(8);
     iv::can::canraw * pxraw314 = xmsg.add_rawmsg();
     pxraw314->CopyFrom(xraw);
+    }
+
+
 
     xraw.set_id(0x14A);
     xraw.set_data(ADS_ONEBOX_3,24);
@@ -673,6 +679,9 @@ void ExecSend()
     {
         std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
     }
+
+    nrd++;
+    if(nrd == 5)nrd = 0;
 }
 
 void initial()