Merge branch 'master' of http://114.116.249.220:3000/ADPilot/modularization

src/controller/controller_changan_a07/main.cpp

@@ -28,6 +28,9 @@
 #include "changan_a07.h"
 #include "dbcsigpacker.h"
 
+bool gbUseAPA;
+int gnAPAstate = 0; //0 not request   1 request   2 active
+
 std::string gstrdbcpath;
 bool gbUseOutDBC;
 
@@ -72,6 +75,7 @@ unsigned char ECU_1B2[32];
 unsigned char ECU_23C[64];
 unsigned char ECU_30A[64];
 unsigned char ECU_2CA[64];
+unsigned char ECU_23F[32];
 
 static int gnState = 0; //0 not act  1 act
 
@@ -120,6 +124,65 @@ void executeDecition(const iv::brain::decition &decition)
     SetMsgSignal("ECU_23C","ACC_AccTrqReq",ftorque);
     SetMsgSignal("ECU_23C","ACC_AccTrqReqActive",decition.acc_active());
 
+    if(gbUseAPA)
+    {
+        static int64_t ntimeChangetoreq = std::chrono::system_clock::now().time_since_epoch().count()/1e6;
+        int64_t nnow;
+        if(decition.angle_active() == 0)
+        {
+            switch (gnAPAstate) {
+            case 0:
+                gnAPAstate = 1;
+                ntimeChangetoreq = std::chrono::system_clock::now().time_since_epoch().count()/1e6;
+                break;
+            case 1:
+                nnow = std::chrono::system_clock::now().time_since_epoch().count()/1e6;
+                if(abs(nnow - ntimeChangetoreq)<300)
+                    gnAPAstate = 1;
+                else
+                    gnAPAstate = 2;
+                break;
+            case 2:
+                gnAPAstate = 2;
+                break;
+            default:
+                gnAPAstate = 0;
+                break;
+            }
+        }
+        else
+        {
+            switch (gnAPAstate) {
+            case 0:
+                gnAPAstate = 0;
+                break;
+            case 1:
+                gnAPAstate = 0;
+                break;
+            case 2:
+                gnAPAstate = 0;
+                break;
+            default:
+                gnAPAstate = 0;
+                break;
+            }
+        }
+
+        SetMsgSignal("ECU_23F","APA_SteeringAngleReqProtection",gnAPAstate);
+        SetMsgSignal("ECU_23F","APA_ErrorStatus",0);
+        SetMsgSignal("ECU_23F","APA_SteeringAngleReq",fWheelAngleReq);
+
+
+    }
+    else
+    {
+        gnAPAstate = 0;
+        SetMsgSignal("ECU_23F","APA_SteeringAngleReqProtection",0);
+        SetMsgSignal("ECU_23F","APA_ErrorStatus",0);
+        SetMsgSignal("ECU_23F","APA_SteeringAngleReq",fWheelAngleReq);
+
+    }
+
     if(decition.brake()<(-5.0))
     {
         SetMsgSignal("ECU_23C","ACC_ACCTargetAcceleration",-5.0);
@@ -504,6 +567,7 @@ void PrepareMsg()
     gpchangan_a07->GetData(ECU_1B2,std::string("ECU_1B2"),32);
     gpchangan_a07->GetData(ECU_23C,std::string("ECU_23C"),64);
     gpchangan_a07->GetData(ECU_30A,std::string("ECU_30A"),64);
+    gpchangan_a07->GetData(ECU_23F,std::string("ECU_23F"),32);
 }
 
 void ExecSend()
@@ -523,6 +587,14 @@ void ExecSend()
         xraw.set_len(64);
         iv::can::canraw * pxraw23C = xmsg.add_rawmsg();
         pxraw23C->CopyFrom(xraw);
+
+        xraw.set_id(0x23F);
+        xraw.set_data(ECU_23F,32);
+        xraw.set_bext(false);
+        xraw.set_bremote(false);
+        xraw.set_len(32);
+        iv::can::canraw * pxraw23F = xmsg.add_rawmsg();
+        pxraw23F->CopyFrom(xraw);
     }
     if(ncount >=10)
     {