complete apollocontroller_shenlanfd. not test.

src/apollo/code/apollocontroller_shenlanfd/apollocontroller_shenlanfd.pro

@@ -1,4 +1,5 @@
 #/apollo/.cache/bazel/679551712d2357b63e6e0ce858ebf90e/execroot/__main__/bazel-out/aarch64-opt-exec-2B5CBBC6/bin/external/com_google_protobuf/protoc -I=./ --cpp_out=./  *.proto
+#/apollo/.cache/bazel/679551712d2357b63e6e0ce858ebf90e/execroot/__main__/bazel-out/aarch64-opt-exec-2B5CBBC6/bin/external/com_google_protobuf/protoc -I=./ -I=/opt/apollo/neo/include  --cpp_out=./  ./modules/common_msgs/chassis_msgs/*.proto
 #qmake apollodriver_nvcan.pro  
 #make
 
@@ -22,7 +23,18 @@ SOURCES += \
         main.cpp \
         canraw.pb.cc  \
         canmsg.pb.cc  \
-        chassis.pb.cc 
+        adchassis.pb.cc  \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/header.pb.cc   \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/error_code.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/control_msgs/control_cmd.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/drive_state.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/pnc_point.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/vehicle_signal.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/control_msgs/input_debug.pb.cc \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/control_msgs/pad_msg.pb.cc  \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/chassis_msgs/chassis.pb.cc   \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/geometry.pb.cc   \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/vehicle_id.pb.cc   
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
@@ -31,6 +43,7 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 
 INCLUDEPATH += /opt/apollo/neo/include
 INCLUDEPATH += /apollo_workspace/bazel-bin/external/apollo_src
+INCLUDEPATH += /apollo_workspace
 
 
 LIBS += -L/opt/apollo/neo/lib/cyber -lcyber -lcyber_binary -lcyber_state

src/apollo/code/apollocontroller_shenlanfd/main.cpp

@@ -9,7 +9,9 @@
 #include "cyber/time/time.h"
 #include "shenlanfd.h"
 #include <canmsg.pb.h>
-#include "chassis.pb.h"
+#include "adchassis.pb.h"
+#include "canmsg.pb.h"
+#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
 
 static bool gbthreadrun = true;
 static std::thread * gpthreadsend;
@@ -23,6 +25,69 @@ static std::shared_ptr<apollo::cyber::Reader<iv::can::canmsg>>
       can0recv_reader_;
 static std::shared_ptr<apollo::cyber::Reader<iv::chassis>>
         chassis_reader_;
+static std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
+        controller_reader_;
+
+static void ExecSend()
+{
+    static int nCount = 0;
+    static int gnIndex = 0;
+    nCount++;
+    iv::can::canmsg xmsg;
+    iv::can::canraw xraw;
+    unsigned char byte_1C4[32];
+    gpShenlanfd->Get1C4(byte_1C4);
+
+    xraw.set_id(0x1C4);
+    xraw.set_data(byte_1C4,32);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(32);
+    iv::can::canraw * pxraw1C4 = xmsg.add_rawmsg();
+    pxraw1C4->CopyFrom(xraw);
+//    qDebug(" 0x144: %02X %02X %02X %02X %02X %02X %02X %02X",byte_144[0],byte_144[1],byte_144[2],byte_144[3],
+//            byte_144[4],byte_144[5],byte_144[6],byte_144[7]);
+    xmsg.set_channel(0);
+    xmsg.set_index(gnIndex);
+
+    unsigned char byte_24E[64];
+    gpShenlanfd->Get1C4(byte_24E);
+    xraw.set_id(0x24E);
+    xraw.set_data(byte_24E,64);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(64);
+    if(nCount%2 == 1)
+    {
+        iv::can::canraw * pxraw24E = xmsg.add_rawmsg();
+        pxraw24E->CopyFrom(xraw);
+    }
+    xmsg.set_channel(0);
+    xmsg.set_index(gnIndex);
+
+    unsigned char byte_25E[32];
+    gpShenlanfd->Get1C4(byte_25E);
+    xraw.set_id(0x25E);
+    xraw.set_data(byte_25E,32);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(32);
+//    if(nCount == 10)
+    if(nCount%2 == 1) //25Ede zhouqi shi 20ms
+    {
+        iv::can::canraw * pxraw25E = xmsg.add_rawmsg();
+        pxraw25E->CopyFrom(xraw);
+//        nCount = 0;
+    }
+    xmsg.set_channel(0);
+    xmsg.set_index(gnIndex);
+
+    std::shared_ptr<iv::can::canmsg> canmsg_ptr;
+    iv::can::canmsg * pcanmsg = new iv::can::canmsg();
+    pcanmsg->CopyFrom(xmsg);
+    canmsg_ptr.reset(pcanmsg);
+    can0send_writer_->Write(canmsg_ptr);
+}
 
 void threadsend()
 {
@@ -31,7 +96,8 @@ void threadsend()
 
     while(gbthreadrun)
     {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        ExecSend();
     }
 
     std::cout<<"  threadsendrecv exit."<<std::endl;
@@ -47,6 +113,62 @@ void RecvChassis(const std::shared_ptr<iv::chassis> &xchassis)
     gpShenlanfd->SetSpeed(xchassis->vel());
 }
 
+void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd)
+{
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = xcmd->brake();
+    double fwheelangle = 0;
+
+    facc = xcmd->acceleration();
+
+        double fVehWeight = 1800;
+        double fg = 9.8;
+        double fRollForce = 50;
+        const double fRatio = 2.5;
+        double fNeed = fRollForce + fVehWeight*facc;
+
+        ftorque = fNeed/fRatio;
+
+        if(fbrake>0.0001)
+        {
+            fbrake = fbrake * (-5.0) /100.0;
+            ftorque = 0;
+        }
+
+        if(ftorque<0)ftorque = 0;
+
+
+
+//    facc = xctrlcmd.linear_acceleration();
+    fwheelangle = xcmd->steering_target() * 430.0/100.0;
+    if(fwheelangle>430)fwheelangle = 430;
+    if(fwheelangle<-430)fwheelangle = -430;
+
+    iv::dcs xdcs;
+    xdcs.mfBrake = fbrake;
+    xdcs.mfTorque = ftorque;
+    xdcs.mfWheel = fwheelangle;
+    xdcs.mblampleft = false; xdcs.mblampright = false;
+    if(xcmd->has_signal())
+    {
+        if(xcmd->signal().has_turn_signal())
+        {
+            if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_LEFT)
+            {
+                xdcs.mblampleft = true;
+            }
+            if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_RIGHT)
+            {
+                xdcs.mblampright = true;
+            }
+        }
+    }
+
+    gpShenlanfd->SetDecision(xdcs);   
+}
+
+
 int main(int argc, char *argv[])
 {
 //    QCoreApplication a(argc, argv);
@@ -68,6 +190,13 @@ int main(int argc, char *argv[])
         }
     );
 
+    controller_reader_ = pilot_node->CreateReader<apollo::control::ControlCommand>(
+        "/aa",
+        [](const std::shared_ptr<apollo::control::ControlCommand> &xcmd){
+            RecvController(xcmd);
+        }
+    );
+
 
 
     gpShenlanfd = new Shenlanfd();