change ui_ads_hmi.add some code for down cpu load, but not complete.

src/driver/driver_can_nvidia_agx_new/canctrl.cpp

@@ -117,7 +117,7 @@ void canctrl::run()
             nSend1 = 0;
             nSend2 = 0;
 
-            msleep(1);
+            msleep(5);
         }
         else
         {

src/driver/driver_can_nvidia_agx_new/main.cpp

@@ -50,7 +50,7 @@ void signal_handler(int sig)
 #ifdef TEST_PROG
 void threadtest()
 {
-
+    return;
     QTimer xTimer;
     xTimer.setTimerType(Qt::PreciseTimer);
     void * pa = iv::modulecomm::RegisterSend("cansend0",100000,100);
@@ -61,7 +61,7 @@ void threadtest()
         xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
         xmsg.set_index(0);
         int i;
-        for(i=0;i<10;i++)
+        for(i=0;i<8;i++)
         {
             iv::can::canraw * pcanraw = xmsg.add_rawmsg();
             pcanraw->set_bext(false);
@@ -92,7 +92,7 @@ void threadtest2()
     while(true)
     {
         iv::can::canmsg xmsg;
-        xmsg.set_channel(0);
+        xmsg.set_channel(1);
         xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
         xmsg.set_index(0);
         int i;

src/driver/driver_can_nvidia_agx_new/nvcan.cpp

@@ -203,33 +203,6 @@ int nvcan::ExecRecv(int & s,fd_set & rdfs,int nch)
     if(bRecv == false)return 0;
     return 1;
 
-
-
-
-//    qint64 nsecondnow = QDateTime::currentSecsSinceEpoch();
-//    if(nlastsecond != nsecondnow)
-//    {
-//        nlastsecond = nsecondnow;
-//        std::cout<<"second recv count:"<<nsecondrecvcount<<std::endl;
-//        nsecondrecvcount = 0;
-//    }
-
-//    if((QDateTime::currentMSecsSinceEpoch() - nLastRecv)> 1000)
-//    {
-//        if(nRecvState == 0)
-//        {
-//            nRecvState = -1;
-//            mfault->SetFaultState(0,1,"More than 1 second not receive data.");
-//        }
-//    }
-//    else
-//    {
-//        if(nRecvState == -1)
-//        {
-//            nRecvState = 0;
-//            mfault->SetFaultState(0,0,"CAN OK.");
-//        }
-//    }
 }
 
 

src/ui/ui_ads_hmi/ADCIntelligentVehicle.cpp

@@ -3,6 +3,8 @@
 #include "chassis.pb.h"
 #include "xmlparam.h"
 
+#include <thread>
+
 #define qtcout qDebug() << "[ " << __FILE__ << ":" << __LINE__<< " ]";
 
 extern std::string gstrmemgps;
@@ -223,6 +225,7 @@ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
     gAV = this;
     gIvlog->info("ADCIntelligentVehicle Start Initialize");
     mTime.start();
+
 /*
     QString strpath = QCoreApplication::applicationDirPath();
     strpath = strpath + "/ADS_decision.xml";
@@ -282,6 +285,7 @@ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
     mMapview->setGeometry(1000,100,500,500);
     /********************************************************************************/
 
+
     image = new QImage(900 * 2, 900 * 2, 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_…); 根据图像宽高来构造一幅图像，程序会自动根据图像格式对齐图像数据。
@@ -332,6 +336,7 @@ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
     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);
 
@@ -504,8 +509,10 @@ void ADCIntelligentVehicle::savestabuyEditinfo(const QString &txt)
  */
 void ADCIntelligentVehicle::timeoutslot()
 {
+
     update();
 
+
     /////////////////////////////////
     //                             //
     //      实时状态信息显示         //
@@ -513,60 +520,103 @@ void ADCIntelligentVehicle::timeoutslot()
     QString is_ok;
     ui->lineEdit->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态
     ui->lineEdit_12->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态
-    if(ServiceCarStatus.location->rtk_status==6)
+    static int oldrtkstatus = -1;
+    if(oldrtkstatus != ServiceCarStatus.location->rtk_status)
     {
-        ui->pushButton_10->setStyleSheet("background-color: green");
-        ui->pushButton_23->setStyleSheet("background-color: green");
+        oldrtkstatus = ServiceCarStatus.location->rtk_status;
+        if(ServiceCarStatus.location->rtk_status==6)
+        {
+            ui->pushButton_10->setStyleSheet("background-color: green");
+            ui->pushButton_23->setStyleSheet("background-color: green");
+        }
+        else
+        {
+            ui->pushButton_10->setStyleSheet("background-color: red");
+            ui->pushButton_23->setStyleSheet("background-color: red");
+        }
     }
-    else
+
+    static int oldinstatus = -1;
+    if(oldinstatus != ServiceCarStatus.location->ins_status)
     {
-        ui->pushButton_10->setStyleSheet("background-color: red");
-        ui->pushButton_23->setStyleSheet("background-color: red");
-    }
+        oldinstatus == ServiceCarStatus.location->ins_status;
+        ui->lineEdit_2->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态
+        if(ServiceCarStatus.location->ins_status==4)
+            ui->pushButton_11->setStyleSheet("background-color: green");
+        else
+            ui->pushButton_11->setStyleSheet("background-color: red");
 
-    ui->lineEdit_2->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态
-    if(ServiceCarStatus.location->ins_status==4)
-        ui->pushButton_11->setStyleSheet("background-color: green");
-    else
-        ui->pushButton_11->setStyleSheet("background-color: red");
+    }
 
     is_ok = (ServiceCarStatus.mRadarS>0)?QStringLiteral("ok"):QStringLiteral("error");
     ui->lineEdit_3->setText(is_ok);//毫米波雷达状态
     //ui->lineEdit_15->setText(is_ok);//毫米波雷达状态
 
+    static int nOldRadarState = 0; //1 ok -1 not ok 0 notdefine
+
+
     if(ServiceCarStatus.mRadarS>0)
     {
-        ui->pushButton_12->setStyleSheet("background-color: green");
-        ui->pushButton_25->setStyleSheet("background-color: green");
+        if(nOldRadarState != 1)
+        {
+            nOldRadarState = 1;
+            ui->pushButton_12->setStyleSheet("background-color: green");
+            ui->pushButton_25->setStyleSheet("background-color: green");
+        }
     }
     else
     {
-        ui->pushButton_12->setStyleSheet("background-color:red");
-        ui->pushButton_25->setStyleSheet("background-color:red");
         ServiceCarStatus.mRadarS = -1;
+        if(nOldRadarState != -1)
+        {
+            nOldRadarState =  -1;
+            ui->pushButton_12->setStyleSheet("background-color:red");
+            ui->pushButton_25->setStyleSheet("background-color:red");
+        }
+
     }
 
     is_ok = (ServiceCarStatus.mLidarS>0)?QStringLiteral("ok"):QStringLiteral("error");
     ui->lineEdit_4->setText(is_ok);//激光雷达状态
    // ui->lineEdit_16->setText(is_ok);//激光雷达状态
+    int nOldLidarState = 0;
     if(ServiceCarStatus.mLidarS>0)
     {
-        ui->pushButton_13->setStyleSheet("background-color: green");
-        ui->pushButton_26->setStyleSheet("background-color: green");
+        if(nOldLidarState != 1)
+        {
+            nOldLidarState = 1;
+
+            ui->pushButton_13->setStyleSheet("background-color: green");
+            ui->pushButton_26->setStyleSheet("background-color: green");
+        }
     }
     else
     {
-        ui->pushButton_13->setStyleSheet("background-color:red");
-        ui->pushButton_26->setStyleSheet("background-color:red");
-        ServiceCarStatus.mLidarS = -1;
+        if(nOldLidarState != -1)
+        {
+            nOldLidarState = -1;
+            ui->pushButton_13->setStyleSheet("background-color:red");
+            ui->pushButton_26->setStyleSheet("background-color:red");
+            ServiceCarStatus.mLidarS = -1;
+        }
     }
+
+    int nMState = 0;
     if(mimobileyeState>0)
     {
-        ui->Mobileye_st->setStyleSheet("background-color: green");
+        if(nMState != 1)
+        {
+            nMState = 1;
+            ui->Mobileye_st->setStyleSheet("background-color: green");
+        }
     }
     else
     {
-        ui->Mobileye_st->setStyleSheet("background-color: red");
+        if(nMState != -1)
+        {
+            nMState =-1;
+            ui->Mobileye_st->setStyleSheet("background-color: red");
+        }
         mimobileyeState = -1;
     }
 
@@ -725,7 +775,7 @@ void ADCIntelligentVehicle::timeoutslot()
  */
 void ADCIntelligentVehicle::onRecvUDP()
 {
-    while (msockrecv.hasPendingDatagrams())  //hasPendingDatagrams()表示是否有待读取的传入数据报
+    if (msockrecv.hasPendingDatagrams())  //hasPendingDatagrams()表示是否有待读取的传入数据报
     {
         qDebug("recv cmd");
         QByteArray ba;
@@ -780,7 +830,6 @@ void ADCIntelligentVehicle::onRecvUDP()
  */
 void ADCIntelligentVehicle::onStateTimer()
 {
-
     if(ServiceCarStatus.mbBrainCtring)
         mDataToUI.mInfo.is_run = 1;
     else
@@ -1005,6 +1054,9 @@ void MyView::zoomOut()
  */
 void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
 {
+
+    QTime xTime;
+    xTime.start();
     if (is_show_enable)
     {
         //       qDebug("enter paint. time1 is %d",mTime.elapsed());
@@ -1244,6 +1296,7 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
                                   (float)(450+myplan_right[i+1].x*kx),(float)(700-myplan_right[i+1].y*ky));
             }
         }
+
 #endif
  //       painter->drawPolyline(tracePoints,myplan.size());
         // draw plan trace end
@@ -1252,7 +1305,7 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
         //pix.load("car.png");
         pix.load(":/ADCIntelligentVehicle/car1.png");
         painter->drawPixmap(435,667,30,67,pix);
-//        painter_small->drawPixmap(442,683,16,34,pix);
+        painter_small->drawPixmap(442,683,16,34,pix);
 ///////////////////////////////////////////////////////////////////
 
 
@@ -1345,6 +1398,7 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
 
 
 
+
         painter->end();
 //        painter_small->end();
         scene->clear();
@@ -1364,6 +1418,8 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
 
 
     }
+
+//    qDebug("painter use %d ms",xTime.elapsed());
 }
 
 /**
@@ -1671,6 +1727,7 @@ void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nS
             *data = x;
             m_navigation_data.push_back(data);
         }
+        mnNaviUpdateTime = QDateTime::currentMSecsSinceEpoch();
         mMutexNavi.unlock();
 
         if(m_navigation_data.size()>0)
@@ -2239,113 +2296,134 @@ void ADCIntelligentVehicle::onStateTimer1()
     ui->lcdNumber_2->display((int)ServiceCarStatus.time_left);
     ui->lcdNumber_3->display((int)ServiceCarStatus.time_straight);
     ui->lcdNumber_4->display((int)ServiceCarStatus.time_right);
-    switch (ServiceCarStatus.st_turn) {
-    case 0:
-        img.load(":/light_image/diaotou-black.png");
-        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_38->setPixmap(QPixmap::fromImage(img));
-        break;
-    case 1:
-        img.load(":/light_image/diaotou-green.png");
-        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_38->setPixmap(QPixmap::fromImage(img));
-        break;
-    case 2:
-        img.load(":/light_image/diaotou-red.png");
-        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_38->setPixmap(QPixmap::fromImage(img));
-        break;
-    case 3:
-        img.load(":/light_image/diaotou-yellow.png");
-        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_38->setPixmap(QPixmap::fromImage(img));
-        break;
-    default:
-        img.load(":/light_image/diaotou-black.png");
-        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_38->setPixmap(QPixmap::fromImage(img));
-        break;
+    static uint32_t  xturn = -1;
+    static uint32_t  xstraiht = -1;
+    static uint32_t  xright = -1;
+    static uint32_t  xleft = -1;
+    if(xturn != ServiceCarStatus.st_turn)
+    {
+        xturn = ServiceCarStatus.st_turn;
+        switch (ServiceCarStatus.st_turn) {
+        case 0:
+            img.load(":/light_image/diaotou-black.png");
+            img = img.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_38->setPixmap(QPixmap::fromImage(img));
+            break;
+        case 1:
+            img.load(":/light_image/diaotou-green.png");
+            img = img.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_38->setPixmap(QPixmap::fromImage(img));
+            break;
+        case 2:
+            img.load(":/light_image/diaotou-red.png");
+            img = img.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_38->setPixmap(QPixmap::fromImage(img));
+            break;
+        case 3:
+            img.load(":/light_image/diaotou-yellow.png");
+            img = img.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_38->setPixmap(QPixmap::fromImage(img));
+            break;
+        default:
+            img.load(":/light_image/diaotou-black.png");
+            img = img.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_38->setPixmap(QPixmap::fromImage(img));
+            break;
+        }
     }
-    switch (ServiceCarStatus.st_left) {
-    case 0:
-        img2.load(":/light_image/zuoguai-black.png");
-        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_33->setPixmap(QPixmap::fromImage(img2));
-        break;
-    case 1:
-        img2.load(":/light_image/zuoguai-green.png");
-        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_33->setPixmap(QPixmap::fromImage(img2));
-        break;
-    case 2:
-        img2.load(":/light_image/zuoguai-red.png");
-        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_33->setPixmap(QPixmap::fromImage(img2));
-        break;
-    case 3:
-        img2.load(":/light_image/zuoguai-yellow.png");
-        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_33->setPixmap(QPixmap::fromImage(img2));
-        break;
-    default:
-        img2.load(":/light_image/zuoguai-black.png");
-        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_33->setPixmap(QPixmap::fromImage(img2));
-        break;
+    if(xleft != ServiceCarStatus.st_left)
+    {
+        xleft = ServiceCarStatus.st_left;
+        switch (ServiceCarStatus.st_left) {
+        case 0:
+            img2.load(":/light_image/zuoguai-black.png");
+            img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_33->setPixmap(QPixmap::fromImage(img2));
+            break;
+        case 1:
+            img2.load(":/light_image/zuoguai-green.png");
+            img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_33->setPixmap(QPixmap::fromImage(img2));
+            break;
+        case 2:
+            img2.load(":/light_image/zuoguai-red.png");
+            img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_33->setPixmap(QPixmap::fromImage(img2));
+            break;
+        case 3:
+            img2.load(":/light_image/zuoguai-yellow.png");
+            img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_33->setPixmap(QPixmap::fromImage(img2));
+            break;
+        default:
+            img2.load(":/light_image/zuoguai-black.png");
+            img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_33->setPixmap(QPixmap::fromImage(img2));
+            break;
+        }
     }
-    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;
+    if(xstraiht != ServiceCarStatus.st_straight)
+    {
+        xstraiht = ServiceCarStatus.st_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;
+        }
     }
-    switch (ServiceCarStatus.st_right) {
-    case 0:
-        img4.load(":/light_image/youguai-black.png");
-        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_32->setPixmap(QPixmap::fromImage(img4));
-        break;
-    case 1:
-        img4.load(":/light_image/youguai-green.png");
-        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_32->setPixmap(QPixmap::fromImage(img4));
-        break;
-    case 2:
-        img4.load(":/light_image/youguai-red.png");
-        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_32->setPixmap(QPixmap::fromImage(img4));
-        break;
-    case 3:
-        img4.load(":/light_image/youguai-yellow.png");
-        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_32->setPixmap(QPixmap::fromImage(img4));
-        break;
-    default:
-        img4.load(":/light_image/youguai-black.png");
-        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
-        ui->label_32->setPixmap(QPixmap::fromImage(img4));
-        break;
+    if(xright != ServiceCarStatus.st_right)
+    {
+        xright = ServiceCarStatus.st_right;
+
+        switch (ServiceCarStatus.st_right) {
+        case 0:
+            img4.load(":/light_image/youguai-black.png");
+            img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_32->setPixmap(QPixmap::fromImage(img4));
+            break;
+        case 1:
+            img4.load(":/light_image/youguai-green.png");
+            img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_32->setPixmap(QPixmap::fromImage(img4));
+            break;
+        case 2:
+            img4.load(":/light_image/youguai-red.png");
+            img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_32->setPixmap(QPixmap::fromImage(img4));
+            break;
+        case 3:
+            img4.load(":/light_image/youguai-yellow.png");
+            img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_32->setPixmap(QPixmap::fromImage(img4));
+            break;
+        default:
+            img4.load(":/light_image/youguai-black.png");
+            img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
+            ui->label_32->setPixmap(QPixmap::fromImage(img4));
+            break;
+        }
     }
 }
 
@@ -2391,3 +2469,174 @@ void ADCIntelligentVehicle::on_pBtn_high_beam_close_clicked()
     hmi.mbyuanguang = false;
     ShareHMIMsg(hmi);
 }
+
+void ADCIntelligentVehicle::threadCalcDrawTrace()
+{
+    bool mbRun = true;
+    qint64 nMapUpdateTime = 0;
+    std::vector<iv::MapPointDomain> xvectorMapDomain;
+    xvectorMapDomain.clear();
+
+    std::vector<iv::GPSData> xvectoroptmap;
+
+    double foldx = 0;
+    double foldy = 0;
+    double foldhdg = 0;
+
+    const int nDomainSize = 30;
+
+    int nOldDomain_x = 0;
+    int nOldDomain_y = 0;
+
+    bool bMapUpdate = false;
+
+    while(mbRun)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(9));
+        if(nMapUpdateTime != mnNaviUpdateTime)
+        {
+            nMapUpdateTime = mnNaviUpdateTime;
+            std::vector<iv::GPSData> navigation_data;
+            mMutexNavi.lock();
+            navigation_data = m_navigation_data;
+            mMutexNavi.unlock();
+            UpdateMapDomain(navigation_data,xvectorMapDomain,nDomainSize);
+            bMapUpdate = true;
+        }
+
+        double fHeading = ServiceCarStatus.location->ins_heading_angle;
+        double fX = ServiceCarStatus.location->gps_x;
+        double fY = ServiceCarStatus.location->gps_y;
+
+        if((fabs(fHeading - foldhdg)<0.1)&&(fabs(foldx - fX)<0.1)&&(fabs(foldy - fY)<0.1) &&(bMapUpdate == false))
+        {
+            continue;
+        }
+
+        foldhdg = fHeading;
+        foldx = fX;
+        foldy = fY;
+
+        int nNowDomain_x = ((int)(fX/(double)nDomainSize))*nDomainSize;
+        int nNowDomain_y = ((int)(fY/(double)nDomainSize))*nDomainSize;
+
+        //Decide Need Update map
+        if((nNowDomain_x != nOldDomain_x) ||(nNowDomain_y != nOldDomain_y) || bMapUpdate)
+        {
+            bMapUpdate = false;
+            nOldDomain_x = nNowDomain_x;
+            nOldDomain_y = nNowDomain_y;
+            //Need Update option Map
+            xvectoroptmap.clear();
+            unsigned int i;
+            for(i=0;i<xvectorMapDomain.size();i++)
+            {
+                if(fabs(pow(xvectorMapDomain[i].mxpos - nNowDomain_x,2)+pow(xvectorMapDomain[i].mypos - nNowDomain_y,2))>150)
+                {
+                    continue;
+                }
+                unsigned int j;
+                for(j=0;j<xvectorMapDomain[i].mvectorgpsdata.size();j++)
+                {
+                    xvectoroptmap.push_back(xvectorMapDomain[i].mvectorgpsdata[j]);
+                }
+            }
+
+        }
+
+        std::vector<double> xDraw_x0,xDraw_y0,xDraw_x1,xDraw_y1,xDraw_x2,xDraw_y2;
+
+
+        int sizeN = xvectoroptmap.size();
+        for (int i = 0; i < sizeN; i++)
+        {
+            //         if(i == 1)std::cout<<" x1 = "<<x0[i]<<std::endl;
+            double x0,y0,x1,y1,x2,y2,k,k1,k2,xx,yy;
+            x0 = xvectoroptmap[i]->gps_x - fX;
+            y0 = xvectoroptmap[i]->gps_y - fY;
+            xx = x0;
+            yy = y0;
+            x0 = xx * cos(fHeading * PI / 180) - yy * sin(fHeading * PI / 180);
+            y0 = xx * sin(fHeading * PI / 180) + yy * cos(fHeading * PI / 180);
+            k1 = sin((90 + (xvectoroptmap[i]->ins_heading_angle - fHeading)) * PI / 180);
+            k2 = cos((90 + (xvectoroptmap[i]->ins_heading_angle - fHeading)) * PI / 180);
+            #if 0
+            x1[i] = x0[i] + k1 * 5;
+            y1[i] = y0[i] + k2 * 5;
+            x2[i] = x0[i] - k1 * 5;
+            y2[i] = y0[i] - k2 * 5;
+            #else
+            x1 = x0+ k1 * 1.75;
+            y1 = y0 + k2 * 1.75;
+            x2 = x0 - k1 * 1.75;
+            y2 = y0 - k2 * 1.75;
+            #endif
+
+            xDraw_x0.push_back(x0);
+            xDraw_y0.push_back(y0);
+            xDraw_x1.push_back(x1);
+            xDraw_y1.push_back(y1);
+            xDraw_x2.push_back(x2);
+            xDraw_y2.push_back(y2);
+
+            mMutexDraw.lock();
+            mDraw_x0 = xDraw_x0;
+            mDraw_x1 = xDraw_x1;
+            mDraw_x2 = xDraw_x2;
+            mDraw_y0 = xDraw_y0;
+            mDraw_y1 = xDraw_y1;
+            mDraw_y2 = xDraw_y2;
+            mMutexDraw.unlock();
+        }
+
+
+    }
+}
+
+void ADCIntelligentVehicle::UpdateMapDomain(std::vector<iv::GPSData> &xvectorMap, std::vector<iv::MapPointDomain> &xvectorMapDomain,const int nDomainSize)
+{
+    xvectorMapDomain.clear();
+    int nsize = xvectorMap.size();
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        iv::GPSData xGPSData = xvectorMap[i];
+        int  npos = -1;
+        int nMapDomainsize = xvectorMapDomain.size();
+        double xpos = xGPSData->gps_x;
+        double ypos = xGPSData->gps_y;
+        xpos = xpos/((double)nDomainSize);
+        ypos = ypos/((double)nDomainSize);
+        int nxpos = (int)xpos;
+        int nypos = (int)ypos;
+        nxpos = nxpos * nDomainSize;
+        nypos = nypos * nDomainSize;
+
+        //Find a map Domain
+        int j;
+        for(j=0;j<nMapDomainsize;j++)
+        {
+            if((nxpos == xvectorMapDomain[j].mxpos) &&(nypos == xvectorMapDomain[j].mypos))
+            {
+                npos = j;
+                break;
+            }
+        }
+
+        //not found, add one.
+        if(npos == -1)
+        {
+            iv::MapPointDomain xMP;
+            xMP.mxpos = nxpos;
+            xMP.mypos = nypos;
+            xMP.mvectorgpsdata.push_back(xGPSData);
+            xvectorMapDomain.push_back(xMP);
+        }
+        else
+        {
+            xvectorMapDomain[npos].mvectorgpsdata.push_back(xGPSData);
+        }
+
+    }
+
+}

src/ui/ui_ads_hmi/ADCIntelligentVehicle.h

@@ -148,6 +148,19 @@ public:
     struct dataInfo mInfo;
 };
 
+
+//Split Map to Some  Domain for quick find gps point for draw.
+namespace iv {
+struct MapPointDomain
+{
+public:
+    int mxpos;
+    int mypos;
+    std::vector<iv::GPSData> mvectorgpsdata;
+};
+}
+
+
 class ADCIntelligentVehicle : public QMainWindow
 {
     Q_OBJECT
@@ -279,6 +292,7 @@ private:
     void * mpa;
     QMutex mMutexNavi;
     std::vector<iv::GPSData> m_navigation_data;
+    qint64 mnNaviUpdateTime = 0;
 
     bool mbBrainRunning = false;
     double mdecition_period = 0;
@@ -388,6 +402,15 @@ public:
 private:
     QImage img,img2,img3,img4;
     QTimer mTimerState1;
+
+private:
+    void threadCalcDrawTrace();  //Thread for caluclate draw trace.
+    void UpdateMapDomain(std::vector<iv::GPSData> & xvectorMap,std::vector<iv::MapPointDomain> & xvectorMapDomain,const int nDomainSize);
+    std::vector<double> mDraw_x0,mDraw_y0,mDraw_x1,mDraw_y1,mDraw_x2,mDraw_y2;  //Save Draw Point
+    QMutex mMutexDraw;
+
+
+
 };