change chassis and controller for cherry es.

src/controller/controller_chery_sterra_es/main.cpp

@@ -51,9 +51,6 @@ static int gnDecitionNum = 0; //when is zero,send default;
 const int gnDecitionNumMax = 100;
 static int gnIndex = 0;
 
-static int gnGear = 0; //当前档位
-static int64_t gnLastGearTime = 0;
-
 static bool gbHaveVehSpd = false;
 static double gfVehSpd = 0.0;
 
@@ -110,6 +107,7 @@ double ADS_1_PilotParkDec2StpReq = 0.0;
 double ADS_1_PilotParkDriOffReq = 0.0;
 double ADS_1_ParkCtrlMod = 0.0;
 double ADS_1_ADCCAvl = 0.0;
+double ADS_1_StopDist = 0.0;
 
 
 
@@ -131,13 +129,19 @@ void set_VCU1_signal(std::string strsigname,double value){
 
 void ExecSend();
 
-//int testnum = 0;
+int testnum = 0;
 void executeDecition(const iv::brain::decition &decition)
 {
 
+
+    double fwheel = decition.wheelangle();
+    if(fwheel<-430)fwheel = 430;
+    if(fwheel>380)fwheel = 380;
+    ADS_1_SteerAgReq =fwheel;
+
 //    std::cout<<"brake: "<<decition.brake()<<std::endl;
 
-//    if((testnum < 2000) || (testnum > 2500))
+//    if((testnum < 1000) || (testnum > 1500))
 //    {
     if(decition.brake()<(-0.0001))
     {
@@ -146,14 +150,14 @@ void executeDecition(const iv::brain::decition &decition)
         ADS_1_ParkCtrlMod = 1.0;
         ADS_1_PilotParkDriOffReq = 0.0;
         ADS_1_PilotParkDec2StpReq = 1.0;
-        ADS_1_PilotParkBrkDecTar =  -1.0;//decition.brake();
+        ADS_1_PilotParkBrkDecTar =  decition.brake();
         ADS_1_PilotParkBrkDecTarVld = 1.0;
         ADS_1_PilotParkCtrlType = 0.0;
         ADS_1_PilotParkBrkDecTarEnable = 1.0;
 
         ADS_1_PilotCtrlRepSta = 1.0;
         ADS_1_PilotParkCtrlRepMod = 1.0;
-        std::cout<<" send brake "<<std::endl;
+ //       std::cout<<" send brake "<<std::endl;
     }
 
     else
@@ -161,19 +165,33 @@ void executeDecition(const iv::brain::decition &decition)
         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_StopDist = 0.0;
+            ADS_1_PilotParkDec2StpReq = 0.0;
+            ADS_1_PilotParkBrkDecTarEnable = 1.0;
+            ADS_1_PilotParkBrkDecTarVld = 1.0;
+            ADS_1_PilotParkBrkDecTar = 1.0;
 
-        ADS_1_DrvTarTq = decition.torque();
-        if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
+            ADS_1_StopDist = 0.0;
 
-        ADS_1_PilotParkDec2StpReq = 0.0;
-        ADS_1_PilotParkBrkDecTarEnable = 1.0;
-        ADS_1_PilotParkBrkDecTarVld = 1.0;
-        ADS_1_PilotParkBrkDecTar = 1.0;
+            ADS_1_PilotCtrlRepSta = 1.0;
+            ADS_1_PilotParkCtrlRepMod = 1.0;
+        }
 
+        ADS_1_DrvTarTq = decition.torque();
+//        if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
 
-        ADS_1_PilotCtrlRepSta = 0.0;
-        ADS_1_PilotParkCtrlRepMod = 0.0;
 
 
     }
@@ -185,23 +203,45 @@ void executeDecition(const iv::brain::decition &decition)
 //    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 > 300)ADS_1_DrvTarTq =300.0;
+//    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;
+////    ADS_1_PilotParkDec2StpReq = 0.0;
+////    ADS_1_PilotParkBrkDecTarEnable = 1.0;
+////    ADS_1_PilotParkBrkDecTarVld = 1.0;
+////    ADS_1_PilotParkBrkDecTar = 1.0;
 
-//    ADS_1_PilotCtrlRepSta = 0.0;
-//    ADS_1_PilotParkCtrlRepMod = 0.0;
-//    std::cout<<" send drive";
+
+//    std::cout<<" send drive"<<std::endl;
 
 //    }
 
 //    testnum++;
+//    if(testnum > 1500)testnum = 0;
 
 
 
@@ -227,6 +267,9 @@ void Activate()
         ADS_1_TarGearReqVld = 1.0;
         ADS_1_GearCtrlEnable = 0.0;
 
+
+
+
         ADS_1_SteerAgReq = gfWheelReq;
         ADS_1_SteerAgVld = 1.0;
         ADS_1_SteerPilotAgEna = 2.0;
@@ -324,12 +367,6 @@ void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned
         gfWheelAngle = xchassis.angle_feedback();
         gbHaveWheelAngle = true;
     }
-
-    if(xchassis.has_shift())
-    {
-        gnGear = xchassis.shift();
-        gnLastGearTime = std::chrono::system_clock::now().time_since_epoch().count();
-    }
 }
 
 
@@ -422,6 +459,7 @@ void PrepareMsg()
     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);
 

src/detection/detection_chassis/decodechassis.cpp

@@ -565,6 +565,7 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
 int ProcSterraesChassis(void *pa, iv::can::canmsg *pmsg){
     int i;
     static iv::chassis xchassis;
+    static int nprint = 0;
     for(i=0;i<pmsg->rawmsg_size();i++)
     {
 
@@ -630,14 +631,17 @@ int ProcSterraesChassis(void *pa, iv::can::canmsg *pmsg){
                 value = value>>5;
                 ngear = value;
                 xchassis.set_shift(ngear);
-                std::cout<<" gear: "<<xchassis.shift()<<std::endl;
+                if(nprint>=100){
+                    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<< " gear: "<<xchassis.shift()<< " vel: "<<xchassis.vel()<<" angle: "<<xchassis.angle_feedback()<< std::endl;
+                    nprint = 0;
+                }
 
             }
         }
 
     }
 
-
+    nprint++;
     return 0;
 }