change controller_yunle_jd03. add velocity soc feedback, and add speed calc for continue change. complete. not test.

src/controller/controller_yunle_jd03/main.cpp

@@ -54,7 +54,7 @@ const int gnDecitionNumMax = 100;
 static int gnIndex = 0;
 
 static bool gbHaveVehSpd = false;
-static double gfVehSpd = 0.0;
+static double gfVehSpd = 0.0;  //km/h
 
 static bool gbHaveWheelAngle = false;
 static double gfWheelAngle = 0.0;
@@ -90,18 +90,43 @@ void ExecSend();
 
 void executeDecition(const iv::brain::decition &decition)
 {
+
     double fwheel = decition.wheelangle();
+
+    static int64_t nspeedcaltime = std::chrono::system_clock::now().time_since_epoch().count();
+    static double fneedvel = 0;
     if(fwheel<-430)fwheel = 430;
     if(fwheel>380)fwheel = 380;
 
     double ftorque = decition.torque() ;
-    if((ftorque>30)&&(ftorque<150))ftorque = 150;
+
+    double fVehWeight = 1800;
+    double fRollForce = 50;
+    const double fRatio = 2.5;
+    double accAim = (ftorque * fRatio - fRollForce)/fVehWeight;
+
+
     double fbrake = decition.brake();
 
+    if(fbrake<0)accAim = fbrake;
 
     double fwheelratio = 430.0/120.0;
+
     double fvelreq = decition.speed();
 
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+
+    int64_t ntimediff = nnow - nspeedcaltime;
+    double ftimediff = ntimediff; ftimediff = ftimediff/1e9;
+    fneedvel = fneedvel + accAim * ftimediff * 3.6;
+    if(fneedvel<0)fneedvel = 0.0;
+    if(fneedvel>fvelreq)fneedvel = fvelreq;
+
+    fvelreq = fneedvel;
+    nspeedcaltime = nnow;
+
+
+
     SetMsgSignal("SCU_1","SCU_Drive_Mode_Req",0.0);
     SetMsgSignal("SCU_1","SCU_Steering_Wheel_Angle_JD01_F",fwheel/fwheelratio);
     SetMsgSignal("SCU_1","SCU_ShiftLevel_Req",1.0);
@@ -249,7 +274,7 @@ void ExecSend()
 
     gnIndex++;
     xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
-    int ndatasize = xmsg.ByteSize();
+    int ndatasize = static_cast<int>(xmsg.ByteSize());
     char * strser = new char[ndatasize];
     std::shared_ptr<char> pstrser;
     pstrser.reset(strser);
@@ -368,45 +393,66 @@ void Listencanrecv0()
         }
 
         int i;
+        int nprinti = 0;
+        int nprintint = 100;
+
+        static double fvel;  //m/s
+        static double fangle;
+        static int nmode;
+
+        static double fsoc;  //m/s
+
         for(i=0;i<xmsg.rawmsg_size();i++)
         {
             iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
-            if(praw->id() == 0x14f)
+            if(praw->id() == 0x51)
             {
-                unsigned char byte[24];
-                if(praw->len() == 24)
+
+                if(praw->len() == 8)
                 {
-                    memcpy(byte,praw->data().data(),24);
-                    unsigned int value;
-                    value = byte[2]&0x01;
-                    if(value == 0)
-                    {
-                         gbSendBrake = true;//std::cout<<" brake available."<<std::endl;
-                    }
-                    else
+                    uint64_t xr;
+                    memcpy(&xr,praw->data().data(),8);
+                    uint64_t x;
+                    x = (xr >> 8) & 0xfff;
+                    uint16_t uangle = x;
+                    fangle = (double)(uangle);
+                    fangle *= 0.1;
+
+                    uint16_t uspeed = (xr >> 20) & 0x1ff;
+                    fvel = (double)uspeed;
+                    fvel = fvel * 0.1;
+
+                    gfVehSpd = fvel * 3.6;
+
+                    nmode = (xr >> 29) & 0x7;
+
+                    nprinti++;
+                    if(nprinti>=nprintint)
                     {
-                        gbSendBrake = false;//std::cout<<" warning: no brake unavailable."<<std::endl;
+                        std::cout<<"SOC: "<<fsoc<<" velocity:"<<fvel<<" angle: "<<fangle<<" mode"<<nmode<<std::endl;
+                        nprinti = 0;
                     }
 
                 }
+
             }
 
-            if(praw->id() == 0x21)
+            if(praw->id() == 0x17904001)
             {
-                unsigned char xdata[8];
+
                 if(praw->len() == 8)
                 {
-                    memcpy(xdata,praw->data().data(),8);
-                    unsigned int value;
-                    value = xdata[2]&0xff;
-                    value = value<<8;
-                    value =value + (xdata[3]&0xff);
-                    double facc = value;
-                    facc = facc * 0.001 - 2.0;
-
-  //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
+                    uint64_t xr;
+                    memcpy(&xr,praw->data().data(),8);
+
+                    uint16_t usoc = (xr >> 48) & 0xffff;
+                    fsoc = (double)usoc;
+                    fsoc = fsoc * 0.1;
+
                 }
+
             }
+
         }
     });