change controller_chery_sterra_es_fcm. for simple signal set, not complete.

include/candbc.h

@@ -84,6 +84,8 @@ public:
     void update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals);
     void query_latest(std::vector<SignalValue> &vals, uint64_t last_ts = 0);
 
+    const DBC * GetDBC();
+
 protected:
     void UpdateCans(const CanData &can);
     void UpdateValid(uint64_t nanos);
@@ -100,5 +102,7 @@ public:
     CANPacker(const std::string& dbc_name,std::istringstream & stringstream);
     std::vector<uint8_t> pack(uint32_t address, const std::vector<SignalPackValue> &values);
     const Msg* lookup_message(uint32_t address);
+
+    const DBC * GetDBC();
 };
 

src/common/candbc/candbc.cpp

@@ -3,6 +3,8 @@
 
 #include "candbc.h"
 
+
+
 unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
     int s = 0;
     bool extended = address > 0x7FF;

src/common/candbc/candbc.h

@@ -84,6 +84,8 @@ public:
     void update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals);
     void query_latest(std::vector<SignalValue> &vals, uint64_t last_ts = 0);
 
+    const DBC * GetDBC();
+
 protected:
     void UpdateCans(const CanData &can);
     void UpdateValid(uint64_t nanos);
@@ -100,5 +102,7 @@ public:
     CANPacker(const std::string& dbc_name,std::istringstream & stringstream);
     std::vector<uint8_t> pack(uint32_t address, const std::vector<SignalPackValue> &values);
     const Msg* lookup_message(uint32_t address);
+
+    const DBC * GetDBC();
 };
 

src/common/candbc/packer.cc

@@ -114,3 +114,8 @@ std::vector<uint8_t> CANPacker::pack(uint32_t address, const std::vector<SignalP
 const Msg* CANPacker::lookup_message(uint32_t address) {
   return dbc->addr_to_msg.at(address);
 }
+
+const DBC * CANPacker::GetDBC(){
+    return dbc;
+}
+

src/common/candbc/parser.cc

@@ -302,3 +302,7 @@ void CANParser::query_latest(std::vector<SignalValue> &vals, uint64_t last_ts) {
     }
   }
 }
+
+const DBC * CANParser::GetDBC(){
+    return dbc;
+}

src/controller/controller_chery_sterra_es_fcm/controller_chery_sterra_es_fcm.pro

@@ -7,6 +7,7 @@ CONFIG += c++17 cmdline
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        dbcsigpacker.cpp \
         main.cpp \
         sterraes.cpp
 
@@ -40,6 +41,7 @@ LIBS += -livprotoif
 LIBS += -lcandbc
 
 HEADERS += \
+    dbcsigpacker.h \
     sterraes.h
 
 RESOURCES += \

src/controller/controller_chery_sterra_es_fcm/dbcsigpacker.cpp

@@ -0,0 +1,37 @@
+#include "dbcsigpacker.h"
+
+#include <iostream>
+
+dbcsigpacker::dbcsigpacker() {}
+
+
+void dbcsigpacker::SetMsgSignal(std::string & strmsgname,std::string & strsigname,const double fvalue){
+
+}
+
+void dbcsigpacker::InitSig()
+{
+    if(mpdbc == NULL){
+        std::cout<<"dbc is null"<<std::endl;
+        return;
+    }
+
+    unsigned int nMsgSize =  mpdbc->msgs.size();
+    unsigned int i;
+    for(i=0;i<nMsgSize;i++){
+        iv::MsgSig xmsgsig;
+        xmsgsig.mid = mpdbc->msgs[i].address;
+        xmsgsig.mstrmsgname = mpdbc->msgs[i].name;
+        unsigned int nSigSize = mpdbc->msgs[i].sigs.size();
+        unsigned int j;
+        for(j=0;j<nSigSize;j++){
+            SignalPackValue spv;
+            spv.name = mpdbc->msgs[i].sigs[j].name;
+            spv.value = 0;
+            xmsgsig.mvectorSPV.push_back(spv);
+        }
+        mmapMsgSig.insert(std::pair<std::string,iv::MsgSig>(xmsgsig.mstrmsgname,xmsgsig));
+    }
+
+
+}

src/controller/controller_chery_sterra_es_fcm/dbcsigpacker.h

@@ -0,0 +1,34 @@
+#ifndef DBCSIGPACKER_H
+#define DBCSIGPACKER_H
+
+#include <map>
+#include <vector>
+#include "candbc.h"
+
+namespace iv {
+struct MsgSig
+{
+    unsigned int mid;  //can message id
+    std::string mstrmsgname;  //can message name
+    std::vector<SignalPackValue> mvectorSPV;
+};
+
+}
+
+class dbcsigpacker
+{
+public:
+    dbcsigpacker();
+
+private:
+    DBC * mpdbc;
+    std::map<std::string,iv::MsgSig> mmapMsgSig;
+
+private:
+    void InitSig();
+
+public:
+    void SetMsgSignal(std::string & strmsgname,std::string & strsigname,const double fvalue);
+};
+
+#endif // DBCSIGPACKER_H

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -143,6 +143,15 @@ double ADS_3_ACCSts = 0;
 
 static bool gbSendBrake = false;
 
+
+static double kp = 0.1;
+static double ki = 0.0;
+static double kd = 0.0;
+static double MAXACC = 3.0;
+static int64_t gnLastAccUptime = 0;
+static bool gbUsePID = true;
+static bool gbPrintPIDOut = true;
+
 static double gfVehAcc = 0.0;
 
 
@@ -181,6 +190,47 @@ void set_ADSCOM2_Signal(std::string strsigname,double value){
 
 void ExecSend();
 
+double  PIDTorque(double torque)
+{
+    if(gbUsePID == false)return torque;
+
+    int nnow = std::chrono::system_clock::now().time_since_epoch().count();
+
+    if((nnow - gnLastAccUptime)>1e9)return torque;
+
+    static double error_previous = 0.0;
+    double fVehWeight = 1800;
+    double fRollForce = 50;
+    const double fRatio = 2.5;
+    double accAim = (torque * fRatio - fRollForce)/fVehWeight;
+
+
+
+    double error = accAim - gfVehAcc;
+    double dt = 0.01; //10ms
+    double diff = (error - error_previous)/dt;
+    double integral = integral + error * dt * ki;
+    double output = error * kp + integral + diff * kd;
+
+    double fAccAjust = accAim + output;
+    if(fAccAjust>MAXACC)fAccAjust = MAXACC;
+    if(fAccAjust<=0)fAccAjust = 0.0;
+
+    double fNeed = fRollForce + fVehWeight*fAccAjust;
+    double ftorqueAjust = fNeed/fRatio;
+
+    if(gbPrintPIDOut)
+    {
+        int64_t timesecond = nnow/1e9;
+        int64_t timepart = nnow - timesecond * 1e9;
+        double fnow = timepart; fnow = fnow/1e9;
+        fnow = fnow + timesecond;
+        std::cout<<fnow<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
+    }
+
+    return ftorqueAjust;
+}
+
 int testnum = 0;
 
 int testwheel = 0;

src/controller/controller_chery_sterra_es_fcm/sterraes.cpp

@@ -270,3 +270,7 @@ void sterraes::GetADSCOM2Data(unsigned char * pdata){
 
     fillcrc(0x0010,pdata);
 }
+
+void SetMsgSignal(std::string & strmsgname,std::string & strsigname,const double fvalue){
+
+}

src/controller/controller_chery_sterra_es_fcm/sterraes.h

@@ -49,6 +49,8 @@ public:
     void GetADSCOM3Data(unsigned char * pdata);
     void GetADSCOM2Data(unsigned char * pdata);
 
+    void SetMsgSignal(std::string & strmsgname,std::string & strsigname,const double fvalue);
+
 
 };