change controller_chery_sterra_es_fcm. for solve brake at stop.

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -141,6 +141,10 @@ double ADS_2_ClosingSpeed = 255;
 
 double ADS_3_ACCSts = 0;
 
+static bool gbSendBrake = false;
+
+static double gfVehAcc = 0.0;
+
 
 
 void set_EPS1_signal(std::string strsigname,double value){
@@ -213,6 +217,22 @@ void executeDecition(const iv::brain::decition &decition)
         FCM_2_AebTarDecVld = 1.0;
 
         FCM_3_PilotParkCtrlRepSta = 1.0;
+
+        if(gbSendBrake == false)
+        {
+            FCM_3_PilotDec2StpReq = 0.0;
+            FCM_3_PilotBrkDecTarReq = 0.0;
+            FCM_3_PilotBrkDecTarVld = 1.0;
+            FCM_3_PilotkBrkDecTar = 0.0;
+            FCM_2_AebTarDec = 1.0;
+            FCM_3_PilotParkCtrlRepSta = 0.0;
+            FCM_2_AebTarDecVld = 1.0;
+
+            if(fabs(gfVehSpd)>0.1)
+            {
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Warning: can't brake."<<std::endl;
+            }
+        }
  //       ADS_1_PilotParkCtrlRepMod = 1.0;
  //       std::cout<<" send brake "<<std::endl;
     }
@@ -445,6 +465,9 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
     //    xdecition.set_brake_type(1);
     xdecition.set_auto_mode(3);
 
+
+//    std::cout<<"decition acc: "<<xdecition.accelerator()<<std::endl;
+
     //   xdecition.set_wheelangle(45.0);
 
     if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %lld diff is %lld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
@@ -851,6 +874,68 @@ void LoadXML(std::string strxmlpath){
     gstrdbcpath = xp.GetParam("dbcpath","./ADCC_CH.dbc");
 }
 
+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() == 0x14f)
+            {
+                unsigned char byte[24];
+                if(praw->len() == 24)
+                {
+                    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
+                    {
+                        gbSendBrake = false;//std::cout<<" warning: no brake unavailable."<<std::endl;
+                    }
+
+                }
+            }
+
+            if(praw->id() == 0x21)
+            {
+                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;
+                    gfVehAcc = facc*9.8;
+  //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
+                }
+            }
+        }
+    });
+
+    (void)pblambda;
+
+
+}
 
 
 #include <QFile>
@@ -916,6 +1001,7 @@ int main(int argc, char *argv[])
         gpsterraes = new sterraes(gstrdbcpath);
     }
 
+
     //    gdecition_def.set_accelerator(-0.5);
     gdecition_def.set_brake(0);
     gdecition_def.set_rightlamp(false);
@@ -955,6 +1041,8 @@ int main(int argc, char *argv[])
 //    testes();
 //    return 0;
 
+    Listencanrecv0();
+
     std::thread xthread(sendthread);
 
     gpsendthread = &xthread;