modularization/src/controller/controller_chery_sterra_es_fcm/main.cpp

#include <QCoreApplication>

#include <QTime>

#include <QMutex>

#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"

#include "canmsg.pb.h"
#include "decition.pb.h"
#include "chassis.pb.h"

#include "torquebrake.h"

#include <thread>
#include <math.h>

#ifdef Q_OS_LINUX
#include <signal.h>
#endif

#include "candbc.h"
#include "sterraes.h"

std::string gstrdbcpath;
bool gbUseOutDBC;

sterraes * gpsterraes;

static void * gpacansend;
static void * gpadecition;
static void * gpachassis;

static std::string gstrmemdecition;
static std::string gstrmemcansend;
static std::string gstrmemchassis;
static bool gbSendRun = true;

static bool gbChassisEPS = false;

static iv::brain::decition gdecition_def;
static iv::brain::decition gdecition;

static QTime gTime;
static int gnLastSendTime = 0;
static int gnLastRecvDecTime = -1000;
static int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;
static int gnIndex = 0;

static bool gbHaveVehSpd = false;
static double gfVehSpd = 0.0;

static bool gbHaveWheelAngle = false;
static double gfWheelAngle = 0.0;

static double gfWheelSpeedLim = 300; //300 degrees per second


static QMutex gMutex;

static std::thread * gpsendthread = NULL;

unsigned char ADS_EPS_1[24];// 0x195/405
unsigned char ADS_EPS_3[24]; // 0x1BC/444
unsigned char ADS_ONEBOX_1[24];// 0x159/345
unsigned char ADS_VCU_1[24];   // 0x167/359
unsigned char ADS_ONEBOX_2[24];
unsigned char ADS_ONEBOX_3[24];
unsigned char ADS_COM2[8];
unsigned char ADS_COM3[24];

static int gnState = 0; //0 not act  1 act

CANPacker * gpPacker;


std::vector<SignalPackValue> mvectorADSEPS1;
std::vector<SignalPackValue> mvectorADSEPS3;
std::vector<SignalPackValue> mvectorADSONEBOX1;
std::vector<SignalPackValue> mvectorADSVCU1;


double ADS_1_DrvCtrlReq = 0.0;
double ADS_1_CtrlReqMod = 0.0;
double ADS_1_DrvTarTqVld = 0.0;
double ADS_1_DrvTarTqEnable = 0.0;
double ADS_1_DrvTarTq = 0.0;
double ADS_1_TarGearReq = 0.0;
double ADS_1_TarGearReqVld = 0.0;
double ADS_1_GearCtrlEnable = 1.0;


double ADS_1_SteerAgReq = 0.0;
double ADS_1_SteerAgVld = 0.0;
double ADS_1_SteerPilotAgEna = 0.0;

double gfWheelReq = 0.0;
double gfTorqueReq = 0.0;
double gfBrakeReq = 0.0;

double ADS_1_PilotCtrlRepSta = 0.0;
double ADS_1_PilotParkCtrlType =  0.0;
double ADS_1_PilotParkBrkDecTar = 0.0;
double ADS_1_PilotParkCtrlRepMod = 0.0;
double ADS_1_PilotParkBrkDecTarVld = 0.0;
double ADS_1_PilotParkBrkDecTarEnable = 0.0;
double ADS_1_PilotParkDec2StpReq = 0.0;
double ADS_1_PilotParkDriOffReq = 0.0;
double ADS_1_ParkCtrlMod = 0.0;
double ADS_1_ADCCAvl = 0.0;
double ADS_1_StopDist = 0.0;

double FCM_2_AebReqTyp = 0;
double FCM_2_AebTarDec = 0;
double FCM_2_AebTarDecVld = 1.0;
double FCM_2_AwbReq = 0;
double FCM_2_AwbLvl = 0;
double FCM_2_BrkPreFillReq = 0;
double FCM_2_AbaReq = 0;
double FCM_2_AbaLvl = 0;
double FCM_2_Avl = 1;

double FCM_3_PilotParkCtrlRepSta = 0;
double FCM_3_PilotCtrlType = 0;
double FCM_3_PilotkBrkDecTar = 0;
double FCM_3_PilotBrkDecTarVld = 1;
double FCM_3_PilotBrkDecTarReq = 0;
double FCM_3_PilotDec2StpReq = 0;
double FCM_3_PilotDriOffReq = 0;
double CutOutFreshvalues_2CB_S = 0.0;
double CutOutMAC_2CB_S = 0.0;

double FCM_2_SysStatus = 1;
double ADS_2_TTC = 5.11;
double ADS_2_AEBStatus = 0;
double ADS_2_ClosingSpeed = 255;

double ADS_3_ACCSts = 0;

static bool gbSendBrake = false;

static double gfVehAcc = 0.0;



void set_EPS1_signal(std::string strsigname,double value){
    gpsterraes->set_EPS1_signal(strsigname,value);
}

void set_EPS3_signal(std::string strsigname,double value){
    gpsterraes->set_EPS3_signal(strsigname,value);
}

void set_ONEBOX1_signal(std::string strsigname,double value){
    gpsterraes->set_ONEBOX1_signal(strsigname,value);
}

void set_VCU1_signal(std::string strsigname,double value){
    gpsterraes->set_VCU1_signal(strsigname,value);
}

void set_ONEBOX2_signal(std::string strsigname,double value){
    gpsterraes->set_ONEBOX2_signal(strsigname,value);
}

void set_ONEBOX3_signal(std::string strsigname,double value){
    gpsterraes->set_ONEBOX3_signal(strsigname,value);
}

void set_ADSCOM3_Signal(std::string strsigname,double value){
    gpsterraes->set_ADSCOM3_signal(strsigname,value);
}

void set_ADSCOM2_Signal(std::string strsigname,double value){
    gpsterraes->set_ADSCOM2_signal(strsigname,value);
}

void ExecSend();

int testnum = 0;

int testwheel = 0;
void executeDecition(const iv::brain::decition &decition)
{


    double fwheel = decition.wheelangle()*0.9;
    if(fwheel<-430)fwheel = 430;
    if(fwheel>380)fwheel = 380;
    ADS_1_SteerAgReq =fwheel;

//    if(testwheel<1000)ADS_1_SteerAgReq = 180;
//    else ADS_1_SteerAgReq = -180;
//    testwheel++;
//    if(testwheel > 2000)testwheel = 0;

//    std::cout<<"brake: "<<decition.brake()<<std::endl;

//    if((testnum < 1000) || (testnum > 1500))
//    {
    if(decition.brake()<(-0.0001))
    {
        ADS_1_DrvTarTq = 0.0;

        FCM_3_PilotDriOffReq = 0.0;
        FCM_3_PilotDec2StpReq = 1.0;
        FCM_3_PilotkBrkDecTar = decition.brake();// 0.0;//decition.brake();
        FCM_2_AebTarDec = 0;
        FCM_3_PilotBrkDecTarVld = 1.0;
        FCM_3_PilotCtrlType = 0.0;
        FCM_3_PilotBrkDecTarReq = 1.0;

        FCM_2_AebTarDecVld = 1.0;

        FCM_3_PilotParkCtrlRepSta = 1.0;

        if(gbSendBrake == false)
        {
            FCM_3_PilotDec2StpReq = 0.0;
            FCM_3_PilotBrkDecTarReq = 0.0;
            FCM_3_PilotBrkDecTarVld = 1.0;
            FCM_3_PilotkBrkDecTar = 0.0;
            FCM_2_AebTarDec = 1.0;
            FCM_3_PilotParkCtrlRepSta = 0.0;
            FCM_2_AebTarDecVld = 1.0;

            if(fabs(gfVehSpd)>0.1)
            {
                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Warning: can't brake."<<std::endl;
            }
        }
 //       ADS_1_PilotParkCtrlRepMod = 1.0;
 //       std::cout<<" send brake "<<std::endl;
    }

    else
    {
        if(fabs(gfVehSpd) < 0.1)
        {
            FCM_3_PilotDriOffReq = 1.0;
            FCM_3_PilotDec2StpReq = 0.0;
            FCM_3_PilotBrkDecTarReq = 1.0;
            FCM_3_PilotBrkDecTarVld = 1.0;
            FCM_3_PilotkBrkDecTar = 1.0;
            FCM_2_AebTarDec = 1.0;
            FCM_3_PilotParkCtrlRepSta = 1.0;
 //           ADS_1_PilotParkCtrlRepMod = 1.0;
        }
        else
        {
            FCM_3_PilotDec2StpReq = 0.0;
            FCM_3_PilotBrkDecTarReq = 0.0;
            FCM_3_PilotBrkDecTarVld = 1.0;
            FCM_3_PilotkBrkDecTar = 0.0;
            FCM_2_AebTarDec = 1.0;
            FCM_3_PilotParkCtrlRepSta = 0.0;
        }
        FCM_2_AebTarDecVld = 1.0;
        ADS_1_DrvTarTq = decition.torque();

    }
//    }

//    else
//    {

//        if(fabs(gfVehSpd) < 0.1)
//        {
//            FCM_3_PilotDriOffReq = 1.0;
//            FCM_3_PilotDec2StpReq = 0.0;
//            FCM_3_PilotBrkDecTarReq = 1.0;
//            FCM_3_PilotBrkDecTarVld = 1.0;
//            FCM_3_PilotkBrkDecTar = 1.0;
//            FCM_2_AebTarDec = 1.0;
//            FCM_3_PilotParkCtrlRepSta = 1.0;
//        }
//        else
//        {
//            FCM_3_PilotDec2StpReq = 0.0;
//            FCM_3_PilotBrkDecTarReq = 0.0;
//            FCM_3_PilotBrkDecTarVld = 1.0;
//            FCM_3_PilotkBrkDecTar = 0.0;
//            FCM_2_AebTarDec = 1.0;
//            FCM_3_PilotParkCtrlRepSta = 0.0;
//        }
//        ADS_1_DrvTarTq = 300;
//        FCM_2_AebTarDecVld = 1.0;

//    }

//    testnum++;
//    if(testnum > 1500)testnum = 0;




//    std::cout<<" send dec. "<<std::endl;

//    ADS_1_SteerAgReq = 90.0;//decition.wheelangle();

}

void Activate()
{
    //    for(int j=0;j<100000;j++)
    //    {
    std::cout<<" activate "<<std::endl;
    for(int i = 0; i < 3; i++){
        ADS_1_DrvCtrlReq = 1.0;
        ADS_1_CtrlReqMod = 1.0;  //1 Pilot  2 Park
        ADS_1_DrvTarTqVld = 1.0;
        ADS_1_DrvTarTqEnable = 1.0;
        ADS_1_DrvTarTq = 0.0;
        ADS_1_TarGearReq = 4.0;  //1 P 4 D
        ADS_1_TarGearReqVld = 1.0;
        ADS_1_GearCtrlEnable = 0.0;




        ADS_1_SteerAgReq = gfWheelReq;
        ADS_1_SteerAgVld = 1.0;
        ADS_1_SteerPilotAgEna = 2.0;

        ADS_1_PilotParkDec2StpReq = 1.0;
        ADS_1_ParkCtrlMod = 1.0;

        ADS_1_ADCCAvl = 1.0;

        FCM_2_Avl = 1.0;

        ADS_3_ACCSts = 3.0;

        FCM_2_SysStatus = 0;
        ADS_2_AEBStatus = 1;



  //      std::cout<<"activate."<<std::endl;

        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    //    }
}

void UnAcitvate()
{
    if(fabs(gfVehSpd)<0.1)
    {
        for(int i = 0; i < 3; i++){
            ADS_1_DrvCtrlReq = 1.0;
            ADS_1_CtrlReqMod = 2.0;
            ADS_1_DrvTarTqVld = 1.0;
            ADS_1_DrvTarTqEnable = 1.0;
            ADS_1_DrvTarTq = 0.0;
            ADS_1_TarGearReq = 1.0;
            ADS_1_TarGearReqVld = 1.0;
            ADS_1_GearCtrlEnable = 0.0;

            ADS_1_SteerAgReq = 0.0;
            ADS_1_SteerAgVld = 1.0;
            ADS_1_SteerPilotAgEna = 1.0;


            ExecSend();
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    }
    for(int i = 0; i < 3; i++){
        ADS_1_DrvCtrlReq = 0.0;
        ADS_1_CtrlReqMod = 0.0;
        ADS_1_DrvTarTqVld = 0.0;
        ADS_1_DrvTarTqEnable = 0.0;
        ADS_1_DrvTarTq = 0.0;
 //       ADS_1_TarGearReq = 1.0;
        ADS_1_TarGearReqVld = 0.0;
        ADS_1_GearCtrlEnable = 1.0;

        ADS_1_SteerAgReq = 0.0;
        ADS_1_SteerAgVld = 0.0;
        ADS_1_SteerPilotAgEna = 0.0;

        ADS_1_PilotParkDec2StpReq = 0.0;
        ADS_1_ParkCtrlMod = 0.0;

        ADS_1_ADCCAvl = 0.0;

        ADS_3_ACCSts = 0;

        FCM_2_Avl = 1.0;

        FCM_2_SysStatus = 1;
        ADS_2_AEBStatus = 0;

        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::chassis xchassis;
    //    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }

    if(xchassis.has_epsmode())
    {
        if(xchassis.epsmode() == 0)
        {
            gbChassisEPS = true;
        }
    }

    if(xchassis.has_vel())
    {
        gfVehSpd = xchassis.vel();
        gbHaveVehSpd = true;
        //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
    }

    if(xchassis.has_angle_feedback())
    {
        gfWheelAngle = xchassis.angle_feedback();
        gbHaveWheelAngle = true;
    }
}


void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    static qint64 oldtime = QDateTime::currentMSecsSinceEpoch();
    iv::brain::decition xdecition;

    if(!xdecition.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenDecition parse error."<<std::endl;
        return;
    }



    //    if(xdecition.gear() != 4)
    //    {
    //        qDebug("not D");
    //    }
    xdecition.set_angle_mode(1);
    xdecition.set_angle_active(1);
    xdecition.set_acc_active(1);
    xdecition.set_brake_active(1);
    //    xdecition.set_brake_type(1);
    xdecition.set_auto_mode(3);


//    std::cout<<"decition acc: "<<xdecition.accelerator()<<std::endl;

    //   xdecition.set_wheelangle(45.0);

    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %lld diff is %lld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
    oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
    gbChassisEPS = false;

}


void PrepareMsg()
{
    static int rollcouter = 0;
    //    std::cout<<" roll count:: "<<rollcouter<<std::endl;

    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
    set_EPS1_signal("ADS_1_Resd1",0.0);
    set_EPS1_signal("ADS_1_SteerAgReq",ADS_1_SteerAgReq);
    set_EPS1_signal("ADS_1_SteerAgVld",ADS_1_SteerAgVld);
    set_EPS1_signal("ADS_1_SteerPilotAgEna",ADS_1_SteerPilotAgEna);
    set_EPS1_signal("ADS_1_RollgCntr2",rollcouter);
    set_EPS1_signal("ADS_1_Resd2",0.0);
    set_EPS1_signal("ADS_1_SteerTqEna",1.0);
    set_EPS1_signal("ADS_1_LdwWarningCmd",0.0);
    set_EPS1_signal("ADS_1_LdwWarningCmdVld",2.0);
    set_EPS1_signal("ADS_1_SteerMaxTqReq",60.0);
    set_EPS1_signal("ADS_1_SteerMinTqReq",-60.0);
    set_EPS1_signal("ADS_1_ADSHealthSts",1.0);
    set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
    set_EPS1_signal("CutOutMAC_2CB_S",1.0);
    gpsterraes->GetEPS1Data(ADS_EPS_1);

    set_VCU1_signal("ADS_1_RollgCntr1",rollcouter);
    set_VCU1_signal("ADS_1_Resd1",0);
    set_VCU1_signal("ADS_1_DrvTarTq",ADS_1_DrvTarTq);
    set_VCU1_signal("ADS_1_DrvTarTqVld",ADS_1_DrvTarTqVld);
    set_VCU1_signal("ADS_1_DrvCtrlReq",ADS_1_DrvCtrlReq);
    set_VCU1_signal("ADS_1_CtrlReqMod",ADS_1_CtrlReqMod);
    set_VCU1_signal("ADS_1_DrvTarTqEnable",ADS_1_DrvTarTqEnable);
    set_VCU1_signal("ADS_1_RollgCntr2",rollcouter);
    set_VCU1_signal("ADS_1_Resd2",0);
//    set_VCU1_signal("ADS_1_TarGearReq",ADS_1_TarGearReq);
//    set_VCU1_signal("ADS_1_TarGearReqVld",ADS_1_TarGearReqVld);
//    set_VCU1_signal("ADS_1_GearCtrlEnable",ADS_1_GearCtrlEnable);
    set_VCU1_signal("ADS_1_ADCCAvl",ADS_1_ADCCAvl);
    gpsterraes->GetVCU1Data(ADS_VCU_1);

//    set_ONEBOX1_signal("ADS_1_RollgCntr1",rollcouter);
//    set_ONEBOX1_signal("ADS_1_Resd1",0);
//    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlType",ADS_1_PilotParkCtrlType);
//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTar",ADS_1_PilotParkBrkDecTar);
//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarVld",ADS_1_PilotParkBrkDecTarVld);
//    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarEnable",ADS_1_PilotParkBrkDecTarEnable);
//    set_ONEBOX1_signal("ADS_1_PilotParkDec2StpReq",ADS_1_PilotParkDec2StpReq);
//    set_ONEBOX1_signal("ADS_1_PilotParkDriOffReq",ADS_1_PilotParkDriOffReq);
//    set_ONEBOX1_signal("ADS_1_ParkCtrlMod",ADS_1_ParkCtrlMod);
//    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
//    set_ONEBOX1_signal("ADS_1_StopDist",ADS_1_StopDist);
//    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
//    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_1);

    set_ONEBOX2_signal("FCM_2_RollgCntr1",rollcouter);
    set_ONEBOX2_signal("FCM_2_Resd1",rollcouter);
    set_ONEBOX2_signal("FCM_2_AebReqTyp",FCM_2_AebReqTyp);
    set_ONEBOX2_signal("FCM_2_AebTarDec",FCM_2_AebTarDec);
    set_ONEBOX2_signal("FCM_2_AebTarDecVld",FCM_2_AebTarDecVld);
    set_ONEBOX2_signal("FCM_2_AwbReq",FCM_2_AwbReq);
    set_ONEBOX2_signal("FCM_2_AwbLvl",FCM_2_AwbLvl);
    set_ONEBOX2_signal("FCM_2_BrkPreFillReq",FCM_2_BrkPreFillReq);
    set_ONEBOX2_signal("FCM_2_AbaReq",FCM_2_AbaReq);
    set_ONEBOX2_signal("FCM_2_AbaLvl",FCM_2_AbaLvl);
    set_ONEBOX2_signal("FCM_2_Avl",FCM_2_Avl);
    gpsterraes->GetONEBOX2Data(ADS_ONEBOX_2);


    set_ONEBOX3_signal("FCM_3_RollgCntr1",rollcouter);
    set_ONEBOX3_signal("FCM_3_Resd1",rollcouter);
    set_ONEBOX3_signal("FCM_3_PilotParkCtrlRepSta",FCM_3_PilotParkCtrlRepSta);
    set_ONEBOX3_signal("FCM_3_PilotCtrlType",FCM_3_PilotCtrlType);
    set_ONEBOX3_signal("FCM_3_PilotkBrkDecTar",FCM_3_PilotkBrkDecTar);
    set_ONEBOX3_signal("FCM_3_PilotBrkDecTarVld",FCM_3_PilotBrkDecTarVld);
    set_ONEBOX3_signal("FCM_3_PilotBrkDecTarReq",FCM_3_PilotBrkDecTarReq);
    set_ONEBOX3_signal("FCM_3_PilotDec2StpReq",FCM_3_PilotDec2StpReq);
    set_ONEBOX3_signal("FCM_3_PilotDriOffReq",FCM_3_PilotDriOffReq);
    set_ONEBOX3_signal("CutOutFreshvalues_2CB_S",CutOutFreshvalues_2CB_S);
    set_ONEBOX3_signal("CutOutMAC_2CB_S",CutOutMAC_2CB_S);
    gpsterraes->GetONEBOX3Data(ADS_ONEBOX_3);

    set_ADSCOM3_Signal("ADS_3_RollgCntr1",rollcouter);
    set_ADSCOM3_Signal("ADS_3_ACCSts",ADS_3_ACCSts);
    gpsterraes->GetADSCOM3Data(ADS_COM3);

    set_ADSCOM2_Signal("ADS_2_RollgCntr1",rollcouter);
    set_ADSCOM2_Signal("ADS_2_AEBStatus",ADS_2_AEBStatus);
    set_ADSCOM2_Signal("ADS_2_TTC",ADS_2_TTC);
    set_ADSCOM2_Signal("FCM_2_SysStatus",FCM_2_SysStatus);
    gpsterraes->GetADSCOM2Data(ADS_COM2);

    rollcouter++;
    if(rollcouter>=14)rollcouter = 0;
}

void ExecSend()
{
    static int nrd = 0;
    PrepareMsg();
//    gpsterraes->GetEPS1Data(ADS_EPS_1);
    static int nCount = 0;
    nCount++;
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
    //    unsigned char * strp = (unsigned char *)&(ServiceControlStatus.command10.byte[0]);
    //    qDebug("%02x %02x %02x %02x %02x %02x %02x %02x",strp[0],strp[1],strp[2],strp[3],strp[4],strp[5],strp[6],strp[7]);

    xraw.set_id(0x195);
    xraw.set_data(ADS_EPS_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw195 = xmsg.add_rawmsg();
    pxraw195->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);

    xraw.set_id(0x1BC);
    xraw.set_data(ADS_EPS_3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    xmsg.set_channel(0);
 //   iv::can::canraw * pxraw1BC = xmsg.add_rawmsg();
//    pxraw1BC->CopyFrom(xraw);

    xraw.set_id(0x159);
    xraw.set_data(ADS_ONEBOX_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    // iv::can::canraw * pxraw159 = xmsg.add_rawmsg();
    // pxraw159->CopyFrom(xraw);

    xraw.set_id(0x145);
    xraw.set_data(ADS_ONEBOX_2,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw145 = xmsg.add_rawmsg();
    pxraw145->CopyFrom(xraw);

    if(nrd == 0)
    {
    xraw.set_id(0x31A);
    xraw.set_data(ADS_COM3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw31A = xmsg.add_rawmsg();
    pxraw31A->CopyFrom(xraw);

    xraw.set_id(0x314);
    xraw.set_data(ADS_COM2,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw314 = xmsg.add_rawmsg();
    pxraw314->CopyFrom(xraw);
    }



    xraw.set_id(0x14A);
    xraw.set_data(ADS_ONEBOX_3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw14A = xmsg.add_rawmsg();
    pxraw14A->CopyFrom(xraw);

    xraw.set_id(0x167);
    xraw.set_data(ADS_VCU_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw167 = xmsg.add_rawmsg();
    pxraw167->CopyFrom(xraw);


    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    gnIndex++;
    xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
    int ndatasize = xmsg.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser;
    pstrser.reset(strser);
    if(xmsg.SerializeToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }

    nrd++;
    if(nrd == 5)nrd = 0;
}

void initial()
{
    for (uint8_t i = 0; i < 24; i++) //CAN  to  canfd
    {
        //byte_36E[i] = 0;
    }
}

void SendEPS3()
{
    static int rollcouter = 0;
    gpsterraes->GetEPS3Data(ADS_EPS_3);
    ExecSend();
    rollcouter++;
    if(rollcouter>14)rollcouter = 0;
}

void SendEPS1()
{
    static int rollcouter = 0;
//    std::cout<<" roll count:: "<<rollcouter<<std::endl;

    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
    set_EPS1_signal("ADS_1_Resd1",0.0);
    gpsterraes->GetEPS1Data(ADS_EPS_1);
    ExecSend();
    rollcouter++;
    if(rollcouter>14)rollcouter = 0;
}

void testes()
{
    int i = 0;
    int rollcouter = 0;
    double fwheelang = 90.0;

    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
    set_EPS1_signal("ADS_1_Resd1",0.0);
    set_EPS1_signal("ADS_1_SteerAgReq",fwheelang);
    set_EPS1_signal("ADS_1_SteerAgVld",1.0);
    set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
    set_EPS1_signal("ADS_1_RollgCntr2",rollcouter);
    set_EPS1_signal("ADS_1_Resd2",0.0);
    set_EPS1_signal("ADS_1_SteerTqEna",1.0);
    set_EPS1_signal("ADS_1_LdwWarningCmd",0.0);
    set_EPS1_signal("ADS_1_LdwWarningCmdVld",2.0);
    set_EPS1_signal("ADS_1_SteerMaxTqReq",10.0);
    set_EPS1_signal("ADS_1_SteerMinTqReq",1.0);
    set_EPS1_signal("ADS_1_ADSHealthSts",1.0);
    set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
    set_EPS1_signal("CutOutMAC_2CB_S",1.0);

    set_EPS3_signal("ADS_3_RollgCntr1",rollcouter);
    set_EPS3_signal("ADS_3_Resd1",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgReq",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgVld",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgEna",0.0);
    set_EPS3_signal("ADS_3_RollgCntr2",rollcouter);
    set_EPS3_signal("ADS_3_Resd2",0.0);
    set_EPS3_signal("ADS_3_ParkFcnMode",0.0);
    set_EPS3_signal("ADS_3_ADSParkHealthSts",0.0);

    for(i=0;i<10;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // for(i=0;i<10;i++)
    // {
    //     set_EPS3_signal("ADS_3_SteerParkAgEna",1.0);
    //     set_EPS3_signal("ADS_3_SteerParkAgReq",0.0);
    //     set_EPS3_signal("ADS_3_SteerParkAgVld",0.0);
    //     SendEPS3();
    //     std::this_thread::sleep_for(std::chrono::milliseconds(10));
    // }

    for(i=0;i<3000;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",2.0);
        set_EPS1_signal("ADS_1_SteerAgReq",fwheelang);
        set_EPS1_signal("ADS_1_SteerAgVld",1.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    for(i=0;i<10;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

void sendthread()
{
    initial();
    iv::brain::decition xdecition;

    UnAcitvate();
    //   UnAcitvate();

    int nstate = 0; //0 Un 1 Activate
    //    Activate();
    while(gbSendRun)
    {
        if(gnDecitionNum <= 0)
        {
            if(nstate == 1)
            {
                UnAcitvate();
                nstate = 0;
            }
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            if(nstate == 0)
            {
                Activate();
                nstate = 1;
            }
            gMutex.lock();
            xdecition.CopyFrom(gdecition);
            gMutex.unlock();
            gnDecitionNum--;
        }

#ifdef TORQUEBRAKETEST
        if(gnothavetb < 10)
        {
            iv::controller::torquebrake xtb;
            gMutextb.lock();
            xtb.CopyFrom(gtb);
            gMutextb.unlock();
            if(xtb.enable())
            {

                xdecition.set_torque(xtb.torque());
                xdecition.set_brake(xtb.brake());

                std::cout<<" use xtb. torque: "<<xtb.torque()<<" brake: "<<xtb.brake()<<std::endl;

                //                gcontroller->control_torque(xtb.torque());
                //                gcontroller->control_brake(xtb.brake());
                //            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
            }
            else
            {
                //            qDebug("torquebrake not enable.");
            }
            gnothavetb++;
        }

#endif
        executeDecition(xdecition);
        if(gbChassisEPS == false) ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    UnAcitvate();
}

#ifdef Q_OS_LINUX
void sig_int(int signo)
{
    gbSendRun = false;
    gpsendthread->join();

    iv::modulecomm::Unregister(gpacansend);
    iv::modulecomm::Unregister(gpachassis);
    iv::modulecomm::Unregister(gpadecition);

    std::cout<<" controller exit."<<std::endl;
    exit(0);
}
#endif


// void processStream(std::istream& stream) {

//     std::string line;

//     while (std::getline(stream, line)) {

//         std::cout << "Read line: " << line << std::endl;

//     }

// }



void LoadXML(std::string strxmlpath){
    iv::xmlparam::Xmlparam xp(strxmlpath);
    gbUseOutDBC = xp.GetParam("UseOutDBC",false);
    gstrdbcpath = xp.GetParam("dbcpath","./ADCC_CH.dbc");
}

void Listencanrecv0()
{
    void * pblambda = iv::modulecomm::RegisterRecv("canrecv0",[](const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
        (void)strdata;
        (void)nSize;
        (void)index;
        (void)dt;
        (void)strmemname;
        iv::can::canmsg xmsg;
        if(false == xmsg.ParseFromArray(strdata,nSize))
        {
            std::cout<<"controller Listencan0 fail."<<std::endl;
            return;
        }

        int i;
        for(i=0;i<xmsg.rawmsg_size();i++)
        {
            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
            if(praw->id() == 0x14f)
            {
                unsigned char byte[24];
                if(praw->len() == 24)
                {
                    memcpy(byte,praw->data().data(),24);
                    unsigned int value;
                    value = byte[2]&0x01;
                    if(value == 0)
                    {
                         gbSendBrake = true;//std::cout<<" brake available."<<std::endl;
                    }
                    else
                    {
                        gbSendBrake = false;//std::cout<<" warning: no brake unavailable."<<std::endl;
                    }

                }
            }

            if(praw->id() == 0x21)
            {
                unsigned char xdata[8];
                if(praw->len() == 8)
                {
                    memcpy(xdata,praw->data().data(),8);
                    unsigned int value;
                    value = xdata[2]&0xff;
                    value = value<<8;
                    value =value + (xdata[3]&0xff);
                    double facc = value;
                    facc = facc * 0.001 - 2.0;
                    gfVehAcc = facc*9.8;
  //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
                }
            }
        }
    });

    (void)pblambda;


}


#include <QFile>
#include <QTextStream>
#include <strings.h>
#include <streambuf>
#include <sstream>

#include <iostream>
#include <istream>
#include <fstream>
int main(int argc, char *argv[])
{

 //    std::istringstream iss("First line.\nSecond line.\n");



    // // 将 iss 传递给 processStream，这是合法的

    // processStream(iss);

    RegisterIVBackTrace();
    showversion("controller_changan_shenlan");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

    if(argc < 2)
        strpath = strpath + "/controller_chery_sterra_es_fcm.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    LoadXML(strpath.toStdString());

    if(gbUseOutDBC == false)
    {
        QFile xFile;
        xFile.setFileName(":/FCM.dbc");
        if(xFile.open(QIODevice::ReadOnly))
        {
            std::cout<<" open qrc dbc file success. "<<std::endl;
            QTextStream in(&xFile);

            QString content = in.readAll(); // 读取文件全部内容到QString

            xFile.close();



            // 将QString转换为std::string，然后传递给std::istringstream

            std::istringstream iss(content.toStdString());
            gpsterraes = new sterraes(std::string(":/ADCC_CH.dbc"),iss);
        }
        else
            std::cout<<" open qrc dbc file fail. "<<std::endl;

    }
    else
    {
        gpsterraes = new sterraes(gstrdbcpath);
    }


    //    gdecition_def.set_accelerator(-0.5);
    gdecition_def.set_brake(0);
    gdecition_def.set_rightlamp(false);
    gdecition_def.set_leftlamp(false);
    gdecition_def.set_wheelangle(0);

    gdecition_def.set_angle_mode(0);
    gdecition_def.set_angle_active(0);
    gdecition_def.set_acc_active(0);
    //    gdecition_def.set_brake_active(1);
    gdecition_def.set_brake_type(0);
    gdecition_def.set_auto_mode(0);

    //    gdecition_def.set_angle_mode(0);
    //    gdecition_def.set_angle_active(0);
    //    gdecition_def.set_acc_active(0);
    //    gdecition_def.set_brake_active(0);
    //    gdecition_def.set_brake_type(0);
    //    gdecition_def.set_auto_mode(0);

  //  gTime.start();

    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    gstrmemcansend = xp.GetParam("cansend","cansend0");
    gstrmemdecition = xp.GetParam("dection","deciton");
    gstrmemchassis = xp.GetParam("chassismsgname","chassis");

    gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
    gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);

#ifdef TORQUEBRAKETEST
    EnableTorqueBrakeTest();
#endif

//    testes();
//    return 0;

    Listencanrecv0();

    std::thread xthread(sendthread);

    gpsendthread = &xthread;

#ifdef Q_OS_LINUX
    signal(SIGINT, sig_int);
    signal(SIGTERM,sig_int);
#endif

    return a.exec();
}