change controller_sterra_es.

src/controller/controller_chery_sterra_es/main.cpp

@@ -16,6 +16,7 @@
 #include "torquebrake.h"
 
 #include <thread>
+#include <math.h>
 
 #ifdef Q_OS_LINUX
 #include <signal.h>
@@ -105,6 +106,7 @@ double ADS_1_PilotParkBrkDecTarEnable = 0.0;
 double ADS_1_PilotParkDec2StpReq = 0.0;
 double ADS_1_PilotParkDriOffReq = 0.0;
 double ADS_1_ParkCtrlMod = 0.0;
+double ADS_1_ADCCAvl = 0.0;
 
 
 
@@ -126,9 +128,14 @@ void set_VCU1_signal(std::string strsigname,double value){
 
 void ExecSend();
 
+//int testnum = 0;
 void executeDecition(const iv::brain::decition &decition)
 {
 
+//    std::cout<<"brake: "<<decition.brake()<<std::endl;
+
+//    if((testnum < 2000) || (testnum > 2500))
+//    {
     if(decition.brake()<(-0.0001))
     {
         ADS_1_DrvTarTq = 0.0;
@@ -136,10 +143,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 = decition.brake();
+        ADS_1_PilotParkBrkDecTar =  -1.0;//decition.brake();
         ADS_1_PilotParkBrkDecTarVld = 1.0;
-        ADS_1_PilotParkCtrlType = 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;
     }
 
     else
@@ -153,12 +164,48 @@ void executeDecition(const iv::brain::decition &decition)
         if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
 
         ADS_1_PilotParkDec2StpReq = 0.0;
-        ADS_1_PilotParkBrkDecTarEnable = 0.0;
-        ADS_1_PilotParkBrkDecTarVld = 0.0;
-        ADS_1_PilotParkBrkDecTar = 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;
+
 
-        ADS_1_SteerAgReq = decition.wheelangle();
     }
+//    }
+
+//    else
+//    {
+
+//    if(fabs(gfVehSpd) < 0.1)
+//    {
+//        ADS_1_PilotParkDriOffReq = 1.0;
+//    }
+
+//    ADS_1_DrvTarTq = 150;
+//    if(ADS_1_DrvTarTq > 300)ADS_1_DrvTarTq =300.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";
+
+//    }
+
+//    testnum++;
+
+
+
+
+//    std::cout<<" send dec. "<<std::endl;
+
+//    ADS_1_SteerAgReq = 90.0;//decition.wheelangle();
 
 }
 
@@ -169,7 +216,7 @@ void Activate()
     std::cout<<" activate "<<std::endl;
     for(int i = 0; i < 3; i++){
         ADS_1_DrvCtrlReq = 1.0;
-        ADS_1_CtrlReqMod = 2.0;  //1 Pilot  2 Park
+        ADS_1_CtrlReqMod = 1.0;  //1 Pilot  2 Park
         ADS_1_DrvTarTqVld = 1.0;
         ADS_1_DrvTarTqEnable = 1.0;
         ADS_1_DrvTarTq = 0.0;
@@ -179,11 +226,15 @@ void Activate()
 
         ADS_1_SteerAgReq = gfWheelReq;
         ADS_1_SteerAgVld = 1.0;
-        ADS_1_SteerPilotAgEna = 1.0;
+        ADS_1_SteerPilotAgEna = 2.0;
 
         ADS_1_PilotParkDec2StpReq = 1.0;
         ADS_1_ParkCtrlMod = 1.0;
 
+        ADS_1_ADCCAvl = 1.0;
+
+  //      std::cout<<"activate."<<std::endl;
+
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
@@ -208,6 +259,7 @@ void UnAcitvate()
             ADS_1_SteerAgVld = 1.0;
             ADS_1_SteerPilotAgEna = 1.0;
 
+
             ExecSend();
             std::this_thread::sleep_for(std::chrono::milliseconds(10));
         }
@@ -229,6 +281,8 @@ void UnAcitvate()
         ADS_1_PilotParkDec2StpReq = 0.0;
         ADS_1_ParkCtrlMod = 0.0;
 
+        ADS_1_ADCCAvl = 0.0;
+
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
@@ -344,6 +398,7 @@ void PrepareMsg()
     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);
@@ -356,7 +411,9 @@ void PrepareMsg()
     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_ParkCtrlMod",ADS_1_ParkCtrlMod);    
+    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
+    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
     gpsterraes->GetONEBOX1Data(ADS_ONEBOX_1);
 
 
@@ -366,6 +423,7 @@ void PrepareMsg()
 void ExecSend()
 {
     PrepareMsg();
+//    gpsterraes->GetEPS1Data(ADS_EPS_1);
     static int nCount = 0;
     nCount++;
     iv::can::canmsg xmsg;
@@ -632,11 +690,18 @@ void LoadXML(std::string strxmlpath){
 
 
 #include <QFile>
+#include <QTextStream>
+#include <strings.h>
+#include <streambuf>
+#include <sstream>
+
+#include <iostream>
+#include <istream>
 #include <fstream>
 int main(int argc, char *argv[])
 {
 
-     std::istringstream iss("First line.\nSecond line.\n");
+ //    std::istringstream iss("First line.\nSecond line.\n");
 
 
 
@@ -723,8 +788,8 @@ int main(int argc, char *argv[])
     EnableTorqueBrakeTest();
 #endif
 
-    // testes();
-    // return 0;
+//    testes();
+//    return 0;
 
     std::thread xthread(sendthread);