fix(driver_ARS408,grpc_BS,chassis.proto,gps_hcp2.pro):add some comment to chassis.proto, delete a empty line in gps_hcp2.pro, fix thread close mistake risk in grpc_BS, finishing driver_ARS408 but not yet.

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -23,7 +23,7 @@ struct msgunit
     char mstrmsgname[256];
     int mnBufferSize = 10000;
     int mnBufferCount = 1;
-    void * mpa;
+    void * mpa = nullptr;
     std::shared_ptr<char> mpstrmsgdata;
     int mndatasize = 0;
     bool mbRefresh = false;

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -33,7 +33,9 @@ VehicleControlClient::VehicleControlClient(std::shared_ptr<Channel> channel)
 
 VehicleControlClient::~VehicleControlClient(void)
 {
-
+    if(shmRemoteCtrl.mpa != nullptr)iv::modulecomm::Unregister(shmRemoteCtrl.mpa);
+    requestInterruption();
+    while(this->isFinished() == false);
 }
 
 void VehicleControlClient::dec_yaml(const char *stryamlpath)
@@ -200,7 +202,7 @@ void VehicleControlClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstrcontrolInterval.c_str());
-    while (true)
+    while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {
@@ -224,7 +226,8 @@ VehicleChangeCtrlModeClient::VehicleChangeCtrlModeClient(std::shared_ptr<Channel
 
 VehicleChangeCtrlModeClient::~VehicleChangeCtrlModeClient(void)
 {
-
+    requestInterruption();
+    while(this->isFinished() == false);
 }
 
 std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
@@ -280,7 +283,7 @@ void VehicleChangeCtrlModeClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstrcontrolInterval.c_str()) + 5; // move 5 ms to avoid ctrl data request
-    while (true)
+    while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {
@@ -299,7 +302,8 @@ VehicleUploadMapClient::VehicleUploadMapClient(std::shared_ptr<Channel> channel)
 
 VehicleUploadMapClient::~VehicleUploadMapClient(void)
 {
-
+    requestInterruption();
+    while(this->isFinished() == false);
 }
 
 std::string VehicleUploadMapClient::uploadMap(void)
@@ -367,7 +371,7 @@ void VehicleUploadMapClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstruploadMapInterval.c_str());
-    while (true)
+    while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {

src/driver/driver_cloud_grpc_client_BS/vehicle_control.h

@@ -26,7 +26,7 @@ struct msgunit
     char mstrmsgname[256];
     int mnBufferSize = 10000;
     int mnBufferCount = 1;
-    void * mpa;
+    void * mpa = nullptr;
     std::shared_ptr<char> mpstrmsgdata;
     int mndatasize = 0;
     bool mbRefresh = false;

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

@@ -29,7 +29,9 @@ VehiclePatrolExceptionClient::VehiclePatrolExceptionClient(std::shared_ptr<Chann
 
 VehiclePatrolExceptionClient::~VehiclePatrolExceptionClient(void)
 {
-
+    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
+    requestInterruption();
+    while(this->isFinished() == false);
 }
 
 void VehiclePatrolExceptionClient::dec_yaml(const char *stryamlpath)
@@ -210,7 +212,7 @@ void VehiclePatrolExceptionClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstrpatrolInterval.c_str());
-    while (true)
+    while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.h

@@ -27,7 +27,7 @@ struct msgunit
     char mstrmsgname[256];
     int mnBufferSize = 10000;
     int mnBufferCount = 1;
-    void * mpa;
+    void * mpa = nullptr;
     std::shared_ptr<char> mpstrmsgdata;
     int mndatasize = 0;
     bool mbRefresh = false;

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -54,7 +54,15 @@ DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
 
 DataExchangeClient::~DataExchangeClient(void)
 {
-
+    if(shmPicFront.mpa != nullptr)iv::modulecomm::Unregister(shmPicFront.mpa);
+    if(shmPicRear.mpa != nullptr)iv::modulecomm::Unregister(shmPicRear.mpa);
+    if(shmPicLeft.mpa != nullptr)iv::modulecomm::Unregister(shmPicLeft.mpa);
+    if(shmPicRight.mpa != nullptr)iv::modulecomm::Unregister(shmPicRight.mpa);
+    if(shmChassis.mpa != nullptr)iv::modulecomm::Unregister(shmChassis.mpa);
+    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
+    if(shmPlatformFeedback.mpa != nullptr)iv::modulecomm::Unregister(shmPlatformFeedback.mpa);
+    requestInterruption();
+    while(this->isFinished() == false);
 }
 
 void DataExchangeClient::dec_yaml(const char *stryamlpath)
@@ -629,7 +637,7 @@ void DataExchangeClient::run()
 
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstruploadInterval.c_str());
-    while (true)
+    while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -27,7 +27,7 @@ struct msgunit
     char mstrmsgname[256];
     int mnBufferSize = 10000;
     int mnBufferCount = 1;
-    void * mpa;
+    void * mpa = nullptr;
     std::shared_ptr<char> mpstrmsgdata;
     int mndatasize = 0;
     bool mbRefresh = false;

src/driver/driver_gps_hcp2/driver_gps_hcp2.pro

@@ -37,5 +37,3 @@ HEADERS += \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/imu.pb.h
-
-DISTFILES +=

src/driver/driver_radar_continental_ARS408_SRR308/can_producer_consumer.cpp

@@ -0,0 +1,107 @@
+#include "can_producer_consumer.h"
+
+CAN_Producer_Consumer::CAN_Producer_Consumer(const int bufSize)
+{
+    bufferSize = bufSize;
+    buffer.reserve(bufferSize);
+    freeSpace.release(bufferSize);
+}
+
+uint64_t CAN_Producer_Consumer::Produce_Element(const iv::can::canraw &xraw) //return the position of first element int this storage
+{
+    uint64_t tempPtr = producerPtr % bufferSize;
+    freeSpace.acquire();
+    producerLock.lock();
+    buffer[producerPtr++ % bufferSize] = xraw;
+    producerLock.unlock();
+    usedSpace.release();
+
+    return tempPtr;
+}
+
+uint64_t CAN_Producer_Consumer::Produce_Elements(const iv::can::canmsg &xmsg,const int &size)
+{
+    uint64_t tempPtr = producerPtr % bufferSize;
+    if(size < xmsg.rawmsg_size())
+    {
+        std::cout<<"Warning! the input size is less than the number of message element."<<std::endl;
+        freeSpace.acquire(size);
+        producerLock.lock();
+        for(int i=0;i<size;i++)
+        {
+            buffer[producerPtr++ % bufferSize] = xmsg.rawmsg(i);
+        }
+        producerLock.unlock();
+        usedSpace.release(size);
+    }
+    else
+    {
+        if(size > xmsg.rawmsg_size())
+            std::cout<<"Warning! the input size is more than the number of message element."<<std::endl;
+        freeSpace.acquire(xmsg.rawmsg_size());
+        producerLock.lock();
+        for(int i=0;i<xmsg.rawmsg_size();i++)
+        {
+            buffer[producerPtr++ % bufferSize] = xmsg.rawmsg(i);
+        }
+        producerLock.unlock();
+        usedSpace.release(xmsg.rawmsg_size());
+    }
+
+    return tempPtr;
+}
+
+uint64_t CAN_Producer_Consumer::Produce_Elements_From_CANMsg(const iv::can::canmsg &xmsg)
+{
+    uint64_t tempPtr = producerPtr % bufferSize;
+    freeSpace.acquire(xmsg.rawmsg_size());
+    producerLock.lock();
+    for(int i=0;i<xmsg.rawmsg_size();i++)
+    {
+        buffer[producerPtr++ % bufferSize] = xmsg.rawmsg(i);
+    }
+    producerLock.unlock();
+    usedSpace.release(xmsg.rawmsg_size());
+
+    return tempPtr;
+}
+
+iv::can::canraw CAN_Producer_Consumer::Consume_Element()
+{
+    iv::can::canraw xraw;
+    usedSpace.acquire(); //this function is block mode
+    consumerLock.lock();
+    xraw = buffer.at(consumerPtr++ % bufferSize);
+    consumerLock.unlock();
+    freeSpace.release();
+
+    return xraw;
+}
+
+iv::can::canmsg CAN_Producer_Consumer::Consume_Elements(const int &size)
+{
+    iv::can::canmsg xmsg;
+    usedSpace.acquire(size); //this function is block mode
+    consumerLock.lock();
+    for(int i=0;i<size;i++)
+    {
+        xmsg.add_rawmsg()->CopyFrom(buffer.at(consumerPtr++ % bufferSize));
+    }
+    consumerLock.unlock();
+    freeSpace.release(size);
+
+    return xmsg;
+}
+
+uint64_t CAN_Producer_Consumer::Consume_Element_To_CANMsg(iv::can::canmsg &xmsg)
+{
+    uint64_t tempPtr = consumerPtr % bufferSize;
+    usedSpace.acquire(); //this function is block mode
+    iv::can::canraw *praw = xmsg.add_rawmsg();
+    consumerLock.lock();
+    praw->CopyFrom(buffer.at(consumerPtr++ % bufferSize));
+    consumerLock.unlock();
+    freeSpace.release();
+
+    return tempPtr;
+}

src/driver/driver_radar_continental_ARS408_SRR308/can_producer_consumer.h

@@ -0,0 +1,35 @@
+#ifndef CAN_PRODUCER_CONSUMER_H
+#define CAN_PRODUCER_CONSUMER_H
+
+#include <QSemaphore>
+#include <QMutex>
+#include <QVector>
+
+#include <iostream>
+
+#include "canmsg.pb.h"
+
+class CAN_Producer_Consumer
+{
+public:
+    CAN_Producer_Consumer(const int bufSize = 4096);
+    uint64_t Produce_Element(const iv::can::canraw &xraw); //return the position of first element int this storage
+    uint64_t Produce_Elements(const iv::can::canmsg &xmsg,const int &size); //return the position of first element int this storage
+    uint64_t Produce_Elements_From_CANMsg(const iv::can::canmsg &xmsg); //return the position of first element int this storage
+    iv::can::canraw Consume_Element();
+    iv::can::canmsg Consume_Elements(const int &size);
+    uint64_t Consume_Element_To_CANMsg(iv::can::canmsg &xmsg); //return the position of the consumed element
+
+    QVector<iv::can::canraw> buffer;
+
+private:
+    uint16_t bufferSize = 4096;
+    QSemaphore freeSpace;
+    QSemaphore usedSpace;
+    uint64_t producerPtr = 0;
+    QMutex producerLock;
+    uint64_t consumerPtr = 0;
+    QMutex consumerLock;
+};
+
+#endif // CAN_PRODUCER_CONSUMER_H

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.cpp

@@ -0,0 +1,63 @@
+#include "canrecv_consumer.h"
+
+#include "decode_cfg.h"
+extern setupConfig_t setupConfig;
+
+#include "radarobjectarray.pb.h"
+
+#include "iv_msgunit.h"
+extern iv::msgunit shmRadar;
+
+CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
+{
+    pBuffer = pBuf;
+}
+
+CANRecv_Consumer::~CANRecv_Consumer()
+{
+    requestInterruption();
+    while(this->isFinished() == false);
+}
+
+uint32_t CANRecv_Consumer::Trans_From_CANRaw(const iv::can::canraw &xraw)
+{
+    CAN_ID = xraw.id();
+    CAN_DLC = xraw.len();
+    char tempData[8];
+    int tempLen = (xraw.data().size() > 8) ? 8 : xraw.data().size();
+    strncpy(tempData,xraw.data().data(),tempLen);
+    for(int i=0;i<tempLen;i++)
+    {
+        CAN_data[i] = (uint8_t)tempData[i];
+    }
+
+    return CAN_ID;
+}
+
+void CANRecv_Consumer::run()
+{
+    radarType = setupConfig.radarType;
+    ars408_can_database_ch0_rx_t ars408_can_database_ch0_rx;
+    ars408_can_database_ch1_rx_t ars408_can_database_ch1_rx;
+    ars408_can_database_ch2_rx_t ars408_can_database_ch2_rx;
+    ars408_can_database_ch3_rx_t ars408_can_database_ch3_rx;
+    ars408_can_database_ch4_rx_t ars408_can_database_ch4_rx;
+    ars408_can_database_ch5_rx_t ars408_can_database_ch5_rx;
+    ars408_can_database_ch6_rx_t ars408_can_database_ch6_rx;
+    ars408_can_database_ch7_rx_t ars408_can_database_ch7_rx;
+
+    iv::can::canraw xraw;
+
+    while (!QThread::isInterruptionRequested())
+    {
+        switch (setupConfig.radarID) {
+        case 0:
+            xraw = pBuffer->Consume_Element();
+            this->Trans_From_CANRaw(xraw);
+            ars408_can_database_ch0_Receive(&ars408_can_database_ch0_rx,CAN_data,CAN_ID,CAN_DLC);
+            break;
+        default:
+            break;
+        }
+    }
+}

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.h

@@ -0,0 +1,34 @@
+#ifndef CANRECV_CONSUMER_H
+#define CANRECV_CONSUMER_H
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QThread>
+
+#include "modulecomm.h"
+#include "can_producer_consumer.h"
+
+class CANRecv_Consumer : public QThread
+{
+    Q_OBJECT
+public:
+    CANRecv_Consumer(CAN_Producer_Consumer *pBuf);
+    ~CANRecv_Consumer();
+
+    uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw);
+
+protected:
+    void run();
+
+private:
+    CAN_Producer_Consumer *pBuffer;
+
+    uint8_t radarType = 0; //0 ARS408-21xx 1 ARS408-21sc3 2 SRR308
+
+    uint32_t CAN_ID = 0x000u;
+    uint8_t CAN_DLC = 0;
+    uint8_t CAN_data[8] = {0};
+};
+
+#endif // CANRECV_CONSUMER_H

src/driver/driver_radar_continental_ARS408_SRR308/cansend_consumer.cpp

@@ -0,0 +1,72 @@
+#include "cansend_consumer.h"
+
+#include "decode_cfg.h"
+extern setupConfig_t setupConfig;
+
+#include "iv_msgunit.h"
+extern iv::msgunit shmCANSend;
+
+CANSend_Consumer::CANSend_Consumer(CAN_Producer_Consumer *pBuf)
+{
+    pBuffer = pBuf;
+}
+
+CANSend_Consumer::~CANSend_Consumer()
+{
+    requestInterruption();
+    while(this->isFinished() == false);
+}
+
+void CANSend_Consumer::run()
+{
+    QTime xTime;
+    xTime.start();
+
+    int lastTime = xTime.elapsed();
+    uint64_t interval = 20; //interval time should more than 10 ms
+
+    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); //this function is block mode
+
+        if(abs(xTime.elapsed() - lastTime)>=interval || xmsg.rawmsg_size() > 3500)
+        {
+            //update timer
+            lastTime = xTime.elapsed();
+            if(xmsg.rawmsg_size()>0) //actrully, because the fucntion before this is block mode, this determine is allways true, just for safety
+            {
+                //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++;
+            }
+            else
+                std::cout << "rawmsg size is zero, no data need to be send." << std::endl;
+        }
+    }
+}

src/driver/driver_radar_continental_ARS408_SRR308/cansend_consumer.h

@@ -0,0 +1,26 @@
+#ifndef CANSEND_CONSUMER_H
+#define CANSEND_CONSUMER_H
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QThread>
+
+#include "modulecomm.h"
+#include "can_producer_consumer.h"
+
+class CANSend_Consumer : public QThread
+{
+    Q_OBJECT
+public:
+    CANSend_Consumer(CAN_Producer_Consumer *pBuf);
+    ~CANSend_Consumer();
+
+protected:
+    void run();
+
+private:
+    CAN_Producer_Consumer *pBuffer;
+};
+
+#endif // CANSEND_CONSUMER_H

src/driver/driver_radar_continental_ARS408_SRR308/cansend_producer.cpp

@@ -0,0 +1,202 @@
+#include "cansend_producer.h"
+
+#include "math.h"
+
+#include "decode_cfg.h"
+extern setupConfig_t setupConfig;
+
+extern uint8_t chassisShift;
+extern double chassisVelocity;
+extern QMutex chassisDataLock;
+
+extern double GPSIMUVelocity; // m/s
+extern double GPSIMUYawRate; // deg/s
+extern QMutex GPSIMUDataLock;
+
+CANSend_Producer::CANSend_Producer(CAN_Producer_Consumer *pBuf)
+{
+    pBuffer = pBuf;
+}
+
+CANSend_Producer::~CANSend_Producer()
+{
+    requestInterruption();
+    while(this->isFinished() == false);
+}
+
+uint32_t CANSend_Producer::Trans_to_CANRaw(iv::can::canraw &xraw)
+{
+    xraw.set_id(CAN_ID);
+    xraw.set_bext(CAN_IDE);
+    xraw.set_bremote(CAN_RTR);
+    xraw.set_len(CAN_DLC);
+    xraw.set_data(CAN_data,CAN_DLC);
+    xraw.set_rectime(QDateTime::currentMSecsSinceEpoch());
+}
+
+void CANSend_Producer::run()
+{
+    QTime xTime;
+    xTime.start();
+
+    SpeedInformation_ch0_t SpeedInformation_ch0;
+    YawRateInformation_ch0_t YawRateInformation_ch0;
+
+    SpeedInformation_ch1_t SpeedInformation_ch1;
+    YawRateInformation_ch1_t YawRateInformation_ch1;
+
+    SpeedInformation_ch2_t SpeedInformation_ch2;
+    YawRateInformation_ch2_t YawRateInformation_ch2;
+
+    SpeedInformation_ch3_t SpeedInformation_ch3;
+    YawRateInformation_ch3_t YawRateInformation_ch3;
+
+    SpeedInformation_ch4_t SpeedInformation_ch4;
+    YawRateInformation_ch4_t YawRateInformation_ch4;
+
+    SpeedInformation_ch5_t SpeedInformation_ch5;
+    YawRateInformation_ch5_t YawRateInformation_ch5;
+
+    SpeedInformation_ch6_t SpeedInformation_ch6;
+    YawRateInformation_ch6_t YawRateInformation_ch6;
+
+    SpeedInformation_ch7_t SpeedInformation_ch7;
+    YawRateInformation_ch7_t YawRateInformation_ch7;
+
+    iv::can::canraw xraw;
+
+    int lastTime = xTime.elapsed();
+    uint64_t interval = 20; //interval time should more than 10 ms
+
+    while (!QThread::isInterruptionRequested())
+    {
+        if(abs(xTime.elapsed() - lastTime)>=interval)
+        {
+            //update timer
+            lastTime = xTime.elapsed();
+
+            chassisDataLock.lock();
+            chasShift = chassisShift; //1P 2R 3N 4D
+            chasVelocity = chassisVelocity;
+            chassisDataLock.unlock();
+
+            GPSIMUDataLock.lock();
+            gpsVelocity = GPSIMUVelocity;
+            gpsYawRate = GPSIMUYawRate;
+            GPSIMUDataLock.unlock();
+
+            if(chasShift == 4)
+                speedDirection = 1; //0 stand 1 forward 2 reverse 3 reserved
+            else if(chasShift == 2)
+                speedDirection = 2;
+            else
+                speedDirection = 0;
+
+            if(fabs(chasVelocity - gpsVelocity) > 1.5)
+                speedFinal = gpsVelocity;
+            else
+                speedFinal = chasVelocity;
+
+            switch (setupConfig.radarID) {
+            case 0:
+                SpeedInformation_ch0.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch0.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch0_ARS408_can_database_ch0(&SpeedInformation_ch0,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch0.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch0_ARS408_can_database_ch0(&YawRateInformation_ch0,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 1:
+                SpeedInformation_ch1.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch1.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch1_ARS408_can_database_ch1(&SpeedInformation_ch1,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch1.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch1_ARS408_can_database_ch1(&YawRateInformation_ch1,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 2:
+                SpeedInformation_ch2.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch2.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch2_ARS408_can_database_ch2(&SpeedInformation_ch2,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch2.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch2_ARS408_can_database_ch2(&YawRateInformation_ch2,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 3:
+                SpeedInformation_ch3.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch3.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch3_ARS408_can_database_ch3(&SpeedInformation_ch3,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch3.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch3_ARS408_can_database_ch3(&YawRateInformation_ch3,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 4:
+                SpeedInformation_ch4.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch4.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch4_ARS408_can_database_ch4(&SpeedInformation_ch4,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch4.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch4_ARS408_can_database_ch4(&YawRateInformation_ch4,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 5:
+                SpeedInformation_ch5.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch5.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch5_ARS408_can_database_ch5(&SpeedInformation_ch5,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch5.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch5_ARS408_can_database_ch5(&YawRateInformation_ch5,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 6:
+                SpeedInformation_ch6.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch6.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch6_ARS408_can_database_ch6(&SpeedInformation_ch6,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch6.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch6_ARS408_can_database_ch6(&YawRateInformation_ch6,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            case 7:
+                SpeedInformation_ch7.RadarDevice_SpeedDirection = speedDirection;
+                SpeedInformation_ch7.RadarDevice_Speed_phys = speedFinal;
+                CAN_ID = Pack_SpeedInformation_ch7_ARS408_can_database_ch7(&SpeedInformation_ch7,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+
+                YawRateInformation_ch7.RadarDevice_YawRate_phys = gpsYawRate;
+                CAN_ID = Pack_YawRateInformation_ch7_ARS408_can_database_ch7(&YawRateInformation_ch7,CAN_data,&CAN_DLC,&CAN_IDE);
+                this->Trans_to_CANRaw(xraw);
+                pBuffer->Produce_Element(xraw);
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}

src/driver/driver_radar_continental_ARS408_SRR308/cansend_producer.h

@@ -0,0 +1,42 @@
+#ifndef CANSEND_PRODUCER_H
+#define CANSEND_PRODUCER_H
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QThread>
+
+#include "modulecomm.h"
+#include "can_producer_consumer.h"
+
+class CANSend_Producer : public QThread
+{
+    Q_OBJECT
+public:
+    CANSend_Producer(CAN_Producer_Consumer *pBuf);
+    ~CANSend_Producer();
+
+    uint32_t Trans_to_CANRaw(iv::can::canraw &xraw); //return CAN_ID
+
+protected:
+    void run();
+
+private:
+    CAN_Producer_Consumer *pBuffer;
+
+    uint8_t chasShift;
+    double chasVelocity;
+    double gpsVelocity;
+    double gpsYawRate;
+
+    uint8_t speedDirection = 0;
+    double speedFinal = 0.0;
+
+    uint32_t CAN_ID = 0x000u;
+    uint8_t CAN_IDE = 0;
+    uint8_t CAN_RTR = 0;
+    uint8_t CAN_DLC = 0;
+    uint8_t CAN_data[8] = {0};
+};
+
+#endif // CANSEND_PRODUCER_H

src/driver/driver_radar_continental_ARS408_SRR308/decode_cfg.cpp

@@ -1,6 +1,6 @@
 #include "decode_cfg.h"
 
-int decode_setup_cfg_from_xml(setupCfg &setupConfig , std::string xmlFilePath)
+int decode_setup_cfg_from_xml(setupConfig_t &setupConfig , std::string xmlFilePath)
 {
     iv::xmlparam::Xmlparam xp(xmlFilePath);
 
@@ -10,6 +10,7 @@ int decode_setup_cfg_from_xml(setupCfg &setupConfig , std::string xmlFilePath)
     setupConfig.strMemGPSIMU = xp.GetParam("gpsimu_shm","hcp2_gpsimu");
     setupConfig.strMemChassis = xp.GetParam("chassis_shm","chassis");
     setupConfig.radarType = xp.GetParam("Radar_Type",0);
+    setupConfig.radarID = xp.GetParam("Radar_ID",0);
     setupConfig.radarAzimuth = xp.GetParam("Radar_Azimuth",0.0);
     setupConfig.radarOffsetX = xp.GetParam("Radar_Offset_X",0.0);
     setupConfig.radarOffsetY = xp.GetParam("Radar_Offset_Y",0.0);
@@ -30,6 +31,7 @@ int decode_setup_cfg_from_xml(setupCfg &setupConfig , std::string xmlFilePath)
 //    std::cout << setupConfig.strMemIMU << std::endl;
 //    std::cout << setupConfig.strMemChassis << std::endl;
 //    std::cout << int(setupConfig.radarType) << std::endl;
+//    std::cout << int(setupConfig.radraID) << std::endl;
 //    std::cout << setupConfig.radarAzimuth << std::endl;
 //    std::cout << setupConfig.radarOffsetX << std::endl;
 //    std::cout << setupConfig.radarOffsetY << std::endl;

src/driver/driver_radar_continental_ARS408_SRR308/decode_cfg.h

@@ -15,7 +15,7 @@
 #include "CAN_database_code/ars408_can_database_ch6-binutil.h"
 #include "CAN_database_code/ars408_can_database_ch7-binutil.h"
 
-struct setupCfg
+struct setupConfig_t
 {
     std::string strMemCANRecv = "canrecv0";
     std::string strMemCANSend = "cansend0";
@@ -23,6 +23,7 @@ struct setupCfg
     std::string strMemGPSIMU = "hcp2_gpsimu";
     std::string strMemChassis = "chassis";
     int8_t radarType = 0;
+    int8_t radarID = 0;
     double radarAzimuth = 0.0;
     double radarOffsetX = 0.0;
     double radarOffsetY = 0.0;
@@ -38,7 +39,7 @@ struct setupCfg
     std::string collDetRegionCfgYAML_Path = "CollDetRegion_Cfg.yaml";
 };
 
-int decode_setup_cfg_from_xml(setupCfg &setupConfig , std::string xmlFilePath);
+int decode_setup_cfg_from_xml(setupConfig_t &setupConfig , std::string xmlFilePath);
 int decode_radar_cfg_from_yaml(RadarConfiguration_ch0_t &radarConfiguration , const std::string &yamlFilePath);
 
 #endif // DECODE_CFG_H

src/driver/driver_radar_continental_ARS408_SRR308/driver_radar_continental_ARS408_SRR308.pro

@@ -61,7 +61,12 @@ HEADERS += \
     CAN_database_code/ars408_can_database_ch7.h \
     CAN_database_code/canmonitorutil.h \
     CAN_database_code/dbccodeconf.h \
-    decode_cfg.h
+    can_producer_consumer.h \
+    canrecv_consumer.h \
+    cansend_consumer.h \
+    cansend_producer.h \
+    decode_cfg.h \
+    iv_msgunit.h
 
 SOURCES += main.cpp \
     ../../include/msgtype/radarobject.pb.cc \
@@ -94,6 +99,10 @@ SOURCES += main.cpp \
     CAN_database_code/ars408_can_database_ch7-binutil.c \
     CAN_database_code/ars408_can_database_ch7-fmon.c \
     CAN_database_code/ars408_can_database_ch7.c \
+    can_producer_consumer.cpp \
+    canrecv_consumer.cpp \
+    cansend_consumer.cpp \
+    cansend_producer.cpp \
     decode_cfg.cpp
 
 # Default rules for deployment.

src/driver/driver_radar_continental_ARS408_SRR308/driver_radar_continental_ARS408_SRR308.xml

@@ -6,10 +6,11 @@
 		<param name="gpsimu_shm" value="hcp2_gpsimu" />
 		<param name="chassis_shm" value="chassis" />
 		<param name="Radar_Type" value="0" /> <!-- 0:ARS408-21XX/1:ARS408-21SC3/2:SRR308-21 -->
+		<param name="Radar_ID" value="0" /> <!-- 0-7 -->
 		<param name="Radar_Azimuth" value="0.0" /> <!-- degrees from head of car to normal vector of radar emitted surface, ccw is positive -->
 		<param name="Radar_Offset_X" value="0.0" /> <!-- the position of radar base on car coordinate system, RFU is XYZ positive -->
 		<param name="Radar_Offset_Y" value="0.0" /> <!-- the position of radar base on car coordinate system, RFU is XYZ positive -->
-		<param name="Radar_Cfg_Apply" value="1" /> <!-- true for sending can message which setting radar -->
+		<param name="Radar_Cfg_Apply" value="0" /> <!-- true for sending can message which setting radar -->
 		<param name="RadarCfg_YAML_Path" value="Radar_Cfg.yaml" /> <!-- setting file path -->
 		<param name="Filter_Cfg_Apply" value="0" /> <!-- true for sending can message which setting filter -->
 		<param name="FilterCfg_YAML_Path" value="Filter_Cfg.yaml" /> <!-- setting file path -->

src/driver/driver_radar_continental_ARS408_SRR308/iv_msgunit.h

@@ -0,0 +1,19 @@
+#ifndef IV_MSGUNIT_H
+#define IV_MSGUNIT_H
+
+namespace iv {
+struct msgunit
+{
+    char mstrmsgname[256];
+    int mnBufferSize = 10000;
+    int mnBufferCount = 1;
+    void * mpa = nullptr;
+    std::shared_ptr<char> mpstrmsgdata;
+    int mndatasize = 0;
+    bool mbRefresh = false;
+    bool mbImportant = false;
+    int mnkeeptime = 100;
+};
+}
+
+#endif // IV_MSGUNIT_H

src/driver/driver_radar_continental_ARS408_SRR308/main.cpp

@@ -1,12 +1,9 @@
 #include <QCoreApplication>
 
 #include <QDateTime>
-#include <QTimer>
-#include <QThread>
-#include <QSemaphore>
+#include <QMutex>
 
 #include <iostream>
-#include <thread>
 
 #include "modulecomm.h"
 #include "ivversion.h"
@@ -21,33 +18,24 @@
 #include "gpsimu.pb.h"
 
 #include "decode_cfg.h"
+#include "can_producer_consumer.h"
+#include "canrecv_consumer.h"
+#include "cansend_consumer.h"
+#include "cansend_producer.h"
 
-#ifndef IV_MSGUNIT
-#define IV_MSGUNIT
+#include "iv_msgunit.h"
 
-namespace iv {
-struct msgunit
-{
-    char mstrmsgname[256];
-    int mnBufferSize = 10000;
-    int mnBufferCount = 1;
-    void * mpa = nullptr;
-    std::shared_ptr<char> mpstrmsgdata;
-    int mndatasize = 0;
-    bool mbRefresh = false;
-    bool mbImportant = false;
-    int mnkeeptime = 100;
-};
-}
-
-#endif
+/*
+ * Notice : the radar config setting function is not finished
+ * Notice : the number of object is less than 64. so this version is used for object mode.
+*/
 
 QCoreApplication * gApp;
 
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
 
-setupCfg setupConfig;
+setupConfig_t setupConfig;
 
 iv::msgunit shmCANRecv;
 iv::msgunit shmCANSend;
@@ -55,19 +43,19 @@ iv::msgunit shmRadar;
 iv::msgunit shmGPSIMU;
 iv::msgunit shmChassis;
 
-const uint16_t CANRecvBufferSize = 4096;
-iv::can::canraw CANRecvBuffer[CANRecvBufferSize];
-QSemaphore CANRecvFreeSpace(CANRecvBufferSize);
-QSemaphore CANRecvUsedSpace(0);
-uint64_t CANRecvProducerPtr = 0;
-uint64_t CANRecvConsumerPtr = 0;
+CAN_Producer_Consumer CANRecv(4096);
+CAN_Producer_Consumer CANSend(4096);
+
+uint8_t chassisShift = 0; //1P 2R 3N 4D
+double chassisVelocity = 0; //m/s
+QMutex chassisDataLock;
 
-const uint16_t CANSendBufferSize = 4096;
-iv::can::canraw CANSendBuffer[CANSendBufferSize];
-QSemaphore CANSendFreeSpace(CANSendBufferSize);
-QSemaphore CANSendUsedSpace(0);
-uint64_t CANSendProducerPtr = 0;
-uint64_t CANSendConsumerPtr = 0;
+double GPSIMUVelocity = 0; // m/s
+double GPSIMUYawRate = 0; // deg/s
+QMutex GPSIMUDataLock;
+
+
+//------------------------------------
 
 iv::radar::radarobjectarray gobj;
 int gntemp = 0;
@@ -198,7 +186,6 @@ void DecodeRadar(iv::can::canmsg xmsgvetor)
 
 void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-
     if(nSize<1)
     {
         std::cout<<"radar ListenCANMsg data empty."<<std::endl;
@@ -213,15 +200,7 @@ void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
 
-    CANRecvFreeSpace.acquire(xmsg.rawmsg_size());
-    for(int i=0;i<xmsg.rawmsg_size();i++)
-    {
-        CANRecvBuffer[CANRecvProducerPtr++ % CANRecvBufferSize] = xmsg.rawmsg(i);
-    }
-    CANRecvUsedSpace.release(xmsg.rawmsg_size());
-
-//    DecodeRadar(xmsg);
-
+    CANRecv.Produce_Elements_From_CANMsg(xmsg);
 }
 
 void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -237,6 +216,10 @@ void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigne
         std::cout<<" radar ListenGPSIMUMsg parse fail."<<std::endl;
         return;
     }
+    GPSIMUDataLock.lock();
+    GPSIMUVelocity = xdata.speed(); //m/s
+    GPSIMUYawRate = xdata.gyro_z(); //deg/s
+    GPSIMUDataLock.unlock();
 }
 
 void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -252,6 +235,10 @@ void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsign
         std::cout<<" radar ListenChassisMsg parse fail."<<std::endl;
         return;
     }
+    chassisDataLock.lock();
+    chassisShift = xdata.shift(); //1P 2R 3N 4D
+    chassisVelocity = xdata.vel()/3.6; //km/h to m/s
+    chassisDataLock.unlock();
 }
 
 void ExitFunc()
@@ -276,10 +263,10 @@ int main(int argc, char *argv[])
 
     gApp = &a;
 
-    gfault = new iv::Ivfault("driver_radar_continental_ARS408_SRR308");
     givlog = new iv::Ivlog("driver_radar_continental_ARS408_SRR308");
+    gfault = new iv::Ivfault("driver_radar_continental_ARS408_SRR308");
 
-    iv::ivexit::RegIVExitCall(ExitFunc);
+//    iv::ivexit::RegIVExitCall(ExitFunc);
     signal(SIGINT,signal_handler);
 
     QString strpath = QCoreApplication::applicationDirPath();
@@ -324,6 +311,11 @@ int main(int argc, char *argv[])
     shmChassis.mnBufferCount = 3;
     shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisMsg);
 
+//    CANSend_Producer can_send_producer(&CANSend);
+//    can_send_producer.start();
+//    CANSend_Consumer can_send_consumer(&CANSend);
+//    can_send_consumer.start();
+
     int rtn = a.exec();
 
     if(gfault != nullptr)delete gfault;

src/include/proto/chassis.proto

@@ -8,7 +8,7 @@ message chassis
   optional int32 EPSMode = 2  [default = 0]; //0 idle 1 Manual 2 Auto
   optional int32 EPBFault = 3 [default = 0];  //0 No 1 Have Fault
   optional int32 DriveMode = 4;      //0 Manual 1 Auto
-  optional int32 Shift = 5;  //0 N  1 D   2 R  3 P
+  optional int32 Shift = 5;  //0 N  1 D   2 R  3 P  hapo:1p2r3n4d
   optional int32 AEBAvailable = 6;
   optional int32 CDDAvailable = 7;
   optional int32 angle_feedback = 8;