modularization/src/controller/controller_Geely_xingyueL/main.cpp

#if 1
#include <QCoreApplication>

#include <QTime>
#include <QMutex>
#include <QTimer>
#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"
#include "decition.pb.h"
#include "chassis.pb.h"
#include "remotectrl.pb.h"
#include "gpsimu.pb.h"
#include <thread>
#include <iostream>
#include <fstream>
#include "include/car_control.h"


enum class AppCtrlSm   //cxw,20220622
{
  kStandby = 0,
  kStartup = 1,
  kActive = 2,
  kShutDown = 3
};

CarControl car_control_module;
AppCtrlSm app_ctrl_car_sm_ = AppCtrlSm::kStandby;

using namespace std;




void * gpadecition;

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

int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;

static QMutex gMutex;

bool gstatus;
GearPrkgAssistReq ggearSetVal;
GearLevelIndicate ggearRealVal;
ChassisErrCode gchassErr;
StsMach gstsMach;
float gsteerDeg, gspeed;
double lastspeedSetVal = 0;
double lastEpsSetVal = 0;

GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;

const int gCommunctionNum = 10;
int communicate_cnt=0; //判断decition中是否有决策，超过设定的数值以后认为没有决策退出控制车辆
bool communicate_state=0;
int  case_state=0;//记录switch  case的状态，调试用

static bool gbAutoDriving = true;
static iv::brain::decition gdecition_remote;
static qint64 gLastRemoteTime = 0;
static qint64 gLastGPSIMUTime = 0;
static double gfVehSpeed = 0; // km/h
const  double gfMaxRemoteVehSpeed = 10; //Max Speed in Remote Mode

void quit_ctrl()  //退出车辆控制，cxw,20220622
{
    car_control_module.set_lat_lgt_ctrl_req(false);//握手请求, true:请求握手， false:退出握手
    car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);// 目标档位请求
    car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());//设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负
    car_control_module.set_target_acc_mps2(0);//// 目标加减速度值
    car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);// 转向灯控制请求
}

static void ShareChassis(void * pa,iv::chassis  * pchassis)
{
    char * str;
    int ndatasize = pchassis->ByteSize();
    str = new char[ndatasize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(!pchassis->SerializeToArray(str,ndatasize))
    {
        std::cout<<"ShareChassis Error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);

}

void executeDecition(const iv::brain::decition decition)
{
    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;

    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
    double speedSetVal = 0;
    double EpsSetVal = 0;
    double Steer_current_value=0;
    Steer_current_value=car_control_module.get_current_steer_ang_in_deg();

    if(decition.has_gear()){
        switch (decition.gear()) {
        case 1:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
            gearSetVal=GearPrkgAssistReq::kTargetGearP;
            break;
        case 2:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
            gearSetVal=GearPrkgAssistReq::kTargetGearR;
            break;
        case 3:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
            gearSetVal=GearPrkgAssistReq::kTargetGearD;
            break;
        default:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
            gearSetVal=GearPrkgAssistReq::kNoRequest;;
            break;
        }
        lastgearSetVal=gearSetVal;
    }
    else
    {
        gearSetVal=lastgearSetVal;
    }

    if(decition.has_accelerator()){
        speedSetVal=decition.accelerator();
        lastspeedSetVal=speedSetVal;
        //speedSetVal=0.1;
       // car_control_module.set_target_acc_mps2(decition.accelerator());
//        car_control_module.set_target_acc_mps2(0.1);
    }
    else
    {
        speedSetVal=lastspeedSetVal;
    }

    if(decition.has_wheelangle())
    {
       // car_control_module.set_target_pinion_ag_in_deg(0.0);
        EpsSetVal=decition.wheelangle();
        lastEpsSetVal=EpsSetVal;
       // EpsSetVal=0.0;//
  // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
    }
    else
    {
        EpsSetVal=lastEpsSetVal;
    }
    // gearSetVal=GearPrkgAssistReq::kTargetGearD;

    if(abs(EpsSetVal-Steer_current_value)<90)//限制车辆方向盘转角，cxw,20220622
    {
        ;
    }
    else
    {
        if(EpsSetVal-Steer_current_value>0)
            EpsSetVal=Steer_current_value+90;
        else
            EpsSetVal=Steer_current_value-90;
    }


    car_control_module.set_target_gear(gearSetVal);
    car_control_module.set_target_acc_mps2(speedSetVal);
    car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);

    if(decition.has_leftlamp() && decition.leftlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
    else if(decition.has_rightlamp() && decition.rightlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
    else
        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);

    /*
    void set_lat_lgt_ctrl_req(bool req);  // 握手请求, true:请求握手， false:退出握手
    void set_target_gear(GearPrkgAssistReq tar);  // 目标档位请求
    bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
    void set_target_pinion_ag_in_deg(float32_t ang_req);// 设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负。
    void set_target_acc_mps2(float32_t tar_acc);// 目标加减速度值。
    void set_turn_light_status(TurnLightIndicReq req);// 转向灯控制请求
*/
}


void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//        ggpsimu->UpdateGPSIMU(strdata,nSize);

    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRaw Parse error."<<std::endl;
        return;
    }

    double fSpeed = xgpsimu.speed() *3.6;
//    std::cout<<" speed is "<<fSpeed<<std::endl;
    gfVehSpeed = fSpeed;
    gLastGPSIMUTime = QDateTime::currentMSecsSinceEpoch();
//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());

}


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

    iv::remotectrl xrc;

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

    std::cout<<" recv remote."<<std::endl;

    if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
    {
        gbAutoDriving = true;
    }
    else
    {
        gnDecitionNum = gnDecitionNumMax;
        gbAutoDriving = false;

        if(gfVehSpeed>=(gfMaxRemoteVehSpeed*1.1))
        {
          xdecition.set_accelerator(0.0);
        }
        else
        {
            if(gfVehSpeed>gfMaxRemoteVehSpeed)
            {
                xdecition.set_accelerator(xrc.acc()*0.01*(gfMaxRemoteVehSpeed*1.1-gfVehSpeed)/(gfMaxRemoteVehSpeed*0.1));
            }
            else
            {
                if(abs(QDateTime::currentMSecsSinceEpoch() - gLastGPSIMUTime)<3000)
                {
                    xdecition.set_accelerator(xrc.acc()*0.01);
                }
                else
                {
                    xdecition.set_accelerator(0.0);
                }
            }
        }
 //       xdecition.set_accelerator(xrc.acc()*0.01);
        if(xrc.brake()>0.01)
        {
            xdecition.set_accelerator(xrc.brake()*(-0.03));
        }
//        xdecition.set_brake(0);  //not use brake
        xdecition.set_wheelangle(xrc.wheel() *5.5);
        xdecition.set_gear(3);
        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();

    }

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

}


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

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

    if(xdecition.gear() != 3)
    {
        qDebug("not D");
    }
    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
        // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
        oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
//    std::cout<<"update decision. "<<std::endl;
}


void sendthread()
{
    iv::brain::decition xdecition;
    static QDateTime dt1 = QDateTime::currentDateTime();
//    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
//    car_control_module.sm_task_20ms();  // 线控状态机函数
//QTime time;
//time.start();

    while(1)
    {
//        int t = time.elapsed();
//        if(t > 21)
//            qDebug("%d",t);
//        time.restart();
        if(gnDecitionNum <= 0)
        {
//            gdecition_def.set_wheelangle(car_control_module.get_current_steer_ang_in_deg());
//            xdecition.CopyFrom(gdecition_def);
//            std::cout<<"copy from gdecition_def@@@@@@@@@@@@"<<std::endl;
            communicate_cnt++;
        }
        else
        {
            if(gbAutoDriving == false)
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_remote);
    //            std::cout<<"copy from ADAS gdecition"<<std::endl;
                gMutex.unlock();
                if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 3000)
                {
                    gbAutoDriving = true;
                }
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition);
                //            std::cout<<"copy from ADAS gdecition"<<std::endl;
                gMutex.unlock();
            }
            gnDecitionNum--;
            communicate_cnt=0;
            communicate_state=1;
        }
        if(communicate_cnt>gCommunctionNum)
        {
            communicate_state=0;   //说明decition无决策
            communicate_cnt=gCommunctionNum+1;
        }

#if 1
        switch (app_ctrl_car_sm_)
        {
            case AppCtrlSm::kStandby://ctrl_car节点有消息输入，否则退出与汽车的控制
                if (communicate_state!=0)
                {
                    app_ctrl_car_sm_ = AppCtrlSm::kStartup;
                    case_state=0;
                }
                else
                {
                    quit_ctrl();
                    std::cout<<" no decition "<<std::endl;
                    case_state=-1;
                }
                break;
            case AppCtrlSm::kStartup:
                if (car_control_module.is_lat_lgt_ctrl_active()) //底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
                {
                    app_ctrl_car_sm_ = AppCtrlSm::kActive;
                    case_state=10;
                }
                else {
                    if (!communicate_state){//通讯失联
                        app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
                        case_state=11;
                    }
                    else {
                        car_control_module.set_lat_lgt_ctrl_req(true);
                        case_state=12;
                    }
                    // app_ctrl_car_sm_ = AppCtrlSm::kStandby;
                    // quit_ctrl();
                }
                break;
            case AppCtrlSm::kActive:
                if (!(communicate_state && car_control_module.is_lat_lgt_ctrl_active()))//通讯失联或底盘不可控
                {
                    app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
                    case_state=30;
                    if(!car_control_module.is_lat_lgt_ctrl_active())
                    {
                        //ctrl_active_state_ =false;
                        quit_ctrl();
                        std::cout<<"quit ADAS "<<std::endl;
                        case_state=31;
                    }
                }
                else
                {
                    executeDecition(xdecition);
                    case_state=32;
                }
                break;
            case AppCtrlSm::kShutDown:
                if (car_control_module.get_chassis_statemachine_sts() != StsMach::kApaActive &&
                    car_control_module.get_chassis_statemachine_sts() != StsMach::kHwaActive)
                {
                    case_state=20;
                    app_ctrl_car_sm_ = AppCtrlSm::kStandby;
                }
                else
                {
                    car_control_module.set_lat_lgt_ctrl_req(false);
                    case_state=21;
                }
                break;
            default:
                app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
                case_state=60;
                break;
        }

#endif






//        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
                    std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
                            <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
//        if(bstatus == true)
//        {
//            //            std::cout<<"di pan ke kong "<<std::endl;
//            executeDecition(xdecition);
//        }
//        else
//        {
//            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
//                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
//            car_control_module.set_lat_lgt_ctrl_req(true);
//        }




#if 0
                       string filename="log.txt";
                       ofstream outfile;
                       outfile.open(filename, ostream::app);
                       QDateTime dt2=QDateTime::currentDateTime();
                       qint64 msec = dt1.msecsTo(dt2);
                       dt1=QDateTime::currentDateTime();
                       outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                              <<"time_cycle"<<","<< msec <<","
                              <<"chasis_status"  << ","  <<(int)car_control_module.get_chassis_err_state()<< ","
                              <<"machine_state"<< ","<<(int)car_control_module.get_chassis_statemachine_sts()<< ","
                              <<"Decide_ACC"  << ","  <<lastspeedSetVal << ","
//                              <<"Decide_gear"<< ","  <<lastgearSetVal << ","
//                              <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.<< ","
                              <<"Decide_Angle"<< "," <<lastEpsSetVal << ","
                              <<"communicate_cnt"<< ","<<communicate_cnt<< ","
                              <<"communicate_state"<< ","<<communicate_state<< ","
                              <<"case_state"<< ","<< case_state<< ","
//                              <<"Trace_Point"<< ","<<PathPoint<< ","
//                              <<"OBS_Distance"<< ","<<obsDistance<< ","                            
                              <<endl;
                       outfile.close();
#endif
//        car_control_module.sm_task_20ms();  // 线控状态机函数
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}

void * gpa;

void recvthread()
{
    iv::chassis xchassis;
    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
    while(1)
    {
        gstatus = car_control_module.is_lat_lgt_ctrl_active();
        ggearSetVal = car_control_module.get_setted_tar_gear();
        ggearRealVal = car_control_module.get_cur_disp_gear();
        gchassErr = car_control_module.get_chassis_err_state();
        gstsMach = car_control_module.get_chassis_statemachine_sts();
        gspeed = car_control_module.get_current_vehicle_spd_in_ms();
        gsteerDeg = car_control_module.get_current_steer_ang_in_deg();

        std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;


        xchassis.set_angle_feedback(gsteerDeg);

        ShareChassis(gpa,&xchassis);
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        /*
            bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
            ChassisErrCode get_chassis_err_state();// 底盘错误状态码
            StsMach get_chassis_statemachine_sts(); // 内部状态机运行状态
            float32_t get_current_steer_ang_in_deg();// 当前方向盘实际角度
            float32_t get_current_vehicle_spd_in_ms();// 当前车辆实际车速，单位ｍ／ｓ
            GearPrkgAssistReq get_setted_tar_gear(); // 获取当前设定目标档位值
            GearLevelIndicate get_cur_disp_gear(); // 当前实际显示档位状态
        */
    }
}

int main(int argc, char *argv[])
{
    auto ret = google_glog_config("/home/nvidia/log1/.");

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

    QString strpath = QCoreApplication::applicationDirPath();

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

    //    car_control_module.set_lat_lgt_ctrl_req(true);
    //    car_control_module.sm_task_20ms();  // 线控状态机函数

    //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);

    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    std::string gstrmemdecition = xp.GetParam("dection","deciton");
    std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");

    gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);

    void * pa1 = iv::modulecomm::RegisterRecv("hcp2_gpsimu",Listengpsimu);
    void * pa2 = iv::modulecomm::RegisterRecv("remotectrl",ListenRemotectrl);

    std::thread xthread(sendthread);
//    std::thread myxthread(recvthread);

    return a.exec();
}

#else

#include <QCoreApplication>

#include <QTime>
#include <QMutex>
#include <QTimer>
#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"
#include "decition.pb.h"
#include "chassis.pb.h"
#include <thread>
#include "include/car_control.h"

CarControl car_control_module;

void * gpadecition;

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

int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;

static QMutex gMutex;

bool gstatus;
GearPrkgAssistReq ggearSetVal;
GearLevelIndicate ggearRealVal;
ChassisErrCode gchassErr;
StsMach gstsMach;
float gsteerDeg, gspeed;
double lastspeedSetVal = 0;
double lastEpsSetVal = 0;

GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;

static void ShareChassis(void * pa,iv::chassis  * pchassis)
{
    char * str;
    int ndatasize = pchassis->ByteSize();
    str = new char[ndatasize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(!pchassis->SerializeToArray(str,ndatasize))
    {
        std::cout<<"ShareChassis Error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);

}

void executeDecition(const iv::brain::decition decition)
{
    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;

    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
    double speedSetVal = 0;
    double EpsSetVal = 0;

    if(decition.has_gear()){
        switch (decition.gear()) {
        case 1:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
            gearSetVal=GearPrkgAssistReq::kTargetGearP;
            break;
        case 2:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
            gearSetVal=GearPrkgAssistReq::kTargetGearR;
            break;
        case 3:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
            gearSetVal=GearPrkgAssistReq::kTargetGearD;
            break;
        default:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
            gearSetVal=GearPrkgAssistReq::kNoRequest;;
            break;
        }
        lastgearSetVal=gearSetVal;
    }
    else
    {
        gearSetVal=lastgearSetVal;
    }

    if(decition.has_accelerator()){
        speedSetVal=decition.accelerator();
        lastspeedSetVal=speedSetVal;
        //speedSetVal=0.1;
       // car_control_module.set_target_acc_mps2(decition.accelerator());
//        car_control_module.set_target_acc_mps2(0.1);
    }
    else
    {
        speedSetVal=lastspeedSetVal;
    }

    if(decition.has_wheelangle())
    {
       // car_control_module.set_target_pinion_ag_in_deg(0.0);
        EpsSetVal=decition.wheelangle();
        lastEpsSetVal=EpsSetVal;
       // EpsSetVal=0.0;//
  // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
    }
    else
    {
        EpsSetVal=lastEpsSetVal;
    }
    // gearSetVal=GearPrkgAssistReq::kTargetGearD;
    car_control_module.set_target_gear(gearSetVal);
    car_control_module.set_target_acc_mps2(speedSetVal);
    car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);

    if(decition.has_leftlamp() && decition.leftlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
    else if(decition.has_rightlamp() && decition.rightlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
    else
        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);

    /*
    void set_lat_lgt_ctrl_req(bool req);  // 握手请求, true:请求握手， false:退出握手
    void set_target_gear(GearPrkgAssistReq tar);  // 目标档位请求
    bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
    void set_target_pinion_ag_in_deg(float32_t ang_req);// 设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负。
    void set_target_acc_mps2(float32_t tar_acc);// 目标加减速度值。
    void set_turn_light_status(TurnLightIndicReq req);// 转向灯控制请求
*/
}




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

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

    if(xdecition.gear() != 3)
    {
        qDebug("not D");
    }
    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
        // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
        oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
    std::cout<<"update decision. "<<std::endl;
}


void sendthread()
{
    iv::brain::decition xdecition;
//    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
//    car_control_module.sm_task_20ms();  // 线控状态机函数

    while(1)
    {
        if(gnDecitionNum <= 0)
        {
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            gMutex.lock();
            xdecition.CopyFrom(gdecition);
            gMutex.unlock();
            gnDecitionNum--;
        }

        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();

        if(bstatus == true)
        {
            //            std::cout<<"di pan ke kong "<<std::endl;
            executeDecition(xdecition);
        }
        else
        {
            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
            car_control_module.set_lat_lgt_ctrl_req(true);
        }

//        car_control_module.sm_task_20ms();  // 线控状态机函数
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}

void * gpa;

void recvthread()
{
    iv::chassis xchassis;
    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
    while(1)
    {
        gstatus = car_control_module.is_lat_lgt_ctrl_active();
        ggearSetVal = car_control_module.get_setted_tar_gear();
        ggearRealVal = car_control_module.get_cur_disp_gear();
        gchassErr = car_control_module.get_chassis_err_state();
        gstsMach = car_control_module.get_chassis_statemachine_sts();
        gspeed = car_control_module.get_current_vehicle_spd_in_ms();
        gsteerDeg = car_control_module.get_current_steer_ang_in_deg();

        std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;


        xchassis.set_angle_feedback(gsteerDeg);

        ShareChassis(gpa,&xchassis);
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        /*
            bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
            ChassisErrCode get_chassis_err_state();// 底盘错误状态码
            StsMach get_chassis_statemachine_sts(); // 内部状态机运行状态
            float32_t get_current_steer_ang_in_deg();// 当前方向盘实际角度
            float32_t get_current_vehicle_spd_in_ms();// 当前车辆实际车速，单位ｍ／ｓ
            GearPrkgAssistReq get_setted_tar_gear(); // 获取当前设定目标档位值
            GearLevelIndicate get_cur_disp_gear(); // 当前实际显示档位状态
        */
    }
}

int main(int argc, char *argv[])
{
    auto ret = google_glog_config("/home/nvidia/log1/.");
    RegisterIVBackTrace();
    showversion("controller_Geely_xingyueL");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

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

    //    car_control_module.set_lat_lgt_ctrl_req(true);
    //    car_control_module.sm_task_20ms();  // 线控状态机函数

    //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);

    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    std::string gstrmemdecition = xp.GetParam("dection","deciton");
    std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");

    gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);

    std::thread xthread(sendthread);
//    std::thread myxthread(recvthread);

    return a.exec();
}

#endif