fix(driver_ultrasonic):data sending is real time now, but need to test

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_consumer.cpp

@@ -2,6 +2,18 @@
 
 #include <thread>
 
+#include <iostream>
+#include <string>
+//#include <stdlib.h>
+#include <unistd.h>
+
+#include <net/if.h>
+//#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmCANSend;
 extern uint64_t gTime_ms;
@@ -15,45 +27,104 @@ CANSend_Consumer::~CANSend_Consumer()
 {
     requestInterruption();
     while(this->isFinished() == false);
+    close(s);
 }
 
+//void CANSend_Consumer::run()
+//{
+//    uint64_t CANPackageIndex = 0;
+//    uint8_t CANPackageChannel;
+//    iv::can::canmsg xmsg;
+
+//    if(setupConfig.strMemCANSend == "cansend0")
+//        CANPackageChannel = 0;
+//    else
+//        CANPackageChannel = 1;
+
+//    while (!QThread::isInterruptionRequested())
+//    {
+//        pBuffer->Consume_Element_To_CANMsg(xmsg);
+//        if(xmsg.rawmsg_size()>0)
+//        {
+//            //pack
+//            xmsg.set_index(CANPackageIndex);
+//            xmsg.set_channel(CANPackageChannel);
+//            xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
+//            //send
+//            int ndatasize = xmsg.ByteSize();
+//            char * strser = new char[ndatasize];
+//            std::shared_ptr<char> pstrser;
+//            pstrser.reset(strser);
+//            if(xmsg.SerializePartialToArray(strser,ndatasize))
+//            {
+//                iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,strser,ndatasize);
+//            }
+//            else
+//            {
+//                std::cout<<"CANSend_Consumer serialize error."<<std::endl;
+//            }
+//            //clear and update index
+//            xmsg.clear_rawmsg();
+//            CANPackageIndex++;
+//            std::cout<<"send consumer time :"<<(QDateTime::currentMSecsSinceEpoch() - gTime_ms)<<std::endl;
+//        }
+//    }
+//}
+
 void CANSend_Consumer::run()
 {
-    uint64_t CANPackageIndex = 0;
-    uint8_t CANPackageChannel;
-    iv::can::canmsg xmsg;
+    iv::can::canraw xraw;
+
+    int required_mtu;
+    struct sockaddr_can addr;
+    struct can_frame frame;
+    struct ifreq ifr;
+
+    required_mtu = sizeof(struct can_frame);
+
+    if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
+    {
+        perror("socket");
+    }
 
+    std::string interfaceName;
     if(setupConfig.strMemCANSend == "cansend0")
-        CANPackageChannel = 0;
+        interfaceName = "can0";
     else
-        CANPackageChannel = 1;
+        interfaceName = "can1";
+
+    strncpy(ifr.ifr_name, interfaceName.c_str(), IFNAMSIZ - 1);
+    ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+    ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
+    if (!ifr.ifr_ifindex)
+    {
+        perror("if_nametoindex");
+    }
+
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
+
+    if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+    {
+        perror("bind");
+    }
 
     while (!QThread::isInterruptionRequested())
     {
-        pBuffer->Consume_Element_To_CANMsg(xmsg);
-        if(xmsg.rawmsg_size()>0)
+        xraw.CopyFrom(pBuffer->Consume_Element());
+        frame.can_id = xraw.id() & 0x000007ff;
+        for(uint8_t i=0;i<xraw.len();i++)
+            frame.data[i] = xraw.data().c_str()[i];
+        frame.can_dlc = xraw.len();
+        frame.__pad = 0;
+
+        if (write(s, &frame, required_mtu) != required_mtu)
         {
-            //pack
-            xmsg.set_index(CANPackageIndex);
-            xmsg.set_channel(CANPackageChannel);
-            xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
-            //send
-            int ndatasize = xmsg.ByteSize();
-            char * strser = new char[ndatasize];
-            std::shared_ptr<char> pstrser;
-            pstrser.reset(strser);
-            if(xmsg.SerializePartialToArray(strser,ndatasize))
-            {
-                iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,strser,ndatasize);
-            }
-            else
-            {
-                std::cout<<"CANSend_Consumer serialize error."<<std::endl;
-            }
-            //clear and update index
-            xmsg.clear_rawmsg();
-            CANPackageIndex++;
-            std::cout<<"send consumer time :"<<(QDateTime::currentMSecsSinceEpoch() - gTime_ms)<<std::endl;
+            perror("write");
         }
+
+        std::cout<<"send consumer time :"<<(QDateTime::currentMSecsSinceEpoch() - gTime_ms)<<std::endl;
     }
 }

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_consumer.h

@@ -24,6 +24,8 @@ protected:
 
 private:
     CAN_Producer_Consumer *pBuffer;
+
+    int s; /* can raw socket */
 };
 
 #endif // CANSEND_CONSUMER_H