modularization/src/controller/controller_changan_shenlan_v2/main.cpp

#include <QCoreApplication>

#include <QTime>

#include <QMutex>

#include "control/control_status.h"
#include "control/controller.h"

#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 "remotectrl.pb.h"

#include <thread>

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

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

static std::string gstrmemdecition;
static std::string gstrmemcansend;
static std::string gstrmemchassis;
std::string gstrmemremote; //Remote Ctrl
static bool gbSendRun = true;

static bool gbChassisEPS = false;

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

bool gbAllowRemote = false;   //Default, Not Allow Remote

qint64 gLastRemoteTime = 0;

bool gbAutoDriving = true; //if true, Auto Driving, false, remote controll

iv::brain::decition gdecition_remote;

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 boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器

static QMutex gMutex;

static std::thread * gpsendthread = NULL;

static double kp = 0.1;
static double ki = 0.0;
static double kd = 0.0;
static double MAXACC = 3.0;
static double gfVehAcc = 0.0;
static int64_t gnLastAccUptime = 0;
static bool gbUsePID = true;
static bool gbPrintPIDOut = true;
static bool gbAccValid = false;


// 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};

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;
}

void executeDecition(const iv::brain::decition &decition)
{




    static int xieya = 50;
//     std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
//     std::cout<<"angle_mode is "<<decition.angle_mode()<<" angle_active is "<<decition.angle_active()<<std::endl;
//     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
//     std::cout<<"brake is "<<decition.brake()<<" brake_active is "<<decition.brake_active()<<std::endl;
//     std::cout<<"auto_mode is "<<decition.auto_mode()<<" rightlamp is "<<decition.rightlamp()<<std::endl;

    double fWheelAngleReq = decition.wheelangle();
    double fsendinter = 0.02;
//    if(fabs(fWheelAngleReq - gfWheelAngle)/fsendinter > gfWheelSpeedLim)
//    {
//        if(fWheelAngleReq > gfWheelAngle)
//        {
//            fWheelAngleReq = gfWheelAngle + fsendinter * gfWheelSpeedLim;
//        }
//        else
//        {
//            fWheelAngleReq = gfWheelAngle - fsendinter * gfWheelSpeedLim;
//        }

//    }

//    std::cout<<" wheel req: "<<decition.wheelangle()<<" real send : "<<fWheelAngleReq<<" real whhel:"<<gfWheelAngle<< std::endl;

    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
    //_m1C4.ADS_Reqmode = decition.angle_mode(); //20221102,新车没有此信号
    _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
    _m1C4.ACC_LatAngReqActive = decition.angle_active();
    _m1C4.ACC_MotorTorqueMinLimitRequest = ECU_1C4_ACC_MotorTorqueMinLimitRequest_toS(-10);
//    _m144.ACC_ADCReqType = decition.brake_type();//ADC请求类型（制动时为1，其余情况为0）
    if(decition.brake()>(-0.0000001))
    {
        //_m144.ACC_ADCReqType = 0;//ADC请求类型（制动时为1，其余情况为0）//20221102,新车没有此信号
       // _m24B.ADCReqMode = 0;//20221102,新车没有此信号
        _m24E.ACC_DecToStop =0;
    }
    else
    {
        //_m144.ACC_ADCReqType = 1;//20221102,新车没有此信号
       // _m24B.ADCReqMode = 1;//20221102,新车没有此信号
         _m24E.ACC_DecToStop =1;
    }

//    std::cout<<" type: "<<(int)_m144.ACC_ADCReqType<<std::endl;


    /*制动过程用的减速度，加速用扭矩*/
    double ftorque = decition.torque();
    if(ftorque>0.1)ftorque = PIDTorque(ftorque);
    _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(decition.torque());
    _m24E.ACC_AccTrqReqActive = decition.acc_active();
    if(decition.brake()<(-5.0))
    {
        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(-5.0);
    }
    else
        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(decition.brake());


 //   std::cout<<" brake : "<<decition.brake()<<std::endl;
 //   std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
    if(decition.brake()>(-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--;
            std::cout<<" xieya. now veh speed: "<<gfVehSpd<<std::endl;
        }
        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;


    }


 //       _m24B.ACC_CDDActive = decition.brake_active();



//    std::cout<<" req mode: "<<_m144.ADS_Reqmode<<std::endl;
    //byte_144[0] = ((_m144.ACC_LatAngReq >> 9) & (0x1FU)) | ((_m144.ADS_Reqmode & (0x07U)) << 5);
 //   qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
   // byte_144[1] = ((_m144.ACC_LatAngReq >> 1) & (0xFFU));
    //byte_144[2] = ((_m144.ACC_LatAngReq & (0x01U)) << 7) | ((_m144.ACC_MotorTorqueMaxLimitRequest >> 5) & (0x3FU)) | ((_m144.ACC_LatAngReqActive & (0x01U)) << 6);
    //byte_144[3] = ((_m144.ACC_MotorTorqueMaxLimitRequest & (0x1FU)) << 3) | ((_m144.ACC_MotorTorqueMinLimitRequest >> 8) & (0x07U));
    //byte_144[4] = (_m144.ACC_MotorTorqueMinLimitRequest & (0xFFU));
    //byte_144[5] = ((_m144.ACC_ADCReqType & (0x03U)) << 6);


    byte_1C4[0] = ((_m1C4.ACC_MotorTorqueMaxLimitRequest >> 3) & (0xFFU));
 //   qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
    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<<4) & (0xF0U)))|((_m1C4.ACC_LatAngReqActive&(0x01U))<<3);
    byte_1C4[4] = (((_m1C4.ACC_LatAngReq)& (0x0fU))<<4)|((_m1C4.ACC_LatAngReqActive&(0x01U))<<3);
    //byte_1C4[5] = ((_m144.ACC_ADCReqType & (0x03U)) << 6);

    //    _m24B.ACC_AEBTargetDeceleration = 0;
    //    _m24B.ACC_AEBVehilceHoldReq = 0;
    //    _m24B.ADCReqMode = 0;
    //    _m24B.ACC_AEBActive = 0;
    //    _m24B.ACC_Driveoff_Request = 0;
    //    _m24B.ACC_DecToStop = 0;
//    std::cout<<" brake : "<<decition.brake()<<std::endl;
//    std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
    if(decition.brake()>(-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(gbHaveVehSpd)
        {
            if(gfVehSpd < 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 = decition.auto_mode();


    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;

    }
//    std::cout<<"acc mode: "<<(int)_m24B.ACC_ACCMode<<std::endl;


   // byte_24B[0] = ((_m24B.ACC_AccTrqReq >> 7) & (0xFFU));
    //byte_24B[1] = ((_m24B.ACC_AccTrqReq & (0x7FU)) << 1) | (_m24B.ACC_AccTrqReqActive & (0x01U));
    //byte_24B[2] = ((_m24B.ACC_ACCTargetAcceleration >> 3) & (0x1FU));
    //byte_24B[3] = ((_m24B.ACC_ACCTargetAcceleration & (0x07U)) << 5) | ((_m24B.ACC_AEBTargetDeceleration >> 15) & (0x01U));
    //byte_24B[4] = ((_m24B.ACC_AEBTargetDeceleration >> 7) & (0xFFU));
    //byte_24B[5] = ((_m24B.ACC_AEBTargetDeceleration & (0x7FU)) << 1) | (_m24B.ACC_AEBVehilceHoldReq & (0x01U));
    //byte_24B[6] = ((_m24B.ADCReqMode & (0x03U)) << 1) | ((_m24B.ACC_AEBActive & (0x01U)) << 3) | ((_m24B.ACC_Driveoff_Request & (0x01U)) << 5) | ((_m24B.ACC_DecToStop & (0x01U)) << 6) | ((_m24B.ACC_CDDActive & (0x01U)) << 7);
    //byte_24B[7] = (_m24B.ACC_ACCMode & (0x07U));


    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(decition.leftlamp() == true && decition.rightlamp() == false)
        _m25E.ADS_UDLCTurnLightReq = 3;
    else if(decition.leftlamp() == false && decition.rightlamp() == true)
        _m25E.ADS_UDLCTurnLightReq = 4;
    else
        _m25E.ADS_UDLCTurnLightReq = 0;

    byte_25E[3] = ((_m25E.ADS_UDLCTurnLightReq & (0x07U)));

}

void Activate()
{
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    iv::brain::decition xdecition;
    xdecition.set_brake(0.0);
    xdecition.set_torque(0.0);
//    for(int j=0;j<100000;j++)
//    {
        std::cout<<" run "<<std::endl;
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0.0);
        xdecition.set_angle_mode(0);
        xdecition.set_angle_active(0);
        xdecition.set_acc_active(0);
        xdecition.set_brake_active(0);
//        xdecition.set_brake_type(0);
        xdecition.set_auto_mode(0);
        gnState = 0;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0.0);
        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);
        gnState = 1;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
//    }
}

void UnAcitvate()
{
    iv::brain::decition xdecition;

    xdecition.set_brake(0.0);
    xdecition.set_torque(0.0);
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0);
        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);
        gnState = 1;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0);
        xdecition.set_angle_mode(0);
        xdecition.set_angle_active(0);
        xdecition.set_acc_active(0);
        xdecition.set_brake_active(0);
//        xdecition.set_brake_type(0);
        gnState = 0;
        xdecition.set_auto_mode(0);
        executeDecition(xdecition);
        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);

 //   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 ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index; (void)dt; (void)strmemname;
    iv::brain::decition xdecition;

    iv::remotectrl xrc;

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

    if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
    {
        gbAutoDriving = true;
    }
    else
    {
        gbAutoDriving = false;
        xdecition.set_accelerator(xrc.acc());
        xdecition.set_brake(xrc.brake());
        xdecition.set_wheelangle(xrc.wheel());

        if(xrc.acc()>0.0)xdecition.set_torque(xrc.acc() * 100);

        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);

        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();

        gnDecitionNum = gnDecitionNumMax;
        gbChassisEPS = false;

    }

}

void ExecSend()
{
    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(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);

    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);

    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);

    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;
    }
}

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

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

    UnAcitvate();
 //   UnAcitvate();

    int nstate = 0; //0 Un 1 Activate
//    Activate();
    while(gbSendRun)
    {

        if(gbAutoDriving)
        {
            if(gnDecitionNum <= 0)
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_def);
                gMutex.unlock();
            }
            else
            {
                xdecition.CopyFrom(gdecition);
                gnDecitionNum--;
            }
        }
        else
        {
            if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 1000)
            {
                gbAutoDriving = true;
                continue;
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_remote);
                gMutex.unlock();
            }
        }


        if(gnDecitionNum <= 0)
        {
            if(nstate == 1)
            {
                UnAcitvate();
                nstate = 0;
            }
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            if(nstate == 0)
            {
                Activate();
                nstate = 1;
            }
            if(gbAutoDriving)
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition);
                gMutex.unlock();
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_remote);
                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 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;
        }
        static double  fLastVehSpeed = 0;
        static bool baccnotokcount = 0;


        int i;
        for(i=0;i<xmsg.rawmsg_size();i++)
        {

            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
            if(praw->id() == 0x1CC)
            {
                if(praw->len() == 64)
                {
                    unsigned char xdata[64];
                    memcpy(xdata,praw->data().data(),64);
                    unsigned int value = xdata[10]; value = value<<7;
                    value = value +((xdata[11]&0xFE)>>1);
                    double facc = value;
                    facc = facc * 0.01 - 32.0;
   //                 qDebug(" %02X %02X",xdata[10],xdata[11]);
    //                gfVehAcc = facc;   //ESP Acc not ok.
                    value = xdata[12]&0x1f;value = value<<8;
                    value = value + xdata[13];
                    double fvehspeed = value;fvehspeed = fvehspeed * 0.05625;
                    int64_t timediff = (std::chrono::system_clock::now().time_since_epoch().count() - gnLastAccUptime)/1e6;
                    if(timediff<16)
                    {
                        double facccalc = ((fvehspeed - fLastVehSpeed)/3.6)/0.01;
                        gfVehAcc = facccalc;
                        gbAccValid = true;
                        baccnotokcount = 0;
                    }
                    else
                    {
                        baccnotokcount++;
                        if(baccnotokcount>3)gbAccValid = false;
                    }

                    fLastVehSpeed = fvehspeed;
  //                  std::cout<<" diff : "<<(std::chrono::system_clock::now().time_since_epoch().count() - gnLastAccUptime)/1e6<<std::endl;
                    gnLastAccUptime =  std::chrono::system_clock::now().time_since_epoch().count();
                                      std::cout<<" acc : "<<gfVehAcc<<std::endl;

                }
            }
            if(praw->id() == 0x3)
            if(praw->id() == 0x322)
            {
                if(praw->len() == 8)
                {
                    unsigned char xdata[8];
                    memcpy(xdata,praw->data().data(),8);
                    unsigned int value = xdata[1]; value = value<<12;
                    value = value +((xdata[2]&0xFF)<<4);
                    value = value + xdata[3];
                    double fmotorrpm = value - 20000;
                    (void)fmotorrpm;
   //                 std::cout<<" motor speed : "<<fmotorrpm<<std::endl;
                }
            }
        }
    });

    (void)pblambda;


}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_changan_shenlan");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

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

//    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();

    gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());

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

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

    kp = xp.GetParam("kp",0.1);
    ki = xp.GetParam("ki",0.0);
    kd = xp.GetParam("kd",0.0);
    MAXACC = xp.GetParam("MAXACC",3.0);
    gbUsePID =  xp.GetParam("UsePID",false);
    gbPrintPIDOut = xp.GetParam("PrintPIDOut",false);

    std::string strremote = xp.GetParam("allowremote","true");
    if(strremote == "true")
    {
        gbAllowRemote = true;
    }

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

    if(gbAllowRemote)
    {
        gparemote = iv::modulecomm::RegisterRecv(gstrmemremote.data(),ListenRemotectrl);
    }

#ifdef TORQUEBRAKETEST
    EnableTorqueBrakeTest();
#endif

    std::thread xthread(sendthread);

    Listencanrecv0();

    gpsendthread = &xthread;

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

    return a.exec();
}