change controller_chery_sterra_es_fcm. test ok.

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -70,6 +70,8 @@ unsigned char ADS_ONEBOX_1[24];// 0x159/345
 unsigned char ADS_VCU_1[24];   // 0x167/359
 unsigned char ADS_ONEBOX_2[24];
 unsigned char ADS_ONEBOX_3[24];
+unsigned char ADS_COM2[8];
+unsigned char ADS_COM3[24];
 
 static int gnState = 0; //0 not act  1 act
 
@@ -91,6 +93,7 @@ double ADS_1_TarGearReq = 0.0;
 double ADS_1_TarGearReqVld = 0.0;
 double ADS_1_GearCtrlEnable = 1.0;
 
+
 double ADS_1_SteerAgReq = 0.0;
 double ADS_1_SteerAgVld = 0.0;
 double ADS_1_SteerPilotAgEna = 0.0;
@@ -113,23 +116,30 @@ double ADS_1_StopDist = 0.0;
 
 double FCM_2_AebReqTyp = 0;
 double FCM_2_AebTarDec = 0;
-double FCM_2_AebTarDecVld = 0;
+double FCM_2_AebTarDecVld = 1.0;
 double FCM_2_AwbReq = 0;
 double FCM_2_AwbLvl = 0;
 double FCM_2_BrkPreFillReq = 0;
 double FCM_2_AbaReq = 0;
 double FCM_2_AbaLvl = 0;
-double FCM_2_Avl = 0;
+double FCM_2_Avl = 1;
 
 double FCM_3_PilotParkCtrlRepSta = 0;
 double FCM_3_PilotCtrlType = 0;
 double FCM_3_PilotkBrkDecTar = 0;
-double FCM_3_PilotBrkDecTarVld = 0;
+double FCM_3_PilotBrkDecTarVld = 1;
 double FCM_3_PilotBrkDecTarReq = 0;
 double FCM_3_PilotDec2StpReq = 0;
 double FCM_3_PilotDriOffReq = 0;
-double CutOutFreshvalues_2CB_S = 0;
-double CutOutMAC_2CB_S = 0;
+double CutOutFreshvalues_2CB_S = 0.0;
+double CutOutMAC_2CB_S = 0.0;
+
+double FCM_2_SysStatus = 1;
+double ADS_2_TTC = 5.11;
+double ADS_2_AEBStatus = 0;
+double ADS_2_ClosingSpeed = 255;
+
+double ADS_3_ACCSts = 0;
 
 
 
@@ -157,6 +167,14 @@ void set_ONEBOX3_signal(std::string strsigname,double value){
     gpsterraes->set_ONEBOX3_signal(strsigname,value);
 }
 
+void set_ADSCOM3_Signal(std::string strsigname,double value){
+    gpsterraes->set_ADSCOM3_signal(strsigname,value);
+}
+
+void set_ADSCOM2_Signal(std::string strsigname,double value){
+    gpsterraes->set_ADSCOM2_signal(strsigname,value);
+}
+
 void ExecSend();
 
 int testnum = 0;
@@ -185,12 +203,15 @@ void executeDecition(const iv::brain::decition &decition)
         ADS_1_DrvTarTq = 0.0;
 
         FCM_3_PilotDriOffReq = 0.0;
-        FCM_3_PilotDec2StpReq = 0.0;
-        FCM_3_PilotkBrkDecTar =  decition.brake();
+        FCM_3_PilotDec2StpReq = 1.0;
+        FCM_3_PilotkBrkDecTar = decition.brake();// 0.0;//decition.brake();
+        FCM_2_AebTarDec = 0;
         FCM_3_PilotBrkDecTarVld = 1.0;
         FCM_3_PilotCtrlType = 0.0;
         FCM_3_PilotBrkDecTarReq = 1.0;
 
+        FCM_2_AebTarDecVld = 1.0;
+
         FCM_3_PilotParkCtrlRepSta = 1.0;
  //       ADS_1_PilotParkCtrlRepMod = 1.0;
  //       std::cout<<" send brake "<<std::endl;
@@ -205,74 +226,49 @@ void executeDecition(const iv::brain::decition &decition)
             FCM_3_PilotBrkDecTarReq = 1.0;
             FCM_3_PilotBrkDecTarVld = 1.0;
             FCM_3_PilotkBrkDecTar = 1.0;
-
-            ADS_1_StopDist = 1.0;
-
+            FCM_2_AebTarDec = 1.0;
             FCM_3_PilotParkCtrlRepSta = 1.0;
  //           ADS_1_PilotParkCtrlRepMod = 1.0;
         }
         else
         {
-  //          ADS_1_StopDist = 0.0;
             FCM_3_PilotDec2StpReq = 0.0;
-            FCM_3_PilotBrkDecTarReq = 1.0;
+            FCM_3_PilotBrkDecTarReq = 0.0;
             FCM_3_PilotBrkDecTarVld = 1.0;
-            FCM_3_PilotkBrkDecTar = 1.0;
-
-
-            FCM_3_PilotParkCtrlRepSta = 1.0;
- //           ADS_1_PilotParkCtrlRepMod = 1.0;
+            FCM_3_PilotkBrkDecTar = 0.0;
+            FCM_2_AebTarDec = 1.0;
+            FCM_3_PilotParkCtrlRepSta = 0.0;
         }
-
+        FCM_2_AebTarDecVld = 1.0;
         ADS_1_DrvTarTq = decition.torque();
 
-//        if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
-
-
-
     }
 //    }
 
 //    else
 //    {
 
-//    if(fabs(gfVehSpd) < 0.1)
-//    {
-//        ADS_1_PilotParkDriOffReq = 1.0;
-//        ADS_1_PilotParkDec2StpReq = 0.0;
-//        ADS_1_PilotParkBrkDecTarEnable = 1.0;
-//        ADS_1_PilotParkBrkDecTarVld = 1.0;
-//        ADS_1_PilotParkBrkDecTar = 1.0;
-
-//        ADS_1_StopDist = 1.0;
-
-//        ADS_1_PilotCtrlRepSta = 1.0;
-//        ADS_1_PilotParkCtrlRepMod = 1.0;
-//    }
-
-//    else
-//    {
-//        ADS_1_PilotParkDec2StpReq = 0.0;
-//        ADS_1_PilotParkBrkDecTarEnable = 1.0;
-//        ADS_1_PilotParkBrkDecTarVld = 1.0;
-//        ADS_1_PilotParkBrkDecTar = 1.0;
-
-//        ADS_1_StopDist = 0.0;
-
-//        ADS_1_PilotCtrlRepSta = 1.0;
-//        ADS_1_PilotParkCtrlRepMod = 1.0;
-//    }
-
-//    ADS_1_DrvTarTq = 150;
-//    if(ADS_1_DrvTarTq >600)ADS_1_DrvTarTq =600.0;
-
-////    ADS_1_PilotParkDec2StpReq = 0.0;
-////    ADS_1_PilotParkBrkDecTarEnable = 1.0;
-////    ADS_1_PilotParkBrkDecTarVld = 1.0;
-////    ADS_1_PilotParkBrkDecTar = 1.0;
-
-
-//    std::cout<<" send drive"<<std::endl;
+//        if(fabs(gfVehSpd) < 0.1)
+//        {
+//            FCM_3_PilotDriOffReq = 1.0;
+//            FCM_3_PilotDec2StpReq = 0.0;
+//            FCM_3_PilotBrkDecTarReq = 1.0;
+//            FCM_3_PilotBrkDecTarVld = 1.0;
+//            FCM_3_PilotkBrkDecTar = 1.0;
+//            FCM_2_AebTarDec = 1.0;
+//            FCM_3_PilotParkCtrlRepSta = 1.0;
+//        }
+//        else
+//        {
+//            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;
+//        }
+//        ADS_1_DrvTarTq = 300;
+//        FCM_2_AebTarDecVld = 1.0;
 
 //    }
 
@@ -317,6 +313,13 @@ void Activate()
 
         FCM_2_Avl = 1.0;
 
+        ADS_3_ACCSts = 3.0;
+
+        FCM_2_SysStatus = 0;
+        ADS_2_AEBStatus = 1;
+
+
+
   //      std::cout<<"activate."<<std::endl;
 
         ExecSend();
@@ -367,7 +370,12 @@ void UnAcitvate()
 
         ADS_1_ADCCAvl = 0.0;
 
-        FCM_2_Avl = 0.0;
+        ADS_3_ACCSts = 0;
+
+        FCM_2_Avl = 1.0;
+
+        FCM_2_SysStatus = 1;
+        ADS_2_AEBStatus = 0;
 
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -481,30 +489,30 @@ void PrepareMsg()
     set_VCU1_signal("ADS_1_DrvTarTqEnable",ADS_1_DrvTarTqEnable);
     set_VCU1_signal("ADS_1_RollgCntr2",rollcouter);
     set_VCU1_signal("ADS_1_Resd2",0);
-    set_VCU1_signal("ADS_1_TarGearReq",ADS_1_TarGearReq);
-    set_VCU1_signal("ADS_1_TarGearReqVld",ADS_1_TarGearReqVld);
-    set_VCU1_signal("ADS_1_GearCtrlEnable",ADS_1_GearCtrlEnable);
+//    set_VCU1_signal("ADS_1_TarGearReq",ADS_1_TarGearReq);
+//    set_VCU1_signal("ADS_1_TarGearReqVld",ADS_1_TarGearReqVld);
+//    set_VCU1_signal("ADS_1_GearCtrlEnable",ADS_1_GearCtrlEnable);
     set_VCU1_signal("ADS_1_ADCCAvl",ADS_1_ADCCAvl);
     gpsterraes->GetVCU1Data(ADS_VCU_1);
 
-    set_ONEBOX1_signal("ADS_1_RollgCntr1",rollcouter);
-    set_ONEBOX1_signal("ADS_1_Resd1",0);
-    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
-    set_ONEBOX1_signal("ADS_1_PilotParkCtrlType",ADS_1_PilotParkCtrlType);
-    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTar",ADS_1_PilotParkBrkDecTar);
-    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
-    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarVld",ADS_1_PilotParkBrkDecTarVld);
-    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarEnable",ADS_1_PilotParkBrkDecTarEnable);
-    set_ONEBOX1_signal("ADS_1_PilotParkDec2StpReq",ADS_1_PilotParkDec2StpReq);
-    set_ONEBOX1_signal("ADS_1_PilotParkDriOffReq",ADS_1_PilotParkDriOffReq);
-    set_ONEBOX1_signal("ADS_1_ParkCtrlMod",ADS_1_ParkCtrlMod);    
-    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
-    set_ONEBOX1_signal("ADS_1_StopDist",ADS_1_StopDist);
-    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
-    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_1);
+//    set_ONEBOX1_signal("ADS_1_RollgCntr1",rollcouter);
+//    set_ONEBOX1_signal("ADS_1_Resd1",0);
+//    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
+//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlType",ADS_1_PilotParkCtrlType);
+//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTar",ADS_1_PilotParkBrkDecTar);
+//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
+//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarVld",ADS_1_PilotParkBrkDecTarVld);
+//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarEnable",ADS_1_PilotParkBrkDecTarEnable);
+//    set_ONEBOX1_signal("ADS_1_PilotParkDec2StpReq",ADS_1_PilotParkDec2StpReq);
+//    set_ONEBOX1_signal("ADS_1_PilotParkDriOffReq",ADS_1_PilotParkDriOffReq);
+//    set_ONEBOX1_signal("ADS_1_ParkCtrlMod",ADS_1_ParkCtrlMod);
+//    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
+//    set_ONEBOX1_signal("ADS_1_StopDist",ADS_1_StopDist);
+//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
+//    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_1);
 
     set_ONEBOX2_signal("FCM_2_RollgCntr1",rollcouter);
-    set_ONEBOX2_signal("ADS_1_Resd1",0);
+    set_ONEBOX2_signal("FCM_2_Resd1",rollcouter);
     set_ONEBOX2_signal("FCM_2_AebReqTyp",FCM_2_AebReqTyp);
     set_ONEBOX2_signal("FCM_2_AebTarDec",FCM_2_AebTarDec);
     set_ONEBOX2_signal("FCM_2_AebTarDecVld",FCM_2_AebTarDecVld);
@@ -514,10 +522,11 @@ void PrepareMsg()
     set_ONEBOX2_signal("FCM_2_AbaReq",FCM_2_AbaReq);
     set_ONEBOX2_signal("FCM_2_AbaLvl",FCM_2_AbaLvl);
     set_ONEBOX2_signal("FCM_2_Avl",FCM_2_Avl);
-    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_2);
+    gpsterraes->GetONEBOX2Data(ADS_ONEBOX_2);
 
 
     set_ONEBOX3_signal("FCM_3_RollgCntr1",rollcouter);
+    set_ONEBOX3_signal("FCM_3_Resd1",rollcouter);
     set_ONEBOX3_signal("FCM_3_PilotParkCtrlRepSta",FCM_3_PilotParkCtrlRepSta);
     set_ONEBOX3_signal("FCM_3_PilotCtrlType",FCM_3_PilotCtrlType);
     set_ONEBOX3_signal("FCM_3_PilotkBrkDecTar",FCM_3_PilotkBrkDecTar);
@@ -527,7 +536,17 @@ void PrepareMsg()
     set_ONEBOX3_signal("FCM_3_PilotDriOffReq",FCM_3_PilotDriOffReq);
     set_ONEBOX3_signal("CutOutFreshvalues_2CB_S",CutOutFreshvalues_2CB_S);
     set_ONEBOX3_signal("CutOutMAC_2CB_S",CutOutMAC_2CB_S);
-    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_3);
+    gpsterraes->GetONEBOX3Data(ADS_ONEBOX_3);
+
+    set_ADSCOM3_Signal("ADS_3_RollgCntr1",rollcouter);
+    set_ADSCOM3_Signal("ADS_3_ACCSts",ADS_3_ACCSts);
+    gpsterraes->GetADSCOM3Data(ADS_COM3);
+
+    set_ADSCOM2_Signal("ADS_2_RollgCntr1",rollcouter);
+    set_ADSCOM2_Signal("ADS_2_AEBStatus",ADS_2_AEBStatus);
+    set_ADSCOM2_Signal("ADS_2_TTC",ADS_2_TTC);
+    set_ADSCOM2_Signal("FCM_2_SysStatus",FCM_2_SysStatus);
+    gpsterraes->GetADSCOM2Data(ADS_COM2);
 
     rollcouter++;
     if(rollcouter>=14)rollcouter = 0;
@@ -581,6 +600,22 @@ void ExecSend()
     iv::can::canraw * pxraw145 = xmsg.add_rawmsg();
     pxraw145->CopyFrom(xraw);
 
+    xraw.set_id(0x31A);
+    xraw.set_data(ADS_COM3,24);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(24);
+    iv::can::canraw * pxraw31A = xmsg.add_rawmsg();
+    pxraw31A->CopyFrom(xraw);
+
+    xraw.set_id(0x314);
+    xraw.set_data(ADS_COM2,8);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    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);
     xraw.set_bext(false);

src/controller/controller_chery_sterra_es_fcm/sterraes.cpp

@@ -76,10 +76,10 @@ void sterraes::initsig()
     sv.name = "ADS_1_AMAPTqLimitVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
     sv.name = "ADS_1_RollgCntr2";sv.value = 0;mvectorADSVCU1.push_back(sv);
     sv.name = "ADS_1_Resd2";sv.value = 0;mvectorADSVCU1.push_back(sv);
-    sv.name = "ADS_1_TarGearReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
-    sv.name = "ADS_1_TarGearReqVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
-    sv.name = "ADS_1_GearCtrlEnable";sv.value = 0;mvectorADSVCU1.push_back(sv);
-    sv.name = "ADS_1_RpaPTReadyReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
+//    sv.name = "ADS_1_TarGearReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
+//    sv.name = "ADS_1_TarGearReqVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
+//    sv.name = "ADS_1_GearCtrlEnable";sv.value = 0;mvectorADSVCU1.push_back(sv);
+//    sv.name = "ADS_1_RpaPTReadyReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
 
     sv.name = "FCM_2_CRC1";sv.value = 0;mvectorADSONEBOX2.push_back(sv);
     sv.name = "FCM_2_RollgCntr1";sv.value = 0;mvectorADSONEBOX2.push_back(sv);
@@ -107,6 +107,25 @@ void sterraes::initsig()
     sv.name = "CutOutFreshvalues_2CB_S";sv.value = 0;mvectorADSONEBOX3.push_back(sv);
     sv.name = "CutOutMAC_2CB_S";sv.value = 0;mvectorADSONEBOX3.push_back(sv);
 
+
+    sv.name = "ADS_2_RollgCntr1";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_Resd1";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_FunctionSuppressReq";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_HWRequest";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_PP_M_Request";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_AEBStatus";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_ClosingSpeed";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_TTC";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_Object";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_Object_Status";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "ADS_2_FCWStatus";sv.value = 0;mvectorADSCOM2.push_back(sv);
+    sv.name = "FCM_2_SysStatus";sv.value = 0;mvectorADSCOM2.push_back(sv);
+
+
+
+    sv.name = "ADS_3_RollgCntr1";sv.value = 0;mvectorADSCOM3.push_back(sv);
+    sv.name = "ADS_3_ACCSts";sv.value = 0;mvectorADSCOM3.push_back(sv);
+
 }
 
 void sterraes::setsignal(std::vector<SignalPackValue> * pvectorspv,std::string strsigname,double value){
@@ -146,6 +165,14 @@ void sterraes::set_ONEBOX3_signal(std::string strsigname,double value){
     setsignal(&mvectorADSONEBOX3,strsigname,value);
 }
 
+void sterraes::set_ADSCOM3_signal(std::string strsigname,double value){
+    setsignal(&mvectorADSCOM3,strsigname,value);
+}
+
+void sterraes::set_ADSCOM2_signal(std::string strsigname,double value){
+    setsignal(&mvectorADSCOM2,strsigname,value);
+}
+
 unsigned char CRCCheck_SAEJ1850(unsigned char msg[], int len, unsigned char idleCrc)
 {
     int i = 0;
@@ -227,3 +254,19 @@ void sterraes::GetONEBOX3Data(unsigned char * pdata){
 
     fillcrc(0x000A,pdata);
 }
+
+void sterraes::GetADSCOM3Data(unsigned char * pdata){
+    std::vector<uint8_t> xpack =  mpPacker->pack(0x31A,mvectorADSCOM3);
+    int i;
+    for(i=0;i<24;i++)pdata[i] = xpack[i];
+
+    fillcrc(0x0011,pdata);
+}
+
+void sterraes::GetADSCOM2Data(unsigned char * pdata){
+    std::vector<uint8_t> xpack =  mpPacker->pack(0x314,mvectorADSCOM2);
+    int i;
+    for(i=0;i<8;i++)pdata[i] = xpack[i];
+
+    fillcrc(0x0010,pdata);
+}

src/controller/controller_chery_sterra_es_fcm/sterraes.h

@@ -18,6 +18,8 @@ private:
     std::vector<SignalPackValue> mvectorADSVCU1;
     std::vector<SignalPackValue> mvectorADSONEBOX2;
     std::vector<SignalPackValue> mvectorADSONEBOX3;
+    std::vector<SignalPackValue> mvectorADSCOM3;
+    std::vector<SignalPackValue> mvectorADSCOM2;
 
     CANPacker * mpPacker;
 
@@ -35,6 +37,8 @@ public:
     void set_VCU1_signal(std::string strsigname,double value);
     void set_ONEBOX2_signal(std::string strsigname,double value);
     void set_ONEBOX3_signal(std::string strsigname,double value);
+    void set_ADSCOM3_signal(std::string strsigname,double value);
+    void set_ADSCOM2_signal(std::string strsigname,double value);
 
     void GetEPS1Data(unsigned char * pdata);
     void GetEPS3Data(unsigned char * pdata);
@@ -42,6 +46,8 @@ public:
     void GetVCU1Data(unsigned char * pdata);
     void GetONEBOX2Data(unsigned char * pdata);
     void GetONEBOX3Data(unsigned char * pdata);
+    void GetADSCOM3Data(unsigned char * pdata);
+    void GetADSCOM2Data(unsigned char * pdata);
 
 
 };