modularization/src/controller/controller_chery_sterra_es/sterraes.cpp

#include "sterraes.h"

#include <iostream>
#include <memory>
#include <string.h>

sterraes::sterraes() {

    mpPacker =  new CANPacker("./ADCC_CH.dbc");
    SignalPackValue sv;
    sv.name = "ADS_1_RollgCntr1";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_Resd1";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerAgReq";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerAgVld";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerPilotAgEna";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_RollgCntr2";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_Resd2";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerTqEna";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_LdwWarningCmd";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_LdwWarningCmdVld";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerMaxTqReq";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_SteerMinTqReq";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "ADS_1_ADSHealthSts";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "CutOutFreshvalues_2CB_S";sv.value = 0;mvectorADSEPS1.push_back(sv);
    sv.name = "CutOutMAC_2CB_S";sv.value = 0;mvectorADSEPS1.push_back(sv);

    sv.name = "ADS_3_RollgCntr1";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_Resd1";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_SteerParkAgReq";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_SteerParkAgVld";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_SteerParkAgEna";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_RollgCntr2";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_Resd2";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_ParkFcnMode";sv.value = 0;mvectorADSEPS3.push_back(sv);
    sv.name = "ADS_3_ADSParkHealthSts";sv.value = 0;mvectorADSEPS3.push_back(sv);

    sv.name = "ADS_1_RollgCntr1";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_Resd1";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotCtrlRepSta";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkCtrlType";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkBrkDecTar";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkCtrlRepMod";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkBrkDecTarVld";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkBrkDecTarEnable";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkDec2StpReq";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PilotParkDriOffReq";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_StopDist";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_ParkCtrlMod";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_1_PreFillReq";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_EPBCtrlReqValid";sv.value = 0;mvectorADSONEBOX1.push_back(sv);
    sv.name = "ADS_EPBCtrlReq";sv.value = 0;mvectorADSONEBOX1.push_back(sv);

    sv.name = "ADS_1_RollgCntr1";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_Resd1";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_DrvTarTq";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_DrvTarTqVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_DrvCtrlReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_CtrlReqMod";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_DrvTarTqEnable";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_AMAPRequest";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_AMAPRequestVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_ADCCAvl";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_AMAPTqLimit";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_AMAPTqLimitVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_RollgCntr2";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_Resd2";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_TarGearReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_TarGearReqVld";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_GearCtrlEnable";sv.value = 0;mvectorADSVCU1.push_back(sv);
    sv.name = "ADS_1_RpaPTReadyReq";sv.value = 0;mvectorADSVCU1.push_back(sv);
}

void sterraes::setsignal(std::vector<SignalPackValue> * pvectorspv,std::string strsigname,double value){
    int size = static_cast<int>(pvectorspv->size());
    int i;
    for(i=0;i<size;i++){
        if(pvectorspv->at(i).name == strsigname){
            pvectorspv->at(i).value = value;
            return ;
        }
    }
    std::cout<<" signal : "<<strsigname.data()<<" not found. please check signal."<<std::endl;
    return ;
}

void sterraes::set_EPS1_signal(std::string strsigname,double value){
    setsignal(&mvectorADSEPS1,strsigname,value);
}

void sterraes::set_EPS3_signal(std::string strsigname,double value){
    setsignal(&mvectorADSEPS3,strsigname,value);
}

void sterraes::set_ONEBOX1_signal(std::string strsigname,double value){
    setsignal(&mvectorADSONEBOX1,strsigname,value);
}

void sterraes::set_VCU1_signal(std::string strsigname,double value){
    setsignal(&mvectorADSVCU1,strsigname,value);
}

unsigned char CRCCheck_SAEJ1850(unsigned char msg[], int len, unsigned char idleCrc)
{
    int i = 0;
    int j = 0;
    unsigned char crc8;
    unsigned char poly = 0x1D;
    crc8 = idleCrc;
    for (i = 0; i<len; i++) {
        crc8 ^= msg[i];
        for (j = 0; j<8; j++) {
            if (crc8 & 0x80) {
                crc8 = (crc8 << 1) ^ poly;
            }
            else {
                crc8 <<= 1;
            }
        }
    }
    //   crc8 ^= 0xFF;
    crc8 ^= 0x00;//crc8 ^= 0xFF;
    return crc8;
}

void sterraes::fillcrc(unsigned short dataid,unsigned char * pdata){
    unsigned char xdata[9];
    memcpy(xdata,&dataid,2);
    memcpy(xdata+2,pdata+1,7);
    unsigned char xcrc = CRCCheck_SAEJ1850(xdata,9,0x0);
    pdata[0] = xcrc;
}

void sterraes::GetEPS1Data(unsigned char * pdata){
    std::vector<uint8_t> xpack =  mpPacker->pack(0x195,mvectorADSEPS1);
    int i;
    for(i=0;i<24;i++)pdata[i] = xpack[i];

    fillcrc(0x0004,pdata);
    fillcrc(0x0005,pdata+8);
}

void sterraes::GetEPS3Data(unsigned char * pdata){
    std::vector<uint8_t> xpack =  mpPacker->pack(0x1BC,mvectorADSEPS3);
    int i;
    for(i=0;i<24;i++)pdata[i] = xpack[i];

    fillcrc(0x0006,pdata);
    fillcrc(0x0007,pdata+8);
}

void sterraes::GetONEBOX1Data(unsigned char * pdata){
    std::vector<uint8_t> xpack =  mpPacker->pack(0x159,mvectorADSONEBOX1);
    int i;
    for(i=0;i<24;i++)pdata[i] = xpack[i];

    fillcrc(0x0008,pdata);
}

void sterraes::GetVCU1Data(unsigned char * pdata){
    std::vector<uint8_t> xpack =  mpPacker->pack(0x167,mvectorADSEPS3);
    int i;
    for(i=0;i<24;i++)pdata[i] = xpack[i];

    fillcrc(0x000B,pdata);
    fillcrc(0x000C,pdata+8);
}