|
@@ -0,0 +1,684 @@
|
|
|
+#include "nvcan.h"
|
|
|
+
|
|
|
+#include <stdio.h>
|
|
|
+#include <stdlib.h>
|
|
|
+#include <stdint.h>
|
|
|
+#include <unistd.h>
|
|
|
+#include <string.h>
|
|
|
+#include <signal.h>
|
|
|
+#include <ctype.h>
|
|
|
+#include <libgen.h>
|
|
|
+#include <time.h>
|
|
|
+#include <errno.h>
|
|
|
+
|
|
|
+#include <sys/time.h>
|
|
|
+#include <sys/types.h>
|
|
|
+#include <sys/socket.h>
|
|
|
+#include <sys/ioctl.h>
|
|
|
+#include <sys/uio.h>
|
|
|
+#include <net/if.h>
|
|
|
+
|
|
|
+
|
|
|
+#include <linux/can.h>
|
|
|
+#include <linux/can/raw.h>
|
|
|
+
|
|
|
+#include <QDateTime>
|
|
|
+
|
|
|
+#include <iostream>
|
|
|
+#include <thread>
|
|
|
+
|
|
|
+/* for hardware timestamps - since Linux 2.6.30 */
|
|
|
+#ifndef SO_TIMESTAMPING
|
|
|
+#define SO_TIMESTAMPING 37
|
|
|
+#endif
|
|
|
+
|
|
|
+/* from #include <linux/net_tstamp.h> - since Linux 2.6.30 */
|
|
|
+#define SOF_TIMESTAMPING_SOFTWARE (1<<4)
|
|
|
+#define SOF_TIMESTAMPING_RX_SOFTWARE (1<<3)
|
|
|
+#define SOF_TIMESTAMPING_RAW_HARDWARE (1<<6)
|
|
|
+
|
|
|
+#define MAXSOCK 16 /* max. number of CAN interfaces given on the cmdline */
|
|
|
+#define MAXIFNAMES 30 /* size of receive name index to omit ioctls */
|
|
|
+#define MAXCOL 6 /* number of different colors for colorized output */
|
|
|
+#define ANYDEV "any" /* name of interface to receive from any CAN interface */
|
|
|
+#define ANL "\r\n" /* newline in ASC mode */
|
|
|
+
|
|
|
+#define SILENT_INI 42 /* detect user setting on commandline */
|
|
|
+#define SILENT_OFF 0 /* no silent mode */
|
|
|
+#define SILENT_ANI 1 /* silent mode with animation */
|
|
|
+#define SILENT_ON 2 /* silent mode (completely silent) */
|
|
|
+
|
|
|
+#include <QTime>
|
|
|
+
|
|
|
+#define BUF_SIZE 1000
|
|
|
+
|
|
|
+std::string CANNAME[] = {"can0","can1"};
|
|
|
+
|
|
|
+nvcan::nvcan(const char * strcanname)
|
|
|
+{
|
|
|
+// qDebug("nvcan");
|
|
|
+// connect(this,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onMsg(bool,int,const char*)));
|
|
|
+
|
|
|
+ CANNAME[0] = strcanname;
|
|
|
+ mfault = new iv::Ivfault("can_socket");
|
|
|
+ mivlog = new iv::Ivlog("can_socket");
|
|
|
+
|
|
|
+
|
|
|
+ mfault->SetFaultState(0,0,"Prepare Initialize.");
|
|
|
+
|
|
|
+ mpsendthread = new std::thread(&nvcan::threadsend,this);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::ExecRecv(int s)
|
|
|
+{
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::run()
|
|
|
+{
|
|
|
+
|
|
|
+ int currmax = 1;
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ addr.can_family = AF_CAN;
|
|
|
+
|
|
|
+ memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
|
|
|
+ strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
|
|
|
+
|
|
|
+ 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 (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
|
+ mfault->SetFaultState(2,3,"bind error.");
|
|
|
+ emit SIG_CANOPENSTATE(false,-3,"bind error");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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");
|
|
|
+
|
|
|
+ std::cout<<"can open succesfully."<<std::endl;
|
|
|
+
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ // std::cout<<"time: "<<QDateTime::currentMSecsSinceEpoch()<<" ret : "<<ret<<std::endl;
|
|
|
+
|
|
|
+ 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;
|
|
|
+
|
|
|
+ mMutexRW.lock();
|
|
|
+ nbytes = recvmsg(s[i], &msg, 0);
|
|
|
+ mMutexRW.unlock();
|
|
|
+
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ((size_t)nbytes == CAN_MTU)
|
|
|
+ maxdlen = CAN_MAX_DLEN;
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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.");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(bRecv)continue;
|
|
|
+
|
|
|
+
|
|
|
+ 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
|
|
|
+
|
|
|
+ for(int nch =0;nch<currmax;nch++)
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ for (i=0; i<currmax; i++)
|
|
|
+ {
|
|
|
+ close(s[i]);
|
|
|
+ }
|
|
|
+ qDebug("nvcan thread close.");
|
|
|
+ mbRunning = false;
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::threadsend()
|
|
|
+{
|
|
|
+
|
|
|
+ return;
|
|
|
+ int currmax = 1;
|
|
|
+ int s[MAXSOCK];
|
|
|
+ int i;
|
|
|
+
|
|
|
+ struct sockaddr_can addr;
|
|
|
+ struct ifreq ifr;
|
|
|
+
|
|
|
+ for(i=0;i<currmax;i++)
|
|
|
+ {
|
|
|
+ s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
|
|
+ if (s[i] < 0) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ addr.can_family = AF_CAN;
|
|
|
+
|
|
|
+ memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
|
|
|
+ strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
|
|
|
+
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"threadsend open can success."<<std::endl;
|
|
|
+
|
|
|
+ mps = &s[0];
|
|
|
+
|
|
|
+ // int currmax = 1;
|
|
|
+ // int i;
|
|
|
+ while(mbCANOpen == false)
|
|
|
+ {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
+ }
|
|
|
+
|
|
|
+ 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);
|
|
|
+ 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++)
|
|
|
+ {
|
|
|
+ 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 > 30)
|
|
|
+ {
|
|
|
+ // std::cout<<"retry fail,retry:"<<nretry<<std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ if(nretry < 100)
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"nvcan::threadsend exit."<<std::endl;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::startdev()
|
|
|
+{
|
|
|
+ start();
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::stopdev()
|
|
|
+{
|
|
|
+ requestInterruption();
|
|
|
+ QTime xTime;
|
|
|
+ xTime.start();
|
|
|
+ while(xTime.elapsed()<100)
|
|
|
+ {
|
|
|
+ if(mbRunning == false)
|
|
|
+ {
|
|
|
+ mfault->SetFaultState(1, 0, "can closed");
|
|
|
+ mivlog->error("can is closed at %d",xTime.elapsed());
|
|
|
+ qDebug("can is closed.");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int nvcan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
|
|
|
+{
|
|
|
+ if((nch>1)||(nch < 0))return -1;
|
|
|
+ if(mMsgRecvBuf[nch].size() == 0)return 0;
|
|
|
+
|
|
|
+ int nRtn;
|
|
|
+ nRtn = nCap;
|
|
|
+ mMutex.lock();
|
|
|
+ if(nRtn > mMsgRecvBuf[nch].size())nRtn = mMsgRecvBuf[nch].size();
|
|
|
+ int i;
|
|
|
+ for(i=0;i<nRtn;i++)
|
|
|
+ {
|
|
|
+ memcpy(&pMsg[i],&(mMsgRecvBuf[nch].at(i)),sizeof(basecan_msg));
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<basecan_msg>::iterator iter;
|
|
|
+ iter = mMsgRecvBuf[nch].begin();
|
|
|
+ for(i=0;i<nRtn;i++)
|
|
|
+ {
|
|
|
+ iter = mMsgRecvBuf[nch].erase(iter);
|
|
|
+ }
|
|
|
+
|
|
|
+ mMutex.unlock();
|
|
|
+
|
|
|
+ return nRtn;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int nvcan::SetMessage(const int nch, basecan_msg *pMsg)
|
|
|
+{
|
|
|
+ if((nch>1)||(nch < 0))return -1;
|
|
|
+
|
|
|
+ mMutex.lock();
|
|
|
+ if(mMsgSendBuf[nch].size() > BUF_SIZE)
|
|
|
+ {
|
|
|
+ std::cout<<"buffer full."<<std::endl;
|
|
|
+ mMutex.unlock();
|
|
|
+ return -2;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(mMsgRecvBuf[nch].size() > 100)
|
|
|
+ {
|
|
|
+ std::cout<<"buffer data more 100"<<std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ mMsgSendBuf[nch].push_back(*pMsg);
|
|
|
+ mMutex.unlock();
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::CmdSend()
|
|
|
+{
|
|
|
+ mwc.wakeAll();
|
|
|
+}
|
|
|
+
|
|
|
+void nvcan::onMsg(bool bCAN, int nR, const char *strres)
|
|
|
+{
|
|
|
+ mivlog->verbose("msg is %s ",strres);
|
|
|
+}
|
|
|
+
|