change driver_can_nvidia_agx_new. not complete.

src/driver/driver_can_nvidia_agx_new/canctrl.cpp

@@ -150,11 +150,27 @@ void canctrl::sendmsg(int index, iv::can::canmsg xmsg)
     std::vector<basecan_msg> * psendmsgvector;
     QMutex * pMutex;
 
-    pMutex = &mMutexcan1;
-    psendmsgvector = &msendmsgvector1;
+    if(index == 0)
+    {
+        pMutex = &mMutexcan1;
+        psendmsgvector = &msendmsgvector1;
+    }
+    else
+    {
+        if(index == 1)
+        {
+            pMutex = &mMutexcan2;
+            psendmsgvector = &msendmsgvector2;
+        }
+        else
+        {
+            std::cout<<" canctrl::sendmsg "<<" index error. index: "<<index<<std::endl;
+            return;
+        }
+    }
 
 
-    if(psendmsgvector->size() > SENDMSGBUFSIZE)
+    if((int)psendmsgvector->size() > SENDMSGBUFSIZE)
     {
         mpcan->mivlog->warn("sendmsg buf full");
         return;
@@ -172,9 +188,7 @@ void canctrl::sendmsg(int index, iv::can::canmsg xmsg)
         sendmsg.id = x.id();
         sendmsg.isExtern = x.bext();
         sendmsg.isRemote = x.bremote();
-#ifdef SEND_STAT
         sendmsg.mSetTime = QDateTime::currentMSecsSinceEpoch();
-#endif
         int nlen = x.len();
 
         if((nlen < 0) || (nlen > 8))
@@ -196,13 +210,13 @@ void canctrl::sendmsg(int index, iv::can::canmsg xmsg)
 
     if(mbCANOpen)
     {
-        mMutexcan1.lock();
-        for(i=0;i<msendmsgvector1.size();i++)
+        pMutex->lock();
+        for(i=0;i<psendmsgvector->size();i++)
         {
-            mpcan->SetMessage(0,&(msendmsgvector1.at(i)));
+            mpcan->SetMessage(index,&(psendmsgvector->at(i)));
         }
-        msendmsgvector1.clear();
-        mMutexcan1.unlock();
+        psendmsgvector->clear();
+        pMutex->unlock();
         mpcan->CmdSend();
     }
 

src/driver/driver_can_nvidia_agx_new/canctrl.h

@@ -32,9 +32,11 @@ private:
     bool mbCANOpen = false;
 
     std::vector<basecan_msg> msendmsgvector1;
+    std::vector<basecan_msg> msendmsgvector2;
     const int SENDMSGBUFSIZE = 3000;
 
     QMutex mMutexcan1;
+    QMutex mMutexcan2;
 
     int mindex[2];
 

src/driver/driver_can_nvidia_agx_new/main.cpp

@@ -50,6 +50,7 @@ void signal_handler(int sig)
 #ifdef TEST_PROG
 void threadtest()
 {
+
     QTimer xTimer;
     xTimer.setTimerType(Qt::PreciseTimer);
     void * pa = iv::modulecomm::RegisterSend("cansend0",100000,100);
@@ -60,7 +61,7 @@ void threadtest()
         xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
         xmsg.set_index(0);
         int i;
-        for(i=0;i<5;i++)
+        for(i=0;i<10;i++)
         {
             iv::can::canraw * pcanraw = xmsg.add_rawmsg();
             pcanraw->set_bext(false);
@@ -84,6 +85,7 @@ void threadtest()
 
 void threadtest2()
 {
+    return;
     QTimer xTimer;
     xTimer.setTimerType(Qt::PreciseTimer);
     void * pa = iv::modulecomm::RegisterSend("cansend1",100000,100);
@@ -94,7 +96,7 @@ void threadtest2()
         xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
         xmsg.set_index(0);
         int i;
-        for(i=0;i<5;i++)
+        for(i=0;i<3;i++)
         {
             iv::can::canraw * pcanraw = xmsg.add_rawmsg();
             pcanraw->set_bext(false);
@@ -111,7 +113,7 @@ void threadtest2()
         if(xmsg.SerializeToArray(str_ptr.get(),nbytesize))
             iv::modulecomm::ModuleSendMsg(pa,str_ptr.get(),nbytesize);
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
  //       std::this_thread::sleep_for(std::chrono::microseconds(50000));
     }
 }

src/driver/driver_can_nvidia_agx_new/nvcan.cpp

@@ -67,417 +67,324 @@ nvcan::nvcan(const char * strcan0name,const char * strcan1name)
 
     mfault->SetFaultState(0,0,"Prepare Initialize.");
 
-//    mpsendthread = new std::thread(&nvcan::threadsend,this);
+    QTimer * timer1 = new QTimer();
+    connect(timer1,SIGNAL(timeout()),this,SLOT(TimerSecond()));
+    timer1->start(1000);
 
 
 }
 
-void nvcan::ExecRecv(int s)
+int nvcan::ExecOpen(int &s, const char *strcanname)
 {
-
-}
-
-void nvcan::run()
-{
-
-    int currmax = 2;
-    fd_set rdfs;
-    int s[MAXSOCK];
-    int ret;
-    
     struct sockaddr_can addr;
-    char ctrlmsg[CMSG_SPACE(sizeof(struct timeval) + 3*sizeof(struct timespec) + sizeof(__u32))];
-    struct iovec iov;
-    struct msghdr msg;
-    struct canfd_frame frame;
-    int nbytes, i, maxdlen;
     struct ifreq ifr;
-    struct timeval tv, last_tv;
-    struct timeval timeout_config = { 0, 0 }, *timeout_current = 0;
-
-    mfault->SetFaultState(0,0,"Initializing.");
-    
-    for(i=0;i<currmax;i++)
-    {
-        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
-        if (s[i] < 0) {
-            mfault->SetFaultState(2,1,"Create Socket Error.");
-            emit SIG_CANOPENSTATE(false,-1,"Create Socket Error");
-            return;
-        }
+    s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (s < 0) {
+        mfault->SetFaultState(2,1,"Create Socket Error.");
+        emit SIG_CANOPENSTATE(false,-1,"Create Socket Error");
+        return -1;
+    }
 
-        addr.can_family = AF_CAN;
+    addr.can_family = AF_CAN;
 
-        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
-        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
+    memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+    strncpy(ifr.ifr_name, strcanname, IFNAMSIZ);
 
-        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
-            mfault->SetFaultState(2,2,"SIOCGIFINDEX.");
-            emit SIG_CANOPENSTATE(false,-2,"SIOCGIFINDEX");
-            return;
-        }
-        addr.can_ifindex = ifr.ifr_ifindex;
+    if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+        mfault->SetFaultState(2,2,"SIOCGIFINDEX.");
+        emit SIG_CANOPENSTATE(false,-2,"SIOCGIFINDEX");
+        return -2;
+    }
+    addr.can_ifindex = ifr.ifr_ifindex;
 
-        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
-            mfault->SetFaultState(2,3,"bind error.");
-            emit SIG_CANOPENSTATE(false,-3,"bind error");
-            return;
-        }
+    if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+        mfault->SetFaultState(2,3,"bind error.");
+        emit SIG_CANOPENSTATE(false,-3,"bind error");
+        return -3;
     }
 
-    mps = &s[0];
-    mbCANOpen = true;
-    mivlog->verbose("open can succesfully.");
-    mfault->SetFaultState(0,0,"CAN OK.");
-    emit SIG_CANOPENSTATE(true,0,"open can card successfully");
+    return 0;
+}
 
-    std::cout<<"can open succesfully."<<std::endl;
+int nvcan::ExecRecv(int & s,fd_set & rdfs,int nch)
+{
+    char ctrlmsg[CMSG_SPACE(sizeof(struct timeval) + 3*sizeof(struct timespec) + sizeof(__u32))];
+    struct sockaddr_can addr;
+    struct timeval timeout_config = { 0, 0 };
+    timeout_config.tv_sec= 0;
+    timeout_config.tv_usec = 0;;
+    struct iovec iov;
+    struct msghdr msg;
+    struct canfd_frame frame;
+    int nbytes, i, maxdlen;
 
     iov.iov_base = &frame;
     msg.msg_name = &addr;
     msg.msg_iov = &iov;
     msg.msg_iovlen = 1;
     msg.msg_control = &ctrlmsg;
-
-    qint64 nLastRecv = QDateTime::currentMSecsSinceEpoch();
-    int nRecvState = 0; // 0 Have Data  1 No Data;
-
-    mbRunning = true;
-
-    int nrecvcount = 0;
-
-    qint64 nlastsecond = 0;
-    int nsecondrecvcount = 0;
-
-    qint64 nLastSecond = 0;
-    int nsecondsend = 0;
-    int nretry = 0;
-#ifdef SEND_STAT
-    std::vector<qint64> xvectorlat;
-#endif
-    int secondretrycount = 0;
-
-    while((!QThread::isInterruptionRequested())&&(mbCANOpen))
-    {
-        FD_ZERO(&rdfs);
-        for (i=0; i<currmax; i++)
-            FD_SET(s[i], &rdfs);
-
-        if (timeout_current)
-            *timeout_current = timeout_config;
-
-        timeout_config.tv_sec= 0;
-        timeout_config.tv_usec = 0;;
-        timeout_current = &timeout_config;
-        ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, timeout_current);
-        if (ret < 0) {
-            emit SIG_CANOPENSTATE(false,-4,"select error");
-            mfault->SetFaultState(2,4,"select error.");
-            std::cout<<"select error."<<std::endl;
-            mbCANOpen = false;
-            continue;
+//    int ret = select(s, &rdfs, NULL, NULL, &timeout_config);
+//    if (ret < 0) {
+//        emit SIG_CANOPENSTATE(false,-4,"select error");
+//        mfault->SetFaultState(2,4,"select error.");
+//        std::cout<<"select error."<<std::endl;
+//        mbCANOpen = false;
+//        return 0;
+//    }
+
+//       std::cout<<"time: "<<QDateTime::currentMSecsSinceEpoch()<<" ret : "<<ret<<std::endl;
+
+    bool bRecv = false;
+
+    if (FD_ISSET(s, &rdfs)) {
+
+
+
+        /* these settings may be modified by recvmsg() */
+        iov.iov_len = sizeof(frame);
+        msg.msg_namelen = sizeof(addr);
+        msg.msg_controllen = sizeof(ctrlmsg);
+        msg.msg_flags = 0;
+
+        mMutexRW.lock();
+        nbytes = recvmsg(s, &msg, 0);
+        mMutexRW.unlock();
+
+ //       std::cout<<"nbytes: "<<nbytes<<std::endl;
+        if (nbytes < 0) {
+            //                   if ((errno == ENETDOWN) && !down_causes_exit) {
+            if ((errno == ENETDOWN)) {
+                mivlog->error("interface down" );
+                mfault->SetFaultState(1, 0, "interface down");
+                emit SIG_CANOPENSTATE(false,-5,"can card down");
+                //                   fprintf(stderr, "%s: interface down\n", CANNAME[i].data());
+                return -1;
+            }
+            return 0;
+            //                    perror("read");
+            //                    return 1;
         }
 
- //       std::cout<<"time: "<<QDateTime::currentMSecsSinceEpoch()<<" ret : "<<ret<<std::endl;
+        if ((size_t)nbytes == CAN_MTU)
+            maxdlen = CAN_MAX_DLEN;
+        else if ((size_t)nbytes == CANFD_MTU)
+            maxdlen = CANFD_MAX_DLEN;
+        else {
 
-        bool bRecv = false;
-        for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */
-
-            if (FD_ISSET(s[i], &rdfs)) {
-
-                nLastRecv = QDateTime::currentMSecsSinceEpoch();
-                /* these settings may be modified by recvmsg() */
-                iov.iov_len = sizeof(frame);
-                msg.msg_namelen = sizeof(addr);
-                msg.msg_controllen = sizeof(ctrlmsg);
-                msg.msg_flags = 0;
+            mivlog->warn("read incomplete message");
+            return 0;
+        }
 
-                mMutexRW.lock();
-                nbytes = recvmsg(s[i], &msg, 0);
-                mMutexRW.unlock();
+        mMutexSecondCount.lock();
+        mnSecond_RecvCount[nch]++;
+        mMutexSecondCount.unlock();
+        bRecv = true;
+        mMutex.lock();
+
+        basecan_msg msg;
+        msg.id = frame.can_id&0x1fffffff;
+        if((frame.can_id&0x80000000)!= 0)msg.isExtern = true;
+        else msg.isExtern = false;
+        if((frame.can_id&0x40000000)!= 0)msg.isRemote = true;
+        else msg.isRemote = false;
+        msg.nLen = frame.len;
+        if((frame.len<9)&&(frame.len>0))memcpy(msg.data,frame.data,frame.len);
+        if(mMsgRecvBuf[i].size()<BUF_SIZE)
+        {
+            mMsgRecvBuf[i].push_back(msg);
+        }
 
-                if (nbytes < 0) {
- //                   if ((errno == ENETDOWN) && !down_causes_exit) {
-                    if ((errno == ENETDOWN)) {
-                        mivlog->error("%s interface down", CANNAME[i].data());
-                        mfault->SetFaultState(1, 0, "interface down");
-                        emit SIG_CANOPENSTATE(false,-5,"can card down");
-                        fprintf(stderr, "%s: interface down\n", CANNAME[i].data());
-                        return;
-                    }
-                    continue;
-//                    perror("read");
-//                    return 1;
-                }
+        mMutex.unlock();
 
-                if ((size_t)nbytes == CAN_MTU)
-                    maxdlen = CAN_MAX_DLEN;
-                else if ((size_t)nbytes == CANFD_MTU)
-                    maxdlen = CANFD_MAX_DLEN;
-                else {
-                    mivlog->warn("read incomplete message");
-                    continue;
-                }
 
-                bRecv = true;
-                nrecvcount++;
- //               qDebug("receive msg.");
-                mMutex.lock();
-
-                basecan_msg msg;
-                msg.id = frame.can_id&0x1fffffff;
-                if((frame.can_id&0x80000000)!= 0)msg.isExtern = true;
-                else msg.isExtern = false;
-                if((frame.can_id&0x40000000)!= 0)msg.isRemote = true;
-                else msg.isRemote = false;
-                msg.nLen = frame.len;
-                nsecondrecvcount++;
-                if((frame.len<9)&&(frame.len>0))memcpy(msg.data,frame.data,frame.len);
-                if(mMsgRecvBuf[i].size()<BUF_SIZE)
-                {
-                    mMsgRecvBuf[i].push_back(msg);
-                }
 
-                mMutex.unlock();
+    }
 
+    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.");
+//        }
+//    }
+}
 
 
-            }
-        }
+void nvcan::TimerSecond()
+{
+    qint64 xSecond_RecvCount[2];
+    qint64 xSecond_SendCount[2];
+    std::vector<qint64> xvectorsendlat[2];
+    qint64 xLatency[2];
+    qint64 xLatencyMax[2];
+    int i;
 
-        qint64 nsecondnow = QDateTime::currentSecsSinceEpoch();
-        if(nlastsecond != nsecondnow)
-        {
-            nlastsecond = nsecondnow;
-            std::cout<<"second recv count:"<<nsecondrecvcount<<std::endl;
-            nsecondrecvcount = 0;
-        }
+    //Copy Data And Reset.
+    mMutexSecondCount.lock();
+    for(i=0;i<2;i++)
+    {
+        xSecond_RecvCount[i] = mnSecond_RecvCount[i];
+        xSecond_SendCount[i] = mnSecond_SendCount[i];
+        xvectorsendlat[i] = mvectorsendlat[i];
+        mnSecond_RecvCount[i] = 0;
+        mnSecond_SendCount[i] = 0;
+        mvectorsendlat[i].clear();
+    }
+    mMutexSecondCount.unlock();
 
-        if((QDateTime::currentMSecsSinceEpoch() - nLastRecv)> 1000)
+   //Calulate Average Latency And Maximum Latency
+    for(i=0;i<2;i++)
+    {
+        xLatency[i] = 0;
+        xLatencyMax[i] = 0;
+        int j;
+        for(j=0;j<((int)xvectorsendlat[i].size());j++)
         {
-            if(nRecvState == 0)
-            {
-                nRecvState = -1;
-                mfault->SetFaultState(0,1,"More than 1 second not receive data.");
-            }
+            qint64 xlat = xvectorsendlat[i].at(j);
+            xLatency[i] = xLatency[i] + xlat;
+            if(xlat > xLatencyMax[i])xLatencyMax[i] =xlat;
         }
-        else
+        if(xvectorsendlat[i].size()  > 0)
         {
-            if(nRecvState == -1)
-            {
-                nRecvState = 0;
-                mfault->SetFaultState(0,0,"CAN OK.");
-            }
+            xLatency[i] = xLatency[i]/xvectorsendlat[i].size();
         }
+    }
 
-        if(bRecv)continue;
+    //Print
+    std::cout<<"Second Count:"<<std::endl;
+    std::cout<<"       "<<"ch0 Recv Count: "<<xSecond_RecvCount[0]<<std::endl;
+    std::cout<<"       "<<"ch1 Recv Count: "<<xSecond_RecvCount[1]<<std::endl;
+    std::cout<<"       "<<"ch0 Send Count: "<<xSecond_SendCount[0]<<std::endl;
+    std::cout<<"       "<<"    Latency Max: "<<xLatencyMax[0]<<" Avg:"<<xLatency[0]<<std::endl;
+    std::cout<<"       "<<"ch1 Send Count: "<<xSecond_SendCount[1]<<std::endl;
+    std::cout<<"       "<<"    Latency Max: "<<xLatencyMax[1]<<" Avg:"<<xLatency[1]<<std::endl;
 
 
-        mWaitMutex.lock();
-        mwc.wait(&mWaitMutex,1);
-        mWaitMutex.unlock();
-
-#ifdef TEST_PROG
- //       qDebug("send time : %lld",QDateTime::currentMSecsSinceEpoch());
-#endif
+}
 
-        struct canfd_frame framesend[2500];
-#ifdef SEND_STAT
-        qint64 sendsettime[2500];
-#endif
+int nvcan::ExecSend(int * s,struct canfd_frame & framesend,int nch,qint64 xMsgSetTime)
+{
+    int nretry = 0;
 
-        for(int nch =0;nch<currmax;nch++)
+    bool bSend = false;
+    //write retry 5 times
+    while(nretry<5)
+    {
+        if (write(*s, &framesend,16) != 16) {
+            mivlog->error("write error 1");
+            nretry++;
+            perror("write error.");
+            std::this_thread::sleep_for(std::chrono::microseconds(100));
+        }
+        else
         {
-            int nsend = 0;
-            mMutex.lock();
-            int nbufsize = mMsgSendBuf[nch].size();
-            if(nbufsize>2500)nbufsize = 2500;
-            for(i=0;i<nbufsize;i++)
-            {
-                if(i>=2500)break;
-                memcpy(framesend[i].data,mMsgSendBuf[nch].at(i).data,8);
-                framesend[i].can_id = mMsgSendBuf[nch].at(i).id;
-                if(mMsgSendBuf[nch].at(i).isExtern)
-                {
-                    framesend[i].can_id = framesend[i].can_id|0x80000000;
-                }
-                else
-                {
-                    framesend[i].can_id = framesend[i].can_id&0x7ff;
-                }
-                if(mMsgSendBuf[nch].at(i).isRemote)
-                {
-                    framesend[i].can_id= framesend[i].can_id|0x40000000;
-                }
-
-                framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
-#ifdef SEND_STAT
-                sendsettime[i] = mMsgSendBuf[nch].at(i).mSetTime;
-#endif
-
-                nsend++;
-            }
-            mMsgSendBuf[nch].clear();
-            mMutex.unlock();
-
-            if(nsend > 0)
-            {
-                for(i=0;i<nsend;i++)
-                {
-                    mMutexRW.lock();
-
-                    if (write(mps[nch], &framesend[i],16) != 16) {
-                        mMutexRW.unlock();
-                        mivlog->error("write error 1");
- //                       perror("write error 1.");
-
-                        nretry++;
-                        secondretrycount++;
-                        if(nretry > 5)
-                        {
- //                           std::cout<<"retry fail,retry:"<<nretry<<std::endl;
-                        }
-                        else
-                        {
-
-
-                        }
-                        if(nretry < 5)
-                        {
-                            i--;
-                        }
-                        else
-                        {
-                            nretry = 0;
-//                            std::cout<<"retry more than 100. drop this message."<<std::endl;
-                        }
-                         std::this_thread::sleep_for(std::chrono::microseconds(100));
- //                       std::cout<<"retry send."<<std::endl;
-                        continue;
-                    }
-                    else
-                    {
-                        mMutexRW.unlock();
-#ifdef SEND_STAT
-                        qint64 nnowms = QDateTime::currentMSecsSinceEpoch();
-                        qint64 nlat = nnowms - sendsettime[i];
-                        xvectorlat.push_back(nlat);
-                        nretry = 0;
-                        nsecondsend++;
-#endif
-                    }
-                }
-            }
-
-                        qint64 nnowsecond = QDateTime::currentSecsSinceEpoch();
-                        if( nnowsecond != nLastSecond)
-                        {
-                            nLastSecond = nnowsecond;
-                            std::cout<<" second send count: "<<nsecondsend<<std::endl;
-                            nsecondsend = 0;
-#ifdef SEND_STAT
-                            int j;
-                            int nsendcount = xvectorlat.size();
-                            if(nsendcount > 0)
-                            {
-                                qint64 xlatmax = 0;
-                                qint64 xlatavg = 0;
-                                for(j=0;j<nsendcount;j++)
-                                {
-                                    if(xvectorlat[j]> xlatmax)xlatmax = xvectorlat[j];
-                                    xlatavg = xlatavg + xvectorlat[j];
-                                }
-                                xlatavg = xlatavg/nsendcount;
-                                std::cout<<" max latency: "<<xlatmax<<" avg latency: "<<xlatavg
-                                        <<" second retry count:"<<secondretrycount<<std::endl;
-
-
-                                xvectorlat.clear();
-                            }
-#endif
-                            secondretrycount = 0;
-                        }
-
+            bSend = true;
 
+            qint64 nLat = QDateTime::currentMSecsSinceEpoch() - xMsgSetTime;
+            mMutexSecondCount.lock();
+            mnSecond_SendCount[nch]++;
+            mvectorsendlat[nch].push_back(nLat);
+            mMutexSecondCount.unlock();
+            break;
         }
 
-
-
     }
+    if(bSend)return 1;
+    return -1;
 
-    for (i=0; i<currmax; i++)
-    {
-        close(s[i]);
-    }
-    qDebug("nvcan thread close.");
-    mbRunning = false;
 }
 
-void nvcan::threadsend()
+void nvcan::run()
 {
 
-    return;
-    int currmax = 1;
+    int currmax = 2;
+    fd_set rdfs;
     int s[MAXSOCK];
     int i;
 
-    struct sockaddr_can addr;
-    struct ifreq ifr;
-
+    mfault->SetFaultState(0,0,"Initializing.");
+    
     for(i=0;i<currmax;i++)
     {
-        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
-        if (s[i] < 0) {
+
+        if(ExecOpen(s[i],CANNAME[i].data()) != 0)
+        {
+            std::cout<<"Open CAN "<<CANNAME[i]<<" Fail"<<std::endl;
             return;
         }
+        continue;
+    }
 
-        addr.can_family = AF_CAN;
+    mps = &s[0];
+    mbCANOpen = true;
+    mivlog->verbose("open can succesfully.");
+    mfault->SetFaultState(0,0,"CAN OK.");
+    emit SIG_CANOPENSTATE(true,0,"open can card successfully");
 
-        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
-        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
+    std::cout<<"can open succesfully."<<std::endl;
 
-        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
-            return;
-        }
-        addr.can_ifindex = ifr.ifr_ifindex;
 
-        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
-            return;
-        }
-    }
+    mbRunning = true;
 
-    std::cout<<"threadsend open can success."<<std::endl;
+    bool bRecv;
 
-    mps = &s[0];
+    struct timeval timeout_config = { 0, 0 };
 
- //   int currmax = 1;
- //   int i;
-    while(mbCANOpen == false)
+    while((!QThread::isInterruptionRequested())&&(mbCANOpen))
     {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    }
+        FD_ZERO(&rdfs);
+        for (i=0; i<currmax; i++)
+            FD_SET(s[i], &rdfs);
+
+
+        int ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, &timeout_config);
+
+        bRecv = false;
+
+        int nrecv1 = ExecRecv(s[0],rdfs,0);
+        if(nrecv1 >0)bRecv = true;
+        else
+        {
+            if(nrecv1<0)std::cout<<"CAN Recv Error."<<std::endl;
+        }
+        int nrecv2 = ExecRecv(s[1],rdfs,1);
+        if(nrecv2 >0)bRecv = true;
+        else
+        {
+            if(nrecv2<0)std::cout<<"CAN Recv Error."<<std::endl;
+        }
+
+        if(bRecv)continue;
 
-    qint64 nLastSecond = 0;
-    int nsecondsend = 0;
-    int nretry = 0;
-#ifdef SEND_STAT
-    std::vector<qint64> xvectorlat;
-#endif
-    int secondretrycount = 0;
-    while(mbSendRun)
-    {
         mWaitMutex.lock();
-        mwc.wait(&mWaitMutex,100);
+        mwc.wait(&mWaitMutex,1);
         mWaitMutex.unlock();
 
-#ifdef TEST_PROG
- //       qDebug("send time : %lld",QDateTime::currentMSecsSinceEpoch());
-#endif
-
         struct canfd_frame framesend[2500];
-#ifdef SEND_STAT
         qint64 sendsettime[2500];
-#endif
 
         for(int nch =0;nch<currmax;nch++)
         {
@@ -504,9 +411,7 @@ void nvcan::threadsend()
                 }
 
                 framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
-#ifdef SEND_STAT
                 sendsettime[i] = mMsgSendBuf[nch].at(i).mSetTime;
-#endif
 
                 nsend++;
             }
@@ -517,86 +422,49 @@ void nvcan::threadsend()
             {
                 for(i=0;i<nsend;i++)
                 {
-                    mMutexRW.lock();
-
-                    if (write(mps[nch], &framesend[i],16) != 16) {
-                        mMutexRW.unlock();
-                        mivlog->error("write error 1");
- //                       perror("write error 1.");
-
-                        nretry++;
-                        secondretrycount++;
-                        if(nretry > 30)
-                        {
- //                           std::cout<<"retry fail,retry:"<<nretry<<std::endl;
-                        }
-                        else
-                        {
-
+                    int nretry = 0;
 
-                        }
-                        if(nretry < 100)
-                        {
-                            i--;
+                    bool bSend = false;
+                    //write retry 5 times
+                    while(nretry<5)
+                    {
+                        if (write(s[nch], &framesend[i],16) != 16) {
+                            mivlog->error("write error 1");
+                            nretry++;
+                            perror("write error.");
+                            std::this_thread::sleep_for(std::chrono::microseconds(100));
                         }
                         else
                         {
-                            nretry = 0;
-//                            std::cout<<"retry more than 100. drop this message."<<std::endl;
+                            bSend = true;
+
+                            qint64 nLat = QDateTime::currentMSecsSinceEpoch() - sendsettime[i];
+                            mMutexSecondCount.lock();
+                            mnSecond_SendCount[nch]++;
+                            mvectorsendlat[nch].push_back(nLat);
+                            mMutexSecondCount.unlock();
+                            break;
                         }
-                         std::this_thread::sleep_for(std::chrono::microseconds(100));
- //                       std::cout<<"retry send."<<std::endl;
-                        continue;
-                    }
-                    else
-                    {
-                        mMutexRW.unlock();
-#ifdef SEND_STAT
-                        qint64 nnowms = QDateTime::currentMSecsSinceEpoch();
-                        qint64 nlat = nnowms - sendsettime[i];
-                        xvectorlat.push_back(nlat);
-                        nretry = 0;
-                        nsecondsend++;
-#endif
+
                     }
+  //                  std::cout<<" i "<<i<<" send "<<std::endl;
+  //                  ExecSend(&(s[nch]),framesend[i],nch,sendsettime[i]);
                 }
             }
 
-                        qint64 nnowsecond = QDateTime::currentSecsSinceEpoch();
-                        if( nnowsecond != nLastSecond)
-                        {
-                            nLastSecond = nnowsecond;
-                            std::cout<<" second send count: "<<nsecondsend<<std::endl;
-                            nsecondsend = 0;
-#ifdef SEND_STAT
-                            int j;
-                            int nsendcount = xvectorlat.size();
-                            if(nsendcount > 0)
-                            {
-                                qint64 xlatmax = 0;
-                                qint64 xlatavg = 0;
-                                for(j=0;j<nsendcount;j++)
-                                {
-                                    if(xvectorlat[j]> xlatmax)xlatmax = xvectorlat[j];
-                                    xlatavg = xlatavg + xvectorlat[j];
-                                }
-                                xlatavg = xlatavg/nsendcount;
-                                std::cout<<" max latency: "<<xlatmax<<" avg latency: "<<xlatavg
-                                        <<" second retry count:"<<secondretrycount<<std::endl;
-
-
-                                xvectorlat.clear();
-                            }
-#endif
-                            secondretrycount = 0;
-                        }
-
 
         }
-    }
 
-    std::cout<<"nvcan::threadsend exit."<<std::endl;
 
+
+    }
+
+    for (i=0; i<currmax; i++)
+    {
+        close(s[i]);
+    }
+    qDebug("nvcan thread close.");
+    mbRunning = false;
 }
 
 void nvcan::startdev()

src/driver/driver_can_nvidia_agx_new/nvcan.h

@@ -9,6 +9,7 @@
 
 #include <QWaitCondition>
 #include <QMutex>
+#include <QTimer>
 
 class nvcan : public basecan
 {
@@ -27,9 +28,10 @@ public:
 private slots:
     void onMsg(bool bCAN,int nR,const char * strres);
 
+    void TimerSecond();
+
 private:
     void run();
-    void threadsend();
     int * mps = 0;
 
 
@@ -54,7 +56,20 @@ private:
     QMutex mMutexRW;
 
 private:
-    void ExecRecv(int s);
+
+    int mnSecond_RecvCount[2];
+    int mnSecond_SendCount[2];
+
+    std::vector<qint64> mvectorsendlat[2];
+
+    QMutex mMutexSecondCount;
+
+private:
+    int ExecOpen(int & s,const char * strcanname);
+    int ExecRecv(int & s,fd_set & rdfs,int nch);
+    int ExecSend(int * s ,struct canfd_frame & framesend,int nch,qint64 xMsgSetTime);
+
+
 };
 
 #endif // NVCAN_H

src/driver/driver_can_socket/nvcan.cpp

@@ -214,6 +214,7 @@ void nvcan::run()
                 else if ((size_t)nbytes == CANFD_MTU)
                     maxdlen = CANFD_MAX_DLEN;
                 else {
+                    std::cout<<"read incomplate message."<<std::endl;
                     mivlog->warn("read incomplete message");
                     continue;
                 }