fix(driver_sonar_ks136A):change distance limit to result by test. but find an mistake that can message sending is not real time.

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -4,6 +4,7 @@
 
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmSonar;
+extern uint64_t gTime_ms;
 
 CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
 {
@@ -13,6 +14,8 @@ CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
 
     objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
     sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
+    objDist_Send.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
+    sensorStatus_Send.fill(false,NUM_OF_SENSOR_MAX);
 }
 
 CANRecv_Consumer::~CANRecv_Consumer()
@@ -48,6 +51,8 @@ void CANRecv_Consumer::run()
 {
     iv::can::canraw tempCANRaw;
     uint8_t tempSensorID = 0;
+    uint32_t tempLastResult = DIST_ERROR;
+    uint16_t tempDist;
     decodeTimer.start();
 
     while (!QThread::isInterruptionRequested())
@@ -55,12 +60,26 @@ void CANRecv_Consumer::run()
         tempCANRaw.CopyFrom(pBuffer->Consume_Element(1));
         if(decodeEnableFlag == true)
         {
-            if(decodeTimer.elapsed() >= 25)
+            if(decodeTimer.elapsed() >= 50)
             {
                 decodeEnableFlag = false;
                 objDist[decodeSensorID] = DIST_ERROR;
                 sensorStatus[decodeSensorID] = false;
                 tempSensorID = decodeSensorID + 1;
+
+                if(tempSensorID >= NUM_OF_SENSOR_MAX)
+                {
+                    sonarSend_Lock.lock();
+                    tempLastResult =  objDist_Send[0];
+                    sonarSend_Lock.unlock();
+                }
+                else
+                {
+                    sonarSend_Lock.lock();
+                    tempLastResult =  objDist_Send[tempSensorID];
+                    sonarSend_Lock.unlock();
+                }
+
                 if(tempSensorID >= NUM_OF_SENSOR_MAX)
                 {
                     //copy and send
@@ -76,21 +95,41 @@ void CANRecv_Consumer::run()
                     sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
                     tempSensorID = 0;
                 }
-                emit Enable_Ask(true,tempSensorID);
+                if(tempLastResult <= 2000)
+                    emit Enable_Ask(true,tempSensorID,2);
+                else if(tempLastResult <= 3800)
+                    emit Enable_Ask(true,tempSensorID,1);
+                else
+                    emit Enable_Ask(true,tempSensorID,0);
                 decodeTimer.restart();
+                gTime_ms = QDateTime::currentMSecsSinceEpoch();
             }
             else
             {
                 this->Trans_From_CANRaw(tempCANRaw);
                 if(CAN_ID == setupConfig.sonarCAN_ID && CAN_DLC == 2)
                 {
-                    uint16_t tempDist = (CAN_data[0] << 8) | CAN_data[1];
-                    if(tempDist <= 0x1493 && tempDist >= 0x88)
+                    tempDist = (CAN_data[0] << 8) | CAN_data[1];
+                    if(tempDist <= 0x1650 && tempDist >= 0xD0)
                     {
                         decodeEnableFlag = false;
                         objDist[decodeSensorID] = tempDist;
                         sensorStatus[decodeSensorID] = true;
                         tempSensorID = decodeSensorID + 1;
+
+                        if(tempSensorID >= NUM_OF_SENSOR_MAX)
+                        {
+                            sonarSend_Lock.lock();
+                            tempLastResult =  objDist_Send[0];
+                            sonarSend_Lock.unlock();
+                        }
+                        else
+                        {
+                            sonarSend_Lock.lock();
+                            tempLastResult =  objDist_Send[tempSensorID];
+                            sonarSend_Lock.unlock();
+                        }
+
                         if(tempSensorID >= NUM_OF_SENSOR_MAX)
                         {
                             //copy and send
@@ -106,8 +145,14 @@ void CANRecv_Consumer::run()
                             sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
                             tempSensorID = 0;
                         }
-                        emit Enable_Ask(true,tempSensorID);
+                        if(tempLastResult <= 2000)
+                            emit Enable_Ask(true,tempSensorID,2);
+                        else if(tempLastResult <= 3800)
+                            emit Enable_Ask(true,tempSensorID,1);
+                        else
+                            emit Enable_Ask(true,tempSensorID,0);
                         decodeTimer.restart();
+                        gTime_ms = QDateTime::currentMSecsSinceEpoch();
                     }
                 }
             }
@@ -125,6 +170,9 @@ void CANRecv_Consumer::Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID
 void CANRecv_Consumer::UltrasonicData_Ready_Slot(void)
 {
     iv::ultrasonic::ultrasonic xmsg;
+    uint64_t tempLastTime = QDateTime::currentMSecsSinceEpoch();
+    std::cout<<"interval time is "<<tempLastTime - gLastTime<<std::endl;
+    gLastTime = tempLastTime;
     sonarSend_Lock.lock();
     xmsg.set_sigobjdist_flside(objDist_Send[0]);
     xmsg.set_sigobjdist_flcorner(objDist_Send[1]);

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.h

@@ -37,7 +37,7 @@ protected:
     uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw); //only for standard data frame, return can id
 
 signals:
-    void Enable_Ask(bool enableFlag,uint8_t sensorID);
+    void Enable_Ask(bool enableFlag,uint8_t sensorID,uint8_t distanceType);
     void UltrasonicData_Ready(void);
 
 private slots:
@@ -60,6 +60,8 @@ private:
     QVector<uint32_t> objDist_Send;
     QVector<bool> sensorStatus_Send;
     QMutex sonarSend_Lock;
+
+    uint64_t gLastTime = QDateTime::currentMSecsSinceEpoch();
 };
 
 #endif // CANRECV_CONSUMER_H

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_consumer.cpp

@@ -4,6 +4,7 @@
 
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmCANSend;
+extern uint64_t gTime_ms;
 
 CANSend_Consumer::CANSend_Consumer(CAN_Producer_Consumer *pBuf)
 {
@@ -52,6 +53,7 @@ void CANSend_Consumer::run()
             //clear and update index
             xmsg.clear_rawmsg();
             CANPackageIndex++;
+            std::cout<<"send consumer time :"<<(QDateTime::currentMSecsSinceEpoch() - gTime_ms)<<std::endl;
         }
     }
 }

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.cpp

@@ -54,7 +54,13 @@ void CANSend_Producer::run()
             CAN_DLC = 3;
             if(askSensorID < NUM_OF_SENSOR_MAX)
             {
-                CAN_data[2] =  askSensorID * 8 + 0x16;
+                if(askDistanceType == 2)
+                    CAN_data[2] =  askSensorID * 8 + 0x14; // 0.2-3m
+                else if(askDistanceType == 1)
+                    CAN_data[2] =  askSensorID * 8 + 0x10; // 0.2-4.5m
+                else
+                    CAN_data[2] =  askSensorID * 8 + 0x16; // 0.3-5.5m
+
                 pBuffer->Produce_Element(this->Trans_to_CANRaw());
                 emit Enable_DecodeResult(true,askSensorID);
             }
@@ -68,8 +74,9 @@ void CANSend_Producer::run()
     }
 }
 
-void CANSend_Producer::Enable_Ask_Slot(bool enableFlag , uint8_t sensorID)
+void CANSend_Producer::Enable_Ask_Slot(bool enableFlag , uint8_t sensorID, uint8_t distanceType)
 {
     askEnableFlag = enableFlag;
     askSensorID = sensorID;
+    askDistanceType = distanceType;
 }

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.h

@@ -24,7 +24,7 @@ public:
     void Clear_CAN_PrivateTempVariable(void);
 
 public slots:
-    void Enable_Ask_Slot(bool enableFlag,uint8_t sensorID);
+    void Enable_Ask_Slot(bool enableFlag,uint8_t sensorID,uint8_t distanceType);
 
 protected:
     void run();
@@ -41,6 +41,7 @@ private:
 
     bool askEnableFlag = true;
     uint8_t askSensorID = 0;
+    uint8_t askDistanceType = 0; //0 0.3-5.5m 1 0.2-4.5m 2 0.2-3m
 
 signals:
     void Enable_DecodeResult(bool enableFlag,uint8_t sensorID);

src/driver/driver_ultrasonic_dauxi_KS136A/main.cpp

@@ -41,6 +41,8 @@ CANSend_Producer* can_send_producer;
 CANSend_Consumer* can_send_consumer;
 CANRecv_Consumer* can_recv_consumer;
 
+uint64_t gTime_ms = QDateTime::currentMSecsSinceEpoch();
+
 void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     if(nSize<1)