#include "ADCIntelligentVehicle.h" #include "chassis.pb.h" #include "xmlparam.h" #define qtcout qDebug() << "[ " << __FILE__ << ":" << __LINE__<< " ]"; extern std::string gstrmemgps; extern std::string gstrvin; extern std::string gstrLat; extern std::string gstrLon; /** 常量 值 描述 QHostAddress::Null 0 空地址对象，相当于QHostAddress()。 QHostAddress::LocalHost 2 IPv4本地主机地址，相当于QHostAddress(“127.0.0.1”)。 QHostAddress::LocalHostIPv6 3 IPv6本地主机地址，相当于 QHostAddress(“::1”)。 QHostAddress::Broadcast 1 Pv4广播地址，相当于QHostAddress(“255.255.255.255”)。 QHostAddress::AnyIPv4 6 IPv4 any-address，相当于QHostAddress(“0.0.0.0”)。与该地址绑定的socket将只监听IPv4接口。 QHostAddress::AnyIPv6 5 IPv6 any-address，相当于QHostAddress(“::”)。与该地址绑定的socket将只监听IPv4接口。 QHostAddress::Any 4 双any-address栈，与该地址绑定的socket将侦听IPv4和IPv6接口。 */ #define VIEW_CENTER viewport()->rect().center() const double PI = 3.1415926535898; extern iv::Ivlog * gIvlog; /** * @brief MyView::MyView * @param parent */ MyView::MyView(QWidget *parent) : QGraphicsView(parent), beishu(1.00000) { setDragMode(QGraphicsView::ScrollHandDrag); //设置视图的拖拽模式 (光标变为指向手，然后拖动鼠标将滚动滚动条，此模式在交互和非交互模式下均有效) } /** * @brief MyView::mousePressEvent * @param event */ void MyView::mousePressEvent(QMouseEvent *event) { bottonstatus = true; QGraphicsView::mousePressEvent(event); } /** * @brief MyView::mouseMoveEvent * @param event */ void MyView::mouseMoveEvent(QMouseEvent *event) { QGraphicsView::mouseMoveEvent(event); } void MyView::keyPressEvent(QKeyEvent *event) { //按键按下，key值放入容器，如果是长按触发的repeat就不判断 if(!event->isAutoRepeat()) mPressKeys.insert(event->key()); if(event->key() == Qt::Key_J) { emit CtrlManual(true); } if(event->key() == Qt::Key_K) { emit CtrlManual(false); } // qDebug("key count is %d",mPressKeys.count()); // QDateTime dt; // qDebug("key press %ld",QDateTime::currentMSecsSinceEpoch()); // qDebug(" key is %d",event->key()); } void MyView::keyReleaseEvent(QKeyEvent *event) { if(!event->isAutoRepeat())mPressKeys.remove(event->key()); qDebug("key count is %d",mPressKeys.count()); // QDateTime dt; // qDebug("key release %ld",QDateTime::currentMSecsSinceEpoch()); } /** * @brief MyView::mouseReleaseEvent * @param event */ void MyView::mouseReleaseEvent(QMouseEvent *event) { bottonstatus = false; QGraphicsView::mouseReleaseEvent(event); } ADCIntelligentVehicle * gAV; //void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) //{ // gAV->UpdateMap(strdata,nSize); //} // void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // { // gAV->UpdateGPSIMU(strdata,nSize); // } // void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // { // gAV->UpdateRADAR(strdata,nSize); // } /** * @brief ListenCANState * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ListenCANState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { // gIvlog->debug("ListenCanState"); gAV->UpdateCANState(strdata,nSize); } //void ListenDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) //{ // gAV->UpdateDecition(strdata,nSize); //} //void ListenVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) //{ // gAV->UpdateVehicleState(strdata,nSize); //} /** * @brief ListenOBS * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { // qDebug("size is %d",nSize); std::shared_ptr> lidar_obs(new std::vector); iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata; int nCount = nSize/sizeof(iv::ObstacleBasic); int i; for(i=0;ipush_back(temp); pdata++; } gAV->UpdateOBS(lidar_obs); // gw->UpdateOBS(lidar_obs); } //v2x使能状态请求 void ListenV2xStReq(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { // gIvlog->info("hmi", "v2x enable request %d %d",nSize,sizeof(iv::v2x::v2xStReq)); // if(nSize < sizeof(iv::v2x::v2xStReq)) // return; iv::v2x::v2xStReq xv2xStReqMsg; if(!xv2xStReqMsg.ParseFromArray(strdata,nSize)) { gIvlog->error("hmi", "iv::v2x::xv2xStReqMsg::ListenV2xStReq parse error"); return; } if(xv2xStReqMsg.v2xstreq()) { gAV->UpdateV2xStEn(gAV->mv2xStEn); gIvlog->info("hmi", "v2x enable request %d",gAV->mv2xStEn); } } //vbu数据处理 void ListenObu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { iv::v2r::v2r_send xv2rSendMsg; if(!xv2rSendMsg.ParseFromArray(strdata,nSize)) { gIvlog->error("hmi", "iv::v2r::xv2rSendMsg::ListenObu parse error"); return; } gAV->updateV2R(xv2rSendMsg); } /** * @brief ADCIntelligentVehicle::ADCIntelligentVehicle * @param parent */ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent) : QMainWindow(parent), ui(new Ui::ADCIntelligentVehicle), m_translateButton(Qt::LeftButton), m_translateSpeed(1.0), m_zoomDelta(0.1), m_bMouseTranslate(false), m_scale(1.0), horizontalOffset(0), verticalOffset(0), scaleFactor(1), currentStepScaleFactor(1) { ui->setupUi(this); gAV = this; gIvlog->info("ADCIntelligentVehicle Start Initialize"); mTime.start(); /* QString strpath = QCoreApplication::applicationDirPath(); strpath = strpath + "/ADS_decision.xml"; iv::xmlparam::Xmlparam xp(strpath.toStdString()); ServiceCarStatus.mvehtype= xp.GetParam("vehType","ge3"); if(ServiceCarStatus.mvehtype=="ge3"){ ; }else if( ServiceCarStatus.mvehtype=="qingyuan"){ ; }else if( ServiceCarStatus.mvehtype=="vv7"){ ; }else if( ServiceCarStatus.mvehtype=="zhongche"){ ; } */ QString strpath = QCoreApplication::applicationDirPath(); strpath = strpath + "/ui_ads_hmi.xml"; iv::xmlparam::Xmlparam xp(strpath.toStdString()); ServiceCarStatus.esr_y_offset = atof(xp.GetParam("esr_y_offset","2.5").data()); ServiceCarStatus.lidar_y_offset = atof(xp.GetParam("lidar_y_offset","0").data()); ui->stackedWidget->setCurrentIndex(0); // /********************************** 百度地图显示 ********************************/ // mMapview = new QWebEngineView(ui->stackedWidget->widget(0)); //// qDebug((QDir::currentPath()).toLatin1().data()); // mMapview->load(QUrl(QString("file:///%1/%2").arg(QApplication::applicationDirPath()).arg("BaiDuMap.html"))); // mMapview->setGeometry(750,250,400,400); // /********************************************************************************/ // ui->listWidget->setCurrentRow(1); //apollo_fu debug 20200409 myview = new MyView(ui->stackedWidget->widget(0)); myview->setObjectName(QStringLiteral("graphicsView")); myview->setGeometry(QRect(0, 100, 900, 750)); //从屏幕上（0，100）位置开始（即为最左上角的点），显示一个900*900的界面（宽900，高900） #if 0 myview_small = new MyView(ui->stackedWidget->widget(1)); myview_small->setObjectName(QStringLiteral("graphicsView_small")); myview_small->setGeometry(QRect(1000,100,500,500)); myview_small->setAlignment(Qt::AlignLeft | Qt::AlignTop); #endif // myview_small->scale(0.7,0.7); //myview_small->centerOn(-200,-200); image = new QImage(900 , 750 , QImage::Format_RGB32);//画布的初始化大小设为600*500，使用32位颜色 //QImage的32、24、8位图。 图像格式:QImage::Format_RGB32 ,QImage::Format_RGB888,QImage::Format_Indexed8。 //QImage myImage2 = QImage(width, height, QImage::Format_…); 根据图像宽高来构造一幅图像，程序会自动根据图像格式对齐图像数据。 // image_small = new QImage(,1500,QImage::Format_RGB32); myview->setCacheMode(myview->CacheBackground); #if 0 myview_small->setCacheMode(myview_small->CacheBackground); #endif painter = new QPainter(image); //创建QPainter对象 // painter_small = new QPainter(image_small); //创建QPainter对象 scene = new QGraphicsScene; scene_small = new QGraphicsScene; timer = new QTimer(this); connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot()));//timeoutslot()为自定义槽 timer->start(100); Lidar_obsread = boost::shared_ptr>(new std::vector); Lidar_read = boost::shared_ptr>(new std::vector); mDataToUI.mHead.type = 1; mDataToUI.mInfo.accelerate = 0; mDataToUI.mInfo.brake = 0; mDataToUI.mInfo.gps_lat = 0; mDataToUI.mInfo.gps_lng = 0; mDataToUI.mInfo.is_mapLoad = 0; mDataToUI.mInfo.is_initSuccess = 0; mDataToUI.mInfo.is_run = 0; mDataToUI.mInfo.speed = 0; msockrecv.bind(QHostAddress::Any,9998);//绑定端口 connect(&msockrecv,SIGNAL(readyRead()),this,SLOT(onRecvUDP())); //接收来自PAD的UDP数据报 readyRead():socket读取缓冲区有数据时发送此信号，在此信号的槽函数里读取缓冲区的数据。 ServiceCarStatus.speed = 0.0; mTimerState.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0，含义：精确定时器试图保持毫秒精度 connect(&mTimerState,SIGNAL(timeout()),this,SLOT(onStateTimer()));//广播发送UDP数据报 mTimerState.start(100); //定时100ms mTimerStateMap.setTimerType(Qt::PreciseTimer); // connect(&mTimerStateMap,SIGNAL(timeout()),this,SLOT(onStateTimerMap()));// mTimerStateMap.start(500); //定时500ms is_show_enable = true; #ifdef USE_PAD_CTRL AutoStart(); #endif ModuleFun funplantrace = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpaplantrace = iv::modulecomm::RegisterRecvPlus("plantrace",funplantrace); ModuleFun funplantrace_left = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_left,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpaplantrace_left = iv::modulecomm::RegisterRecvPlus("obstraceleft",funplantrace_left); ModuleFun funplantrace_right = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_right,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpaplantrace_right = iv::modulecomm::RegisterRecvPlus("obstraceright",funplantrace_right); ModuleFun funfusion = std::bind(&ADCIntelligentVehicle::UpdateFusion,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpafusion = iv::modulecomm::RegisterRecvPlus("li_ra_fusion",funfusion); ModuleFun funmap = std::bind(&ADCIntelligentVehicle::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpa = iv::modulecomm::RegisterRecvPlus("tracemap",funmap); // mpa = iv::modulecomm::RegisterRecv("tracemap",ListenTraceMap); ModuleFun fungpsimu =std::bind(&ADCIntelligentVehicle::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpagpsimu = iv::modulecomm::RegisterRecvPlus(gstrmemgps.data(),fungpsimu); // mpagpsimu = iv::modulecomm::RegisterRecv("gpsimu",Listengpsimu); ModuleFun funradar =std::bind(&ADCIntelligentVehicle::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mparadar = iv::modulecomm::RegisterRecvPlus("radar",funradar); ModuleFun funchassis =std::bind(&ADCIntelligentVehicle::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis); mpacanstate = iv::modulecomm::RegisterRecv("canstate",ListenCANState); gIvlog->debug("canState ptr: %x",mpacanstate); ModuleFun funbrainstate =std::bind(&ADCIntelligentVehicle::UpdateVehicleState,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); // mpaVechicleState = iv::modulecomm::RegisterRecvPlus("vehiclestate",funbrainstate); mpaVechicleState = iv::modulecomm::RegisterRecvPlus("brainstate",funbrainstate); ModuleFun fundecition =std::bind(&ADCIntelligentVehicle::UpdateDecition,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition); mpaLidar = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS); mpaObu = iv::modulecomm::RegisterRecv("v2r_send",ListenObu); mpaHMI = iv::modulecomm::RegisterSend("hmi",10*sizeof(iv::hmi::hmimsg),10); mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);//地图站点gps信息 iv::modulecomm::RegisterRecv("v2xStReq", ListenV2xStReq); mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStEn",1000,1); mTimeState.start(); mpManualCtrl = iv::modulecomm::RegisterSend("ivmanual",1000,1); mTimerManual = new QTimer(this); connect(mTimerManual,SIGNAL(timeout()),this,SLOT(onTimerManual())); connect(myview,SIGNAL(CtrlManual(bool)),this,SLOT(onCtrlManual(bool))); mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0; // timermanualctrl->start(10); //connect(ui->lineEdit_6, SIGNAL(textEdited(const QString &)), this, SLOT(savestabuyEditinfo(const QString &))); mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1); shareMapReqMsg(); ModuleFun funvbox = std::bind(&ADCIntelligentVehicle::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpaplantrace = iv::modulecomm::RegisterRecvPlus("vbox",funvbox); #if 1 //红绿灯 ui->lcdTurn->setDigitCount(3); //设置显示几个数字 ui->lcdTurn->setMode(QLCDNumber::Dec); ui->lcdTurn->setSegmentStyle(QLCDNumber::Flat);//设置数字字体：Outline,轮廓突出，颜色为背景色；Filled，内部填充型，颜色为黑色；Flat,平面型 // 设置数字颜色时要注意： 函数setSegmentStyle(QLCDNumber::Flat)中选择Flat属性，否则在setColor()设置数字颜色时不生效 // 设置背景色直接用setStyleSheet()函数设定就可以了。 //调色板 QPalette lcdpat = ui->lcdTurn->palette(); /*设置颜色，整体背景颜色 颜色蓝色,此函数的第一个参数可以设置多种。如文本、按钮按钮文字、多种*/ lcdpat.setColor(QPalette::Normal,QPalette::WindowText,Qt::red); //设置当前窗口的调色板 ui->lcdTurn->setPalette(lcdpat); //设置背景色 //ui->lcdNumber->setStyleSheet("background-color: black"); //ui->lcdNumber->display(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss")); ui->lcdTurn->display(0); ui->lcdLeft->setDigitCount(3); ui->lcdLeft->setMode(QLCDNumber::Dec); ui->lcdLeft->setSegmentStyle(QLCDNumber::Flat); ui->lcdLeft->setPalette(lcdpat); //ui->lcdLeft->setStyleSheet("background-color: black"); ui->lcdLeft->display(0); ui->lcdStraight->setDigitCount(3); ui->lcdStraight->setMode(QLCDNumber::Dec); ui->lcdStraight->setSegmentStyle(QLCDNumber::Flat); ui->lcdStraight->setPalette(lcdpat); //ui->lcdStraight->setStyleSheet("background-color: black"); ui->lcdStraight->display(0); ui->lcdRight->setDigitCount(3); ui->lcdRight->setMode(QLCDNumber::Dec); ui->lcdRight->setSegmentStyle(QLCDNumber::Flat); ui->lcdRight->setPalette(lcdpat); //ui->lcdRight->setStyleSheet("background-color: black"); ui->lcdRight->display(0); // 新建qimage // QImage img,img2,img3,img4; // 加载图片 img.load(":/light_image/diaotou-black.png"); img = img.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_38->setPixmap(QPixmap::fromImage(img)); img2.load(":/light_image/zuoguai-black.png"); img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_33->setPixmap(QPixmap::fromImage(img2)); img3.load(":/light_image/zhixing-black.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); img4.load(":/light_image/youguai-black.png"); img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_32->setPixmap(QPixmap::fromImage(img4)); mTimerState1.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0，含义：精确定时器试图保持毫秒精度 connect(&mTimerState1,SIGNAL(timeout()),this,SLOT(onStateTimerTraffic()));//广播发送UDP数据报 mTimerState1.start(100); //定时100ms #endif ui->lineEdit_vin->setText(QString::fromStdString(gstrvin)); mdLon = QString::fromStdString(gstrLon).toDouble(); mdLat = QString::fromStdString(gstrLat).toDouble(); } /** * @brief ADCIntelligentVehicle::savestabuyEditinfo * @param txt */ void ADCIntelligentVehicle::savestabuyEditinfo(const QString &txt) { //ServiceControlStatus.set_accelerate((float)txt.toDouble()); } /** * @brief ADCIntelligentVehicle::timeoutslot */ void ADCIntelligentVehicle::timeoutslot() { update(); ///////////////////////////////// // // // 实时状态信息显示 // //////////////////////////////// QString is_ok; ui->lineEdit_RTK->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态 if(ServiceCarStatus.location->rtk_status==6) { ui->button_RTK_st->setStyleSheet("background-color: green"); } else { ui->button_RTK_st->setStyleSheet("background-color: red"); } ui->lineEdit_INS->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态 if(ServiceCarStatus.location->ins_status==4) ui->button_INS_st->setStyleSheet("background-color: green"); else ui->button_INS_st->setStyleSheet("background-color: red"); ui->lineEdit_lon->setText(QString::number(ServiceCarStatus.location->gps_lng,'f',7)); ui->lineEdit_lat->setText(QString::number(ServiceCarStatus.location->gps_lat,'f',7)); ui->lineEdit_heading->setText(QString::number(ServiceCarStatus.location->ins_heading_angle,'f',1)); is_ok = (ServiceCarStatus.mLidarS>0)?QStringLiteral("ok"):QStringLiteral("error"); ui->lineEdit_lidar->setText(is_ok);//激光雷达状态 if(ServiceCarStatus.mLidarS>0) { ui->button_lidar_st->setStyleSheet("background-color: green"); } else { ui->button_lidar_st->setStyleSheet("background-color:red"); } ui->lineEdit_dec_period->setText(QString::number(mdecition_period)+QStringLiteral("ms"));//决策周期 if(mdecition_period>0) { ui->button_decition_st->setStyleSheet("background-color: green"); } if((mTimeState.elapsed()-mnTimeUpdateGPS) > 1000) { ui->button_RTK_st->setStyleSheet("background-color: red"); ui->button_INS_st->setStyleSheet("background-color: red"); } if((mTimeState.elapsed() - mnTimeUpdateDecition)>1000) { ui->button_decition_st->setStyleSheet("background-color:red"); ui->label_decition->setText("No decition"); mbBrainRunning = false; } else { QString strdec = "Acc:"; strdec = strdec + QString::number(ServiceCarStatus.mfAcc,'f',1); // strdec = strdec + " Brake:"; // strdec = strdec + QString::number(ServiceCarStatus.mfBrake,'f',1); strdec = strdec + " EPS:"; strdec = strdec + QString::number(ServiceCarStatus.mfWheel,'f',1); ui->label_decition->setText(strdec); } if(mbManual) { char strmanual[256]; snprintf(strmanual,256,"Manual %d %d %d",mnCtrlValue[0],mnCtrlValue[1],mnCtrlValue[2]); ui->label_decition->setText(strmanual); } ///////////////////////////////// // // // 实时显示障碍物状态 // ///////////////////////////////// // ServiceCarStatus.mObs = 10; // ServiceCarStatus.mLidarObs = 9.123; // ServiceCarStatus.mRadarObs = 9.222; // ServiceCarStatus.mfttc = 0.5; // ServiceCarStatus.mfBrake = 20; if(ServiceCarStatus.mObs > 0) { ui->label_13->setText("Obs:"+QString::number(ServiceCarStatus.mObs,'f',1)+" Lidar:" +QString::number(ServiceCarStatus.mLidarObs,'f',1) + " Radar:" +QString::number(ServiceCarStatus.mRadarObs,'f',1)); } else { ui->label_13->setText(QString("前方没有障碍物")); } ui->label_26->setText("ttc is "+QString::number(ServiceCarStatus.mfttc,'f',1) + " brake is " + QString::number(ServiceCarStatus.mfBrake,'f',2)); //add 20200518 QString strstate = "Map:"; if(mDataToUI.mInfo.is_mapLoad == 1) { strstate = strstate+"Yes "; } else { strstate = strstate+"No "; } strstate = strstate+" CANCARD:"; if(mDataToUI.mInfo.is_initSuccess == 1) { strstate = strstate+"OK "; } else { strstate = strstate+"Fail "; } strstate = strstate + " Run State:"; if(mbBrainRunning) { strstate = strstate+"RUN"; } else { strstate = strstate+"STOP"; } ui->label_State->setText(strstate); ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed,'f',1)); // QString is_ok; // is_ok = (ServiceLidar.did_lidar_ok() == true)?QStringLiteral("ok"):QStringLiteral("error"); // ui->lineEdit_4->setText(is_ok); //can state mDataToUI.mInfo.is_initSuccess = 0; } /** * @brief ADCIntelligentVehicle::on_pb_load_navigation_data_clicked */ //void ADCIntelligentVehicle::on_pb_load_navigation_data_clicked() //加载地图 //{ //} void ADCIntelligentVehicle::updateV2R(iv::v2r::v2r_send xv2rMsg) { if(xv2rMsg.has_radiolighttype()) { miLightType = xv2rMsg.radiolighttype(); miLightRemain = xv2rMsg.radiolightremain(); ServiceCarStatus.st_straight = miLightType; ServiceCarStatus.time_straight = miLightRemain; miobuSt = 0; } if(xv2rMsg.has_radiobroadcastgpslat()) { miBroadcastGpsLat = xv2rMsg.radiobroadcastgpslat(); miBroadcastGpsLon = xv2rMsg.radiobroadcastgpslon(); miobuSt = 0; } if(xv2rMsg.has_radiobroadcastrange()) { miBroadcastRange = xv2rMsg.radiobroadcastrange(); miBroadcastTrafficType = xv2rMsg.radiobroadcasttraffictype(); miobuSt = 0; } if(xv2rMsg.has_radiowarningspeedlimit()) { miBroadcastSpeedLimit = xv2rMsg.radiobroadcastspeedlimit(); miobuSt = 0; } if(xv2rMsg.has_radiowarningtype()) { miWarningType = xv2rMsg.radiowarningtype(); miobuSt = 0; } if(xv2rMsg.has_radiowarningspeedlimit()) { miWarningSpeedLimit = xv2rMsg.radiowarningspeedlimit(); miobuSt = 0; } if(xv2rMsg.has_radioidentistart()) { miIdentiStart = xv2rMsg.radioidentistart(); miobuSt = 0; } } /** * @brief ADCIntelligentVehicle::onRecvUDP */ void ADCIntelligentVehicle::onRecvUDP() { while (msockrecv.hasPendingDatagrams()) //hasPendingDatagrams()表示是否有待读取的传入数据报 { qDebug("recv cmd"); QByteArray ba; ba.resize(msockrecv.pendingDatagramSize());//pendingDatagramSize()返回待读取数据报的字节数 msockrecv.readDatagram(ba.data(), ba.size()); //readDatagram()函数用于读取数据报内容 int * p; iv::hmi::HMIBasic hmi; p = (int *)ba.data(); // qDebug("cmd is ",*p); // std::cout<<"data len is "<gps_lat; mDataToUI.mInfo.gps_lng = ServiceCarStatus.location->gps_lng; mDataToUI.mInfo.accelerate = ServiceCarStatus.mfAcc; mDataToUI.mInfo.speed = ServiceCarStatus.speed; mDataToUI.mInfo.brake = ServiceCarStatus.mfBrake; mDataToUI.mInfo.swerve = ServiceCarStatus.mfWheel; mDataToUI.mInfo.boche_status = ServiceCarStatus.bocheEnable; if(fabs(mDataToUI.mInfo.gps_lat)<1 ) { // qDebug("gps lat is error."); // std::cout<<"no gps."<writeDatagram(str.toUtf8(), QHostAddress::Broadcast, 10002); // multicast, 224.0.0.1~224.0.0.255 is multicast address of LAN // _udp->writeDatagram(str.toUtf8(), QHostAddress("224.0.0.131"), 10002); } void ADCIntelligentVehicle::onStateTimerMap() { /*******************************更新百度地图******************************/ double flat = ServiceCarStatus.location->gps_lat; double flon = ServiceCarStatus.location->gps_lng; double fang = ServiceCarStatus.location->ins_heading_angle; char strscript[256]; snprintf(strscript,255,"theLocation(%11.7f,%11.7f,%11.7f);",flon,flat,fang); /************************************************************************/ } /** * @brief ADCIntelligentVehicle::AutoStart */ void ADCIntelligentVehicle::AutoStart() { // is_brain_running_flag_ = true; // mDataToUI.mInfo.is_initSuccess = 1; // std::cout<lcdStraight->display((int)ServiceCarStatus.time_straight); switch (ServiceCarStatus.st_straight) { case 0: img3.load(":/light_image/zhixing-black.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); break; case 1: img3.load(":/light_image/zhixing-green.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); break; case 2: img3.load(":/light_image/zhixing-red.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); break; case 3: img3.load(":/light_image/zhixing-yellow.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); break; default: img3.load(":/light_image/zhixing-black.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); break; } } else { img3.load(":/light_image/zhixing-black.png"); img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio); ui->label_35->setPixmap(QPixmap::fromImage(img3)); ui->lcdStraight->display(0); } if(mobuEn) { if(mbTrafficInfoEn) { if(miobuSt == 1) { QString eventType; switch(miBroadcastTrafficType){ case 1: eventType = "塌方"; break; case 2: eventType = "道路施工"; break; case 3: eventType = "道路结冰"; break; case 4: eventType = "前方限速"; break; default: eventType = ""; break; } ui->button_trafficInfoLight_st->setStyleSheet("background-color: green"); ui->textBr_trafficInfo->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \ "\n\t事件范围[ 经度:" + QString::number(miBroadcastGpsLon,'g',6) +\ " 纬度:" + QString::number(miBroadcastGpsLat,'g',6) + \ " ]\n\t辐射范围: " + QString::number(miBroadcastRange) + "米" + \ "\n\t事件类型: " + eventType); switch(micarSt){ case 0: ui->lineEd_trafficInfo->setText("正常行驶"); break; case 1: ui->lineEd_trafficInfo->setText("减速至 " + QString::number(micarSpeedDis,'g',1) + "km/h"); break; case 2: ui->lineEd_trafficInfo->setText("停车"); break; default: break; } } else if(miobuSt == 10) { ui->button_trafficInfoLight_st->setStyleSheet("background-color: red"); ui->textBr_trafficInfo->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失"); ui->lineEd_trafficInfo->clear(); miobuSt++; } } else if(mbFCWEn)//碰撞预警 { if(miobuSt == 1) { switch(miWarningType){ case 1: ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \ "前方碰撞预警，减速至:" + QString::number(miWarningSpeedLimit)); break; case 2: ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \ "前方碰撞预警，停车"); break; default: break; } ui->button_FWCLight_st->setStyleSheet("background-color: green"); } else if(miobuSt == 10) { ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失"); ui->button_FWCLight_st->setStyleSheet("background-color: red"); miobuSt++; } } else if(mbjamsEn)//交通拥堵 { if(miobuSt == 1) { ui->button_jamsLight_st->setStyleSheet("background-color: green"); ui->lineEdit_jamsMode->setText("开启"); ui->lineEdit_jamsVIN->setText(QString::fromStdString(gstrvin)); } else if(miobuSt == 10) { ui->button_jamsLight_st->setStyleSheet("background-color: red"); ui->lineEdit_jamsMode->clear(); ui->lineEdit_jamsVIN->clear(); miobuSt++; } } else if(mbdriCrimsEn)//危险驾驶 { if(miobuSt == 1) { ui->button_DriCrimsLight_st->setStyleSheet("background-color: green"); ui->textBr_FCW_2->append(QTime::currentTime().toString("hh:mm:ss.zzz")+"进入危险驾驶模式"); } else if(miobuSt == 10) { ui->button_DriCrimsLight_st->setStyleSheet("background-color: red"); ui->textBr_FCW_2->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失"); miobuSt++; } } } } /** * @brief ADCIntelligentVehicle::on_pb_open_can_card_clicked */ //void ADCIntelligentVehicle::on_pb_open_can_card_clicked() { //} /** * @brief ADCIntelligentVehicle::on_timer_car_status_time_out */ //void ADCIntelligentVehicle::on_timer_car_status_time_out() { // ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed)); // //ui->sb_accelerate_percent->setValue(ServiceControlStatus.accelerate_percent); // //ui->sb_wheel_percent->setValue(ServiceControlStatus.wheel_angle); //} /** * @brief ADCIntelligentVehicle::keyPressEvent 加/减键进行缩放 * @param event */ void ADCIntelligentVehicle::keyPressEvent(QKeyEvent *event) { switch (event->key()) { case Qt::Key_Plus: // 放大 myview->zoomIn(); break; case Qt::Key_Minus: // 缩小 myview->zoomOut(); break; default: QMainWindow::keyPressEvent(event); } } /** * @brief ADCIntelligentVehicle::closeEvent * @param event */ void ADCIntelligentVehicle::closeEvent(QCloseEvent *event) { QMessageBox::StandardButton button; button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No); if(button==QMessageBox::No) { event->ignore(); // 忽略退出信号，程序继续进行 } else if(button==QMessageBox::Yes) { event->accept(); // 接受退出信号，程序退出 } } /** * @brief MyView::wheelEvent 放大/缩小 * @param event */ void MyView::wheelEvent(QWheelEvent *event) { // 滚轮的滚动量 // QPoint scrollAmount = event->angleDelta(); // // 正值表示滚轮远离使用者（放大），负值表示朝向使用者（缩小） // scrollAmount.y() > 0 ? zoomIn() : zoomOut(); } /** * @brief MyView::zoomIn 放大 */ void MyView::zoomIn() { scale(1.1, 1.1); beishu *= 1.1; centerOn(450, 700 - (200 / beishu)); } /** * @brief MyView::zoomOut 缩小 */ void MyView::zoomOut() { scale(1 / 1.1, 1 / 1.1); beishu /= 1.1; centerOn(450, 700 - (200 / beishu)); } /** * @brief ADCIntelligentVehicle::paintEvent 刷新 */ void ADCIntelligentVehicle::paintEvent(QPaintEvent *) { if (is_show_enable) { // qDebug("enter paint. time1 is %d",mTime.elapsed()); if(fabs(mTime.elapsed() - mnTimeLastUpdatePaint)<100) { return; } mnTimeLastUpdatePaint = mTime.elapsed(); // qDebug("enter paint. time is %d",mTime.elapsed()); painter->begin(image); // painter_small->begin(image_small); // image->fill(QColor(60, 60, 60));//对画布进行填充 image->fill(QColor(220, 220, 220));//对画布进行填充 // image_small->fill(QColor(220,220,220)); std::vector navigation_data; mMutexNavi.lock(); navigation_data = m_navigation_data; mMutexNavi.unlock(); std::vector myplan,myplan_left,myplan_right; mMutexPlan.lock(); myplan = m_plan; mMutexPlan.unlock(); mMutexPlan_left.lock(); myplan_left = m_plan_left; mMutexPlan_left.unlock(); mMutexPlan_right.lock(); myplan_right = m_plan_right; mMutexPlan_right.unlock(); // std::cout<<"plan size is "< navigation_data = brain->navigation_data; painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点 // painter_small->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点 int pointx = 450, pointy = 700;//确定坐标轴起点坐标，这里定义(35,280) // int pointx_small = 450, pointy_small = 700; // double x0[22000], y0[22000], lng[22000], x1[22000], y1[22000], x2[22000], y2[22000]; double * x0, * y0, * lng, * x1, * y1, * x2, * y2; std::shared_ptr ptrx0,ptry0,ptrlng,ptrx1,ptry1,ptrx2,ptry2; int nmapsize = navigation_data.size(); x0 = new double[nmapsize]; y0 = new double[nmapsize]; lng = new double[nmapsize]; x1 = new double[nmapsize]; y1 = new double[nmapsize]; x2 = new double[nmapsize]; y2 = new double[nmapsize]; ptrx0.reset(x0); ptry0.reset(y0); ptrlng.reset(lng); ptrx1.reset(x1); ptry1.reset(y1); ptrx2.reset(x2); ptry2.reset(y2); double xx = 0, yy = 0; double sumx = 0, sumy = 0;//, ave_x = 0, ave_y = 0; int sizeN = navigation_data.size(); //int max_x_pos = 0, min_x_pos = 0, max_y_pos = 0, min_y_pos = 0; int x_max = 0, y_max = 0;//数组里的最大值 int x_min = 0x3f3f3f3f, y_min = 0x3f3f3f3f;//inf为 #define inf 0x3f3f3f3f double k1, k2; QPen pen, penPoint; //先绘制车位置点及框图 static const QPointF points1[2] = { QPointF(300, 700), QPointF(600, 700) }; static const QPointF points2[2] = { QPointF(450, 0), QPointF(450, 900) }; // static const QPointF points1_small[2] = { QPointF(300, 700), QPointF(600, 700) }; // static const QPointF points2_small[2] = { QPointF(450, 0), QPointF(450, 900) }; penPoint.setColor(Qt::red); penPoint.setWidth(2); painter->setPen(penPoint); // painter_small->setPen(penPoint); painter->drawPoint(pointx, pointy); // painter_small->drawPoint(pointx_small,pointy_small); painter->drawPolyline(points1, 2); painter->drawPolyline(points2, 2); // painter_small->drawPolyline(points1_small, 2); // painter_small->drawPolyline(points2_small, 2); //路径点的预处理 for (int i = 0; i < sizeN; i++) { x0[i] = navigation_data[i]->gps_x; y0[i] = navigation_data[i]->gps_y; lng[i] = navigation_data[i]->ins_heading_angle; sumx = sumx + navigation_data[i]->gps_x; sumy = sumy + navigation_data[i]->gps_y; if (x0[i] > x_max) { x_max = x0[i]; //max_x_pos = i; } if (x0[i] < x_min) { x_min = x0[i]; //min_x_pos = i; } if (y0[i] > y_max) { y_max = y0[i]; //max_y_pos = i; } if (y0[i] < y_min) { y_min = y0[i]; //min_y_pos = i; } } //ave_x = sumx / sizeN; //ave_y = sumy / sizeN; //获取到实时 GPS信息，并做路径点的显示更新 if (ServiceCarStatus.location->gps_x == 0) { painter->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息")); // painter_small->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息")); } else { x0[0] = ServiceCarStatus.location->gps_x; y0[0] = ServiceCarStatus.location->gps_y; // std::cout<<"x = "<drawPixmap(435,667,30,67,pix); // painter_small->drawPixmap(442,683,16,34,pix); /////////////////////////////////////////////////////////////////// penPoint.setColor(Qt::blue); penPoint.setWidth(3); painter->setPen(penPoint); QFont font; font.setFamily("Microsoft YaHei"); font.setPointSize(50); font.setItalic(true); painter->setFont(font); ////////////////////////////////////// // // // 显示毫米波雷达和激光雷达的信息 // // // ////////////////////////////////////// if(ui->checkBox_lidar_show->isChecked()) { ServiceLidar.copylidarto(Lidar_read); //激光雷达障碍物 ServiceLidar.copylidarobsto(Lidar_obsread); painter->setPen(QColor(255, 0, 0)); for (unsigned int x = 0; x < Lidar_read->size(); x++) { //painter->drawPoint(((*Lidar_read)[x].nomal_x) * 10 + 450, -(*Lidar_read)[x].nomal_y * 10 + 700); } painter->setPen(QColor(255,0,0)); for (unsigned int x = 0; x < Lidar_obsread->size(); x++) { painter->drawPoint(((*Lidar_obsread)[x].nomal_x) * 10 + 450, -((*Lidar_obsread)[x].nomal_y + ServiceCarStatus.lidar_y_offset) * 10 + 700); } } ////////////////////////////////////// // // // 显示融合的信息 // // // ////////////////////////////////////// if(ui->checkBox_fusion_show->isChecked()) { painter->setPen(QColor(255,0,0)); iv::fusion::fusionobjectarray xfusionarray; if(mbfusionUpdate) { mMutexFusion.lock(); xfusionarray.CopyFrom(mfusionarray); mMutexFusion.unlock(); for(int a = 0; a < xfusionarray.obj_size(); a++) { for(int b = 0; b < xfusionarray.obj(a).nomal_centroid_size(); b++) { painter->drawPoint((xfusionarray.obj(a).nomal_centroid(b).nomal_x())*10 + 450, -(xfusionarray.obj(a).nomal_centroid(b).nomal_y() + ServiceCarStatus.lidar_y_offset)*10 + 700); } } } } painter->end(); // painter_small->end(); scene->clear(); // scene_small->clear(); scene->addPixmap(QPixmap::fromImage(*image)); // scene_small->addPixmap(QPixmap::fromImage(*image_small)); myview->setScene(scene); // myview_small->setScene(scene_small); myview->show(); // myview_small->show(); navigation_data.clear(); } } /** * @brief ADCIntelligentVehicle::on_listWidget_clicked */ void ADCIntelligentVehicle::on_listWidget_clicked() { // int currentRow=ui->listWidget->currentRow(); //currentRow()获取当前行号 // if(currentRow==0) //点击第一切换到第一页 // { // ui->stackedWidget->setCurrentIndex(0);//为堆栈窗体切换到当前索引 // } // else if(currentRow==1) // { // ui->stackedWidget->setCurrentIndex(1); // } // else if(currentRow==2) // { // ui->stackedWidget->setCurrentIndex(2); // } // else // { // ui->stackedWidget->setCurrentIndex(0); // } } /** * @brief ADCIntelligentVehicle::UpdatePlanTrace * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { mMutexPlan.lock(); m_plan.clear(); int nplansize = nSize/sizeof(iv::TracePoint); int npsize = sizeof(iv::TracePoint); int i; for(i=0;iwarn("ADCIntelligentVehicle::UpdateFusion Parse Error."); return; } mMutexFusion.lock(); mfusionarray.CopyFrom(xfusionarray); mbfusionUpdate = true; mMutexFusion.unlock(); } /** * @brief ADCIntelligentVehicle::UpdateMap * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { // std::cout<<"update map "<gps_lat == m_navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == m_navigation_data.at(0)->ins_heading_angle)) { // qDebug("same map"); bUpdate = false; } else { bUpdate = true; } } if(bUpdate) { int i; mMutexNavi.lock(); m_navigation_data.clear(); for(i=0;i0) { mDataToUI.mInfo.is_mapLoad = 1; } else { mDataToUI.mInfo.is_mapLoad = 0; } } else { // qDebug("not need update"); } } //=======================zhaobo0904 #define PI 3.14159265358979 /** * @brief GaussProjCal * @param lon * @param lat * @param X * @param Y */ /* void GaussProjCal(double lon, double lat, double *X, double *Y) { // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2 double a = 6378140.0; double e2 = 0.006694384999588; double ep2 = e2/(1-e2); // 原点所在经度 double lon_origin = 6.0*int(lon/6) + 3.0; lon_origin *= PI / 180.0; double k0 = 0.9996; // 角度转弧度 double lat1 = lat * PI / 180.0; double lon1 = lon * PI / 180.0; // 经线在该点处的曲率半径, double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1)); // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项. // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的. // 首先计算前四项的系数 a1~a4. double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256; double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024; double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024; double a4 = (35*e2*e2*e2)/3072; double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1)); // 辅助量 double T = tan(lat1)*tan(lat1); double C = ep2*cos(lat1)*cos(lat1); double A = (lon1 - lon_origin)*cos(lat1); *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120); *Y = M + N * tan(lat1) * ((A*A)/2 + (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 + (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720); *Y *= k0; return; } */ //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres) void GaussProjCal(double longitude, double latitude, double *X, double *Y) { int ProjNo = 0; int ZoneWide; ////带宽 double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval; double a, f, e2, ee, NN, T, C, A, M, iPI; iPI = 0.0174532925199433; ////3.1415926535898/180.0; ZoneWide = 6; ////6度带宽 a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数 ////a=6378140.0; f=1/298.257; //80年西安坐标系参数 ProjNo = (int)(longitude / ZoneWide); longitude0 = ProjNo * ZoneWide + ZoneWide / 2; longitude0 = longitude0 * iPI; latitude0 = 0; longitude1 = longitude * iPI; //经度转换为弧度 latitude1 = latitude * iPI; //纬度转换为弧度 e2 = 2 * f - f * f; ee = e2 * (1.0 - e2); NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1)); T = tan(latitude1)*tan(latitude1); C = ee * cos(latitude1)*cos(latitude1); A = (longitude1 - longitude0)*cos(latitude1); M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2 *e2 / 1024)*sin(2 * latitude1) + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1)); xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120); yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720); X0 = 1000000L * (ProjNo + 1) + 500000L; Y0 = 0; xval = xval + X0; yval = yval + Y0; *X = xval; *Y = yval; } //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD) void GaussProjInvCal(double X, double Y, double *longitude, double *latitude) { int ProjNo; int ZoneWide; ////带宽 double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval; double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI; iPI = 0.0174532925199433; ////3.1415926535898/180.0; a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数 ////a=6378140.0; f=1/298.257; //80年西安坐标系参数 ZoneWide = 6; ////6度带宽 ProjNo = (int)(X / 1000000L); //查找带号 longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2; longitude0 = longitude0 * iPI; //中央经线 X0 = ProjNo * 1000000L + 500000L; Y0 = 0; xval = X - X0; yval = Y - Y0; //带内大地坐标 e2 = 2 * f - f * f; e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2)); ee = e2 / (1 - e2); M = yval; u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)); fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin( 4 * u) + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u); C = ee * cos(fai)*cos(fai); T = tan(fai)*tan(fai); NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai)); R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin (fai)*sin(fai))); D = xval / NN; //计算经度(Longitude) 纬度(Latitude) longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D *D*D*D*D / 120) / cos(fai); latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24 + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720); //转换为度 DD *longitude = longitude1 / iPI; *latitude = latitude1 / iPI; } //========================================================== /** * @brief ADCIntelligentVehicle::UpdateGPSIMU * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { iv::gps::gpsimu xgpsimu; if(!xgpsimu.ParseFromArray(strdata,nSize)) { gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize); return; } iv::GPSData data(new iv::GPS_INS); data->gps_lat = xgpsimu.lat(); data->gps_lng = xgpsimu.lon(); data->ins_heading_angle = xgpsimu.heading(); data->rtk_status = xgpsimu.rtk_state(); data->ins_status = xgpsimu.ins_state(); data->vel_D = xgpsimu.vd(); //地向速度,单位(米/秒) data->vel_E = xgpsimu.ve(); //东向速度,单位(米/秒) data->vel_N = xgpsimu.vn(); //北向速度,单位(米/秒) GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); ServiceCarStatus.mRTKStatus = data->rtk_status; ServiceCarStatus.speed = data->speed; ServiceCarStatus.location->gps_lat = data->gps_lat; ServiceCarStatus.location->gps_lng = data->gps_lng; ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle; ServiceCarStatus.location->gps_x = data->gps_x; ServiceCarStatus.location->gps_y = data->gps_y; ServiceCarStatus.location->rtk_status = data->rtk_status; ServiceCarStatus.location->ins_status = data->ins_status; ServiceCarStatus.speed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6; //double pow(double x, double y) 返回 x 的 y 次幂，即 xy。 mnTimeUpdateGPS = mTimeState.elapsed(); } /** * @brief ADCIntelligentVehicle::UpdateRADAR * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { static qint64 oldrecvtime; iv::radar::radarobjectarray xradararray; if(!xradararray.ParseFromArray(strdata,nSize)) { gIvlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error."); return; } // gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch()); if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100) { gIvlog->warn("radar interval is more than 100ms. value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime); } oldrecvtime = QDateTime::currentMSecsSinceEpoch(); mMutexRadar.lock(); mradararray.CopyFrom(xradararray); mbradarUpdate = true; mMutexRadar.unlock(); // int i; // for(i=0;i<64;i++) // { // iv::ObstacleBasic x; // memcpy(&x,pdata + i*sizeof(iv::ObstacleBasic),sizeof(iv::ObstacleBasic)); // ServiceCarStatus.obs_radar[i] = x; // } ServiceCarStatus.mRadarS = 10; // mnTimeUpdateRadar = mTimeState.elapsed(); // mnTimeUpdateCANState = mTimeState.elapsed(); } /** * @brief ADCIntelligentVehicle::UpdateCANState * @param pdata * @param ndatasize */ void ADCIntelligentVehicle::UpdateCANState(const char *pdata, const int ndatasize) { iv::canstate::canstate recCanstate; if(!recCanstate.ParseFromArray(pdata, ndatasize)) { gIvlog->warn("ADCIntelligentVehicle::updateCanstate error"); return; } // if(ndatasizedebug("UpdateCANState"); mDataToUI.mInfo.is_initSuccess = 1; } else { // gIvlog->debug("UpdateCANStatesize %d %s "); mDataToUI.mInfo.is_initSuccess = 0; } #endif // iv::can_state x; // memcpy(&x,pdata,sizeof(iv::can_state)); // if(x.mbCANOpen) // { // mDataToUI.mInfo.is_initSuccess = 1; // } // else // { // mDataToUI.mInfo.is_initSuccess = 0; // } } /** * @brief ADCIntelligentVehicle::UpdateDecition * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { iv::brain::decition xdecition; if(!xdecition.ParseFromArray(strdata,nSize)) { gIvlog->warn("ADCIntelligentVehicle::UpdateDecition parse errror. nSize is %d",nSize); return; } //ServiceCarStatus.mfAcc = xdecition.accelerator(); // ServiceCarStatus.mfAcc = xdecition.torque(); // ServiceCarStatus.mfBrake = xdecition.brake(); ServiceCarStatus.mfWheel = xdecition.wheelangle(); ServiceCarStatus.mfttc = xdecition.ttc(); mnTimeUpdateDecition = mTimeState.elapsed(); } /** * @brief ADCIntelligentVehicle::UpdateVehicleState * @param strdata * @param nSize * @param index * @param dt * @param strmemname */ void ADCIntelligentVehicle::UpdateVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { iv::brain::brainstate xbrainstate; if(!xbrainstate.ParseFromArray(strdata,nSize)) { gIvlog->warn("ADCIntelligentVehicle::UpdateVehicleState parse error,nSize is %d ",nSize); return; } if(xbrainstate.has_mbbocheenable()) ServiceCarStatus.bocheEnable = xbrainstate.mbbocheenable(); if(xbrainstate.has_mfobs()) ServiceCarStatus.mObs = xbrainstate.mfobs(); if(xbrainstate.has_mflidarobs()) ServiceCarStatus.mLidarObs = xbrainstate.mflidarobs(); if(xbrainstate.has_mfradarobs()) ServiceCarStatus.mRadarObs = xbrainstate.mfradarobs(); if(xbrainstate.has_mbbrainrunning()) mbBrainRunning = xbrainstate.mbbrainrunning(); if(xbrainstate.has_mbbrainrunning()) ServiceCarStatus.mbBrainCtring = xbrainstate.mbbrainrunning(); if(xbrainstate.has_decision_period()) mdecition_period = xbrainstate.decision_period(); if(xbrainstate.has_vehicle_state()) micarSt = xbrainstate.vehicle_state(); if(xbrainstate.has_vehicle_spd()) micarSpeedDis = xbrainstate.vehicle_spd(); mnTimeUpdateVS = mTimeState.elapsed(); } /** * @brief ADCIntelligentVehicle::UpdateOBS * @param lidar_obs */ void ADCIntelligentVehicle::UpdateOBS(std::shared_ptr > lidar_obs) { iv::ObsLiDAR _obs_grid_ = boost::shared_ptr>(new std::vector); int i; int nSize = lidar_obs->size(); for(i=0;ipush_back(lidar_obs->at(i)); ServiceLidar.copyfromlidarobs(_obs_grid_); ServiceCarStatus.mLidarS = 10; } void ADCIntelligentVehicle::UpdateV2xStEn(unsigned int enable) { iv::v2x::v2xStEn x; x.set_v2xsten(enable); int nsize = x.ByteSize(); char * str = new char[nsize]; if(x.SerializeToArray(str,nsize)) { iv::modulecomm::ModuleSendMsg(mp_v2xStSend,str,nsize); } else { gIvlog->error("hmi","send enable st error"); } delete str; } /** * @brief ADCIntelligentVehicle::ShareHMIMsg * @param xhmi */ void ADCIntelligentVehicle::ShareHMIMsg(iv::hmi::HMIBasic xhmi) { iv::hmi::hmimsg xhmimsg; xhmimsg.set_mbpause(xhmi.mbPause); xhmimsg.set_mbbusmode(xhmi.mbbusmode); xhmimsg.set_mbbochemode(xhmi.mbBocheMode); xhmimsg.set_mbchemen(xhmi.mbchemen); xhmimsg.set_mbjinguang(xhmi.mbjinguang); xhmimsg.set_mbyuanguang(xhmi.mbyuanguang); int nsize = xhmimsg.ByteSize(); char * str = new char[nsize]; std::shared_ptr pstr; pstr.reset(str); //将就对象的引用计数减1（当然，如果发现引用计数为0时，则析构旧对象），然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。 if(!xhmimsg.SerializeToArray(str,nsize)) { std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<error("ADCIntelligentVehicle::ShareHMIMsg serialize error."); return; } iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize); } /** * @brief ADCIntelligentVehicle::ShareHMIMsg * @param xhmi */ void ADCIntelligentVehicle::ShareHMIMsgPro(iv::hmi::hmimsg xhmimsg) { int nsize = xhmimsg.ByteSize(); char * str = new char[nsize]; std::shared_ptr pstr; pstr.reset(str); //将就对象的引用计数减1（当然，如果发现引用计数为0时，则析构旧对象），然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。 if(!xhmimsg.SerializeToArray(str,nsize)) { std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<error("ADCIntelligentVehicle::ShareHMIMsg serialize error."); return; } iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize); } void ADCIntelligentVehicle::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname) { iv::chassis xchassis; static int ncount = 0; if(!xchassis.ParseFromArray(strdata,nSize)) { std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<lineEdit_soc->setText(QString::number(mfSOC,'f',1)); } unsigned char errSig = 0; if((xchassis.has_driver_error())) { errSig |= xchassis.driver_error(); } if((xchassis.has_swj_error())) { errSig |= xchassis.swj_error(); } if((xchassis.has_battery_error())) { errSig |= xchassis.battery_error(); } if((xchassis.has_remote_error())) { errSig |= xchassis.remote_error(); } if((xchassis.has_steer_motor_error())) { errSig |= xchassis.steer_motor_error(); } if((xchassis.has_rrmotor_error())) { errSig |= xchassis.rrmotor_error(); } if((xchassis.has_rlmotor_error())) { errSig |= xchassis.rlmotor_error(); } if((xchassis.has_fsteer_code_error())) { errSig |= xchassis.fsteer_code_error(); } ui->lineEdit_errCode->setText(QString::number(errSig,16)); } void ADCIntelligentVehicle::onTimerManual() { // if(myview->mPressKeys.count() == 0)return; if(mbManual == false) { if(myview->mPressKeys.contains(Qt::Key_J)) { mbManual = true; } } if(mbManual == true) { if(myview->mPressKeys.contains(Qt::Key_K)) { mbManual = false; } } if(mbManual == false)return; int timediff = mManualTime.elapsed() - mnLastManualTime; mnLastManualTime = mManualTime.elapsed(); if(myview->mPressKeys.contains(Qt::Key_W)) { if(mnCtrlValue[0]<1000)mnCtrlValue[0] = mnCtrlValue[0] + 5 * timediff /20; } if(myview->mPressKeys.contains(Qt::Key_S)) { mnCtrlValue[0] = 0; if(mnCtrlValue[1]<990) mnCtrlValue[1] = mnCtrlValue[1] + 10* timediff /20; } else { mnCtrlValue[1] = 0; } if(myview->mPressKeys.contains(Qt::Key_A)) { if(mnCtrlValue[2]<500)mnCtrlValue[2] = mnCtrlValue[2] + 3* timediff /20; } if(myview->mPressKeys.contains(Qt::Key_D)) { if(mnCtrlValue[2]>-500)mnCtrlValue[2] = mnCtrlValue[2] - 3* timediff /20; } iv::modulecomm::ModuleSendMsg(mpManualCtrl,(char *)mnCtrlValue,3*sizeof(int)); } void ADCIntelligentVehicle::onCtrlManual(bool bCtrl) { mbManual = bCtrl; if(bCtrl) { mTimerManual->start(10); mManualTime.start(); mnLastManualTime = mManualTime.elapsed(); } else { mTimerManual->stop(); mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0; } } void ADCIntelligentVehicle::shareMapReqMsg() { iv::map::mapreq x; x.set_maptype(1); int nsize = x.ByteSize(); char * str = new char[nsize]; if(x.SerializeToArray(str,nsize)) { iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize); } else { std::cout<<"iv::map::mapreq serialize error."<warn("ADCIntelligentVehicle::UpdateVbox parse errror. nSize is %d",nSize); return; } ServiceCarStatus.st_straight = xvbox.st_straight(); ServiceCarStatus.st_left = xvbox.st_left(); ServiceCarStatus.st_right = xvbox.st_right(); ServiceCarStatus.st_turn = xvbox.st_turn(); ServiceCarStatus.time_straight = xvbox.time_straight(); ServiceCarStatus.time_left = xvbox.time_left(); ServiceCarStatus.time_right = xvbox.time_right(); ServiceCarStatus.time_turn = xvbox.time_turn(); } void ADCIntelligentVehicle::on_button_start_clicked() { #ifdef USE_PAD_CTRL iv::hmi::hmimsg hmi; if(mbPause){ ServiceCarStatus.mbRunPause = false; hmi.set_mbpause(false); mbPause = false; ui->button_start->setText("暂停自动驾驶"); }else{ ServiceCarStatus.mbRunPause = true; hmi.set_mbpause(true); mbPause = true; ui->button_start->setText("启动自动驾驶"); } ShareHMIMsgPro(hmi); return; #endif } //云平台使能控制 void ADCIntelligentVehicle::on_button_platform_en_clicked() { if(mplatformEn){ ui->button_platform_en->setStyleSheet("background-color: gray"); mplatformEn = 0; } else{ ui->button_platform_en->setStyleSheet("background-color: green"); mplatformEn = 1; } gIvlog->info("hmi","platform enable:%d", mplatformEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; hmi.set_platformen(mplatformEn); ShareHMIMsgPro(hmi); } //路测设备使能控制 void ADCIntelligentVehicle::on_button_obu_en_clicked() { if(mobuEn){ ui->button_obu_en->setStyleSheet("background-color: gray"); mobuEn = 0; } else{ ui->button_obu_en->setStyleSheet("background-color: green"); mobuEn = 1; } gIvlog->info("hmi","platform enable:%d", mobuEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; hmi.set_obuen(mobuEn); ShareHMIMsgPro(hmi); } void ADCIntelligentVehicle::on_button_map_set_clicked() { QString fileName = QFileDialog::getOpenFileName(this, tr("选择地图文件"), "/home/nvidia/map", tr("Text Files (*.xodr)")); if (fileName.isEmpty()) { QMessageBox msgBox(QMessageBox::Warning, tr("警告"), "No Map Selected", 0, this); msgBox.addButton(tr("OK"), QMessageBox::AcceptRole); // msgBox.addButton(tr("&Continue"), QMessageBox::RejectRole); // if (msgBox.exec() == QMessageBox::AcceptRole) // qDebug()<<"accept"; // else // qDebug()<<"reject"; }else{ //发送地图文件名到driver_map_xodrload iv::hmi::hmimsg hmi; hmi.set_mapname(fileName.toStdString()); gIvlog->info("mapLoad","send map name"); ShareHMIMsgPro(hmi); QThread::sleep(1); gIvlog->info("mapLoad","send des gps"); //发送站点gps信息 xodrobj xo; xo.flon = mdLon; xo.flat = mdLat; xo.lane = 1; iv::modulecomm::ModuleSendMsg(mpadst,(char *)&xo,sizeof(xodrobj)); } } void ADCIntelligentVehicle::on_button_vin_set_clicked() { bool ok; QString text = QInputDialog::getText(this, tr("输入车辆VIN："), tr("VIN:"), QLineEdit::Normal, Q_NULLPTR, &ok); if (ok && !text.isEmpty()) { gstrvin = text.toStdString(); iv::hmi::hmimsg hmi; hmi.set_vin(gstrvin); ShareHMIMsgPro(hmi); ui->lineEdit_vin->setText(text); } } void ADCIntelligentVehicle::on_button_speedLimit_set_clicked() { bool ok; double d = QInputDialog::getDouble(this, tr("QInputDialog::getDouble()"), tr("Amount:"), 0.0, 0, 30, 1, &ok); if (ok) { // doubleLabel->setText(QString("$%1").arg(d)); mdspeedLimit = d; iv::hmi::hmimsg hmi; hmi.set_speedlimit(mdspeedLimit); ShareHMIMsgPro(hmi); ui->lineEdit_speedLimit->setText(QString::number(mdspeedLimit,'f',1)); } } void ADCIntelligentVehicle::buttonColorReset() { ui->button_trafficInfo_en->setStyleSheet("background-color: gray"); ui->button_FCW_en->setStyleSheet("background-color: gray"); ui->button_jams_en->setStyleSheet("background-color: gray"); ui->button_DriCrims_en->setStyleSheet("background-color: gray"); } void ADCIntelligentVehicle::on_button_trafficInfo_en_clicked() { buttonColorReset(); if(mbTrafficInfoEn){ ui->button_trafficInfo_en->setStyleSheet("background-color: gray"); mbTrafficInfoEn = false; } else{ ui->button_trafficInfo_en->setStyleSheet("background-color: green"); mbTrafficInfoEn = true; } gIvlog->info("hmi","traffic info enable:%d", mbTrafficInfoEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; mbFCWEn = false; mbjamsEn = false; mbdriCrimsEn = false; hmi.set_rodeinfoen(mbTrafficInfoEn); hmi.set_rodefcwen(mbFCWEn); hmi.set_roadjamsen(mbjamsEn); hmi.set_roaddricrimsen(mbdriCrimsEn); ShareHMIMsgPro(hmi); } void ADCIntelligentVehicle::on_button_FCW_en_clicked() { buttonColorReset(); if(mbFCWEn){ ui->button_FCW_en->setStyleSheet("background-color: gray"); mbFCWEn = false; } else{ ui->button_FCW_en->setStyleSheet("background-color: green"); mbFCWEn = true; } gIvlog->info("hmi","traffic FCW enable:%d", mbFCWEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; mbTrafficInfoEn = false; mbjamsEn = false; mbdriCrimsEn = false; hmi.set_rodeinfoen(mbTrafficInfoEn); hmi.set_rodefcwen(mbFCWEn); hmi.set_roadjamsen(mbjamsEn); hmi.set_roaddricrimsen(mbdriCrimsEn); ShareHMIMsgPro(hmi); } void ADCIntelligentVehicle::on_button_jams_en_clicked() { buttonColorReset(); if(mbjamsEn){ ui->button_jams_en->setStyleSheet("background-color: gray"); mbjamsEn = false; } else{ ui->button_jams_en->setStyleSheet("background-color: green"); mbjamsEn = true; } gIvlog->info("hmi","traffic jams enable:%d", mbjamsEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; mbTrafficInfoEn = false; mbFCWEn = false; mbdriCrimsEn = false; hmi.set_rodeinfoen(mbTrafficInfoEn); hmi.set_rodefcwen(mbFCWEn); hmi.set_roadjamsen(mbjamsEn); hmi.set_roaddricrimsen(mbdriCrimsEn); ShareHMIMsgPro(hmi); } void ADCIntelligentVehicle::on_button_SimCar_en_clicked() { iv::hmi::hmimsg hmi; hmi.set_lonup(ui->lineEdit_jamsLon_up->text().toDouble()); hmi.set_lonlow(ui->lineEdit_jamsLon_low->text().toDouble()); hmi.set_latup(ui->lineEdit_jamsLat_Up->text().toDouble()); hmi.set_latlow(ui->lineEdit_jamsLat_low->text().toDouble()); hmi.set_headingup(ui->lineEdit_jamsHead_up->text().toDouble()); hmi.set_headinglow(ui->lineEdit_jamsHead_low->text().toDouble()); hmi.set_carcount(ui->lineEdit_jamsCarNum->text().toInt()); hmi.set_speedlow(ui->lineEdit_jamsSpeed_low->text().toDouble()); hmi.set_speedup(ui->lineEdit_jamsSpeed_up->text().toDouble()); ShareHMIMsgPro(hmi); } void ADCIntelligentVehicle::on_button_DriCrims_en_clicked() { buttonColorReset(); if(mbdriCrimsEn){ ui->button_DriCrims_en->setStyleSheet("background-color: gray"); mbdriCrimsEn = false; } else{ ui->button_DriCrims_en->setStyleSheet("background-color: green"); mbdriCrimsEn = true; } gIvlog->info("hmi","traffic jams enable:%d", mbdriCrimsEn); // UpdateV2xStEn(mv2xStEn); iv::hmi::hmimsg hmi; mbTrafficInfoEn = false; mbFCWEn = false; mbjamsEn = false; hmi.set_rodeinfoen(mbTrafficInfoEn); hmi.set_rodefcwen(mbFCWEn); hmi.set_roadjamsen(mbjamsEn); hmi.set_roaddricrimsen(mbdriCrimsEn); ShareHMIMsgPro(hmi); }