add shenlanfd. not complete.

src/apollo/apollolib/shenlanfd/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/apollo/apollolib/shenlanfd/shenlanfd.cpp

@@ -0,0 +1,269 @@
+#include "shenlanfd.h"
+
+
+// signal: @ACC_LatAngReq    //更改CANid
+#define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_LatAngReq_toS(x) ((int16_t)((x) / 0.1 + 7200))
+
+// signal: @ACC_MotorTorqueMaxLimitRequest
+#define ECU_1C4_ACC_MotorTorqueMaxLimitRequest_CovFactor (0.02)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))
+
+// signal: @ACC_MotorTorqueMinLimitRequest
+#define ECU_1C4_ACC_MotorTorqueMinLimitRequest_CovFactor (0.02)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_MotorTorqueMinLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))
+
+typedef struct
+{
+    int16_t ACC_LatAngReq;
+    //uint8_t ADS_Reqmode;  //20221102， 新车没有此信号
+    int16_t ACC_MotorTorqueMaxLimitRequest;
+    uint8_t ACC_LatAngReqActive;
+    int16_t ACC_MotorTorqueMinLimitRequest;
+    //uint8_t ACC_ADCReqType; //20221102， 新车没有此信号
+} ECU_1C4_t;
+
+// signal: @ACC_AccTrqReq
+#define ECU_24E_ACC_AccTrqReq_CovFactor (1)
+// conversion value to CAN signal
+#define ECU_24E_ACC_AccTrqReq_toS(x) ((int16_t)((x) + 16384))
+
+// signal: @ACC_ACCTargetAcceleration
+#define ECU_24E_ACC_ACCTargetAcceleration_CovFactor (0.05)
+// conversion value to CAN signal
+#define ECU_24E_ACC_ACCTargetAcceleration_toS(x) ((int16_t)((x) / 0.05 + 100))
+
+// signal: @ACC_AEBTargetDeceleration
+#define ECU_24E_ACC_AEBTargetDeceleration_CovFactor (0.0005)
+// conversion value to CAN signal
+#define ECU_24E_ACC_AEBTargetDeceleration_toS(x) ((int32_t)((x) / 0.0005 + 32000))
+
+
+static int gnState = 0; //0 not act  1 act
+typedef struct
+{
+    int16_t ACC_AccTrqReq;
+    uint8_t ACC_AccTrqReqActive;
+    int16_t ACC_ACCTargetAcceleration;
+    int32_t ACC_AEBTargetDeceleration;
+    uint8_t ACC_AEBVehilceHoldReq;
+    uint8_t ADCReqMode;
+    uint8_t ACC_AEBActive;
+    uint8_t ACC_Driveoff_Request;
+    uint8_t ACC_DecToStop;
+    uint8_t ACC_CDDActive;
+    uint8_t ACC_ACCMode;
+} ECU_24E_t;
+
+typedef struct
+{
+    uint8_t CdcDoor;
+    uint8_t res1;
+    uint8_t res2;
+    uint8_t ADS_UDLCTurnLightReq;
+} ECU_25E_t;  //zhuanxiangdeng IDgenghuan
+
+unsigned char byte_1C4[64];//byte_144[8];
+unsigned char byte_24E[64];
+unsigned char byte_25E[32];
+
+ECU_1C4_t _m1C4 = {0,0,0,0};
+ECU_24E_t _m24E = {0,0,0,0,0,0,0,0,0,0,0};
+ECU_25E_t _m25E = {0,0,0,0};
+
+Shenlanfd::Shenlanfd()
+{
+    mpthreadsend = new std::thread(&Shenlanfd::threadsend,this);
+}
+
+Shenlanfd::~Shenlanfd()
+{
+    mbRun = false;
+    mpthreadsend->join();
+}
+
+void Shenlanfd::threadsend()
+{
+    while(mbRun)
+    {
+        iv::dcs xdcs;
+        xdcs.mfTorque = 0;xdcs.mfBrake = 0; xdcs.mfWheel = 0; xdcs.mblampleft = false; xdcs.mblampright = false;
+        int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+        if(abs((nnow - mnLastUpdate)/1e6)<1000)
+        {
+            mmutexdcs.lock();
+            xdcs = mdcs;
+            mmutexdcs.unlock();
+            gnState = 1;
+        }
+        else
+        {
+            gnState = 0;
+        }
+        ExecuteDecision(xdcs);
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+}
+
+void Shenlanfd::ExecuteDecision(iv::dcs & xdcs)
+{
+
+    static int xieya = 50;
+    double fWheelAngleReq = xdcs.mfWheel;
+
+    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
+    _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
+    _m1C4.ACC_LatAngReqActive = 1;
+    _m1C4.ACC_MotorTorqueMinLimitRequest = ECU_1C4_ACC_MotorTorqueMinLimitRequest_toS(-10);
+    if(xdcs.mfBrake>(-0.0000001))
+    {
+        _m24E.ACC_DecToStop =0;
+    }
+    else
+    {
+         _m24E.ACC_DecToStop =1;
+    }
+
+    _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(xdcs.mfTorque);
+    _m24E.ACC_AccTrqReqActive = 1;
+    if(xdcs.mfBrake<(-5.0))
+    {
+        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(-5.0);
+    }
+    else
+        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(xdcs.mfBrake);
+
+
+    if(xdcs.mfBrake>(-0.0000001))
+    {
+
+        if(xieya > 0)
+        {
+            _m24E.ACC_CDDActive = 1;
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0.5);
+            _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(0);
+            _m24E.ACC_Driveoff_Request = 1;
+
+            //_m144.ACC_ADCReqType = 1;//20221102,新车没有此信号
+            //_m24B.ADCReqMode = 1;   //20221102,新车没有此信号
+             _m24E.ACC_DecToStop =1;
+            xieya--;
+        }
+        else
+        {
+            _m24E.ACC_CDDActive = 0;
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0);
+            _m24E.ACC_Driveoff_Request = 0;
+        }
+    }
+    else
+    {
+        _m24E.ACC_CDDActive = 1;
+        _m24E.ACC_Driveoff_Request = 0;
+
+    }
+
+
+
+    byte_1C4[0] = ((_m1C4.ACC_MotorTorqueMaxLimitRequest >> 3) & (0xFFU));
+    byte_1C4[1] = ((_m1C4.ACC_MotorTorqueMaxLimitRequest & (0x07U)) << 5) | ((_m1C4.ACC_MotorTorqueMinLimitRequest >> 6) & (0x1FU));
+    byte_1C4[2] = ((_m1C4.ACC_MotorTorqueMinLimitRequest & (0x3FU)) << 2) | ((_m1C4.ACC_LatAngReq >> 12) & (0x03U));
+    byte_1C4[3] = (((_m1C4.ACC_LatAngReq>>4 )& (0xFFU)));
+    byte_1C4[4] = (((_m1C4.ACC_LatAngReq)& (0x0fU))<<4)|((_m1C4.ACC_LatAngReqActive&(0x01U))<<3);
+
+    if(xdcs.mfBrake>(-0.0000001))
+    {
+        _m24E.ACC_CDDActive = 0;
+        _m24E.ACC_Driveoff_Request = 1;
+        if(xieya > 0)
+        {
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0.5);
+            xieya--;
+        }
+        else
+        {
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0);
+        }
+        _m24E.ACC_AccTrqReqActive = 1;
+        _m24E.ACC_DecToStop = 0;
+
+    }
+    else
+    {
+        _m24E.ACC_CDDActive = 1;
+
+        _m24E.ACC_Driveoff_Request = 0;
+
+        if(mbHaveVehSpd)
+        {
+            if(mfVehSpd < 0.01)
+            {
+                if(xieya != 50)std::cout<<"need xieya. "<<std::endl;
+                xieya = 50;
+
+            }
+        }
+
+        _m24E.ACC_AccTrqReqActive = 0;
+        _m24E.ACC_DecToStop = 1;
+    }
+ //       _m24B.ACC_CDDActive = decition.brake_active();
+    _m24E.ACC_ACCMode = 3;
+
+
+    if(gnState == 0)
+    {
+        _m24E.ACC_CDDActive = 0;
+        _m24E.ACC_Driveoff_Request = 0;
+        _m24E.ACC_ACCMode = 0;
+        _m24E.ACC_ACCTargetAcceleration = 0;
+        _m24E.ACC_AccTrqReqActive = 0;
+        _m24E.ACC_DecToStop = 0;
+        _m1C4.ACC_LatAngReqActive = 0;
+
+    }
+
+
+
+    byte_24E[0] = ((_m24E.ACC_ACCTargetAcceleration) & (0xFFU));
+    //byte_24E[1] = ((_m24B.ACC_AccTrqReq & (0x7FU)) << 1) | (_m24B.ACC_AccTrqReqActive & (0x01U));
+    //byte_24E[2] = ((_m24B.ACC_ACCTargetAcceleration >> 3) & (0x1FU));
+    byte_24E[3] = ((_m24E.ACC_DecToStop & (0x01U)) << 7);// | ((_m24B.ACC_AEBTargetDeceleration >> 15) & (0x01U));
+    //byte_24E[4] = ((_m24B.ACC_AEBTargetDeceleration >> 7) & (0xFFU));
+    byte_24E[5] = ((_m24E.ACC_CDDActive & (0x01U)) << 4);//((_m24E.ACC_AEBTargetDeceleration & (0x7FU)) << 1) | (_m24E.ACC_AEBVehilceHoldReq & (0x01U));
+    byte_24E[6] = ((_m24E.ACC_Driveoff_Request & (0x01U)) << 7)|((_m24E.ACC_ACCMode & (0x07U))<<4);//((_m24E.ADCReqMode & (0x03U)) << 1) | ((_m24E.ACC_AEBActive & (0x01U)) << 3) | ((_m24E.ACC_Driveoff_Request & (0x01U)) << 5) | ((_m24E.ACC_DecToStop & (0x01U)) << 6) | ((_m24E.ACC_CDDActive & (0x01U)) << 7);
+    byte_24E[8] = ((_m24E.ACC_AEBTargetDeceleration>>8) & (0xFFU));
+    byte_24E[9] = ((_m24E.ACC_AEBTargetDeceleration) & (0xFFU));
+    byte_24E[10] = ((_m24E.ACC_AEBActive & (0x01U)) << 7);
+    byte_24E[11] = ((_m24E.ACC_AccTrqReq>>13 )& (0x03U));
+    byte_24E[12] = ((_m24E.ACC_AccTrqReq>>5 )& (0xFFU));
+    //byte_24E[13] = ((_m24E.ACC_AccTrqReq & (0x1FU)<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
+    byte_24E[13] = (((_m24E.ACC_AccTrqReq & (0x1FU))<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
+
+    if(xdcs.mblampleft == true && xdcs.mblampright == false)
+        _m25E.ADS_UDLCTurnLightReq = 3;
+    else if(xdcs.mblampleft == false && xdcs.mblampright == true)
+        _m25E.ADS_UDLCTurnLightReq = 4;
+    else
+        _m25E.ADS_UDLCTurnLightReq = 0;
+
+    byte_25E[3] = ((_m25E.ADS_UDLCTurnLightReq & (0x07U)));
+}
+
+
+void Shenlanfd::SetSpeed(double fSpeed)
+{
+    mfVehSpd = fSpeed;
+    mbHaveVehSpd = true;
+}
+
+void Shenlanfd::SetDecision(iv::dcs & xdcs)
+{
+    mmutexdcs.lock();
+    mdcs = xdcs;
+    mnLastUpdate = std::chrono::system_clock::now().time_since_epoch().count();
+    mmutexdcs.unlock();
+}

src/apollo/apollolib/shenlanfd/shenlanfd.h

@@ -0,0 +1,48 @@
+#ifndef SHENLANFD_H
+#define SHENLANFD_H
+
+#include <thread>
+#include <iostream>
+#include <mutex>
+
+namespace iv {
+struct dcs
+{
+    double mfTorque;
+    double mfBrake;
+    double mfWheel;
+    double mblampleft;
+    double mblampright;
+};
+
+
+}
+
+class  Shenlanfd
+{
+public:
+    Shenlanfd();
+    ~Shenlanfd();
+
+    void SetSpeed(double fSpeed);
+    void SetDecision(iv::dcs &xdcs);
+
+private:
+    void threadsend();
+
+    void ExecuteDecision(iv::dcs & xdcs);
+
+
+private:
+    std::thread * mpthreadsend;
+    bool mbRun = true;
+
+    bool mbHaveVehSpd  = false;
+    double mfVehSpd = 0.0;
+
+    iv::dcs mdcs;
+    std::mutex mmutexdcs;
+    int64_t mnLastUpdate = 0;
+};
+
+#endif // SHENLANFD_H

src/apollo/apollolib/shenlanfd/shenlanfd.pro

@@ -0,0 +1,31 @@
+CONFIG -= qt
+
+TEMPLATE = lib
+DEFINES += SHENLANFD_LIBRARY
+
+CONFIG += c++11
+
+CONFIG += plugin
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    shenlanfd.cpp
+
+HEADERS += \
+    shenlanfd.h
+
+# Default rules for deployment.
+unix {
+    target.path = /usr/lib
+}
+!isEmpty(target.path): INSTALLS += target