ultra modify

src/detection/detection_ultrasonic/detection_ultrasonic.pro

@@ -0,0 +1,37 @@
+QT -= gui
+
+QT += dbus
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+QMAKE_LFLAGS += -no-pie
+HEADERS += \
+    ../../include/msgtype/ultrasonic.pb.h \
+    ../../include/msgtype/ultraarea.pb.h
+
+SOURCES += main.cpp \
+    ../../include/msgtype/ultrasonic.pb.cc \
+    ../../include/msgtype/ultraarea.pb.cc
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+
+LIBS += -lprotobuf
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
+
+
+

src/detection/detection_ultrasonic/main.cpp

@@ -0,0 +1,199 @@
+#include <QCoreApplication>
+
+#include <iostream>
+#include <QDateTime>
+#include <math.h>
+
+#include <thread>
+#include <QElapsedTimer>
+#include "modulecomm.h"
+#include "xmlparam.h"
+#include "ivlog.h"
+#include "ivfault.h"
+
+iv::Ivlog * givlog;
+iv::Ivfault * givfault;
+
+#include "ultrasonic.pb.h"
+#include "ultraarea.pb.h"
+
+iv::ultrasonic::ultrasonic gobj;
+
+void * gpa , * gpb;
+
+QElapsedTimer gTime;
+int min(int ids[],int len)
+{
+    int i = 1;
+    int* p = &ids[0];
+    for(; i < len; i++)
+    {
+        if (*p>ids[i]){
+            p = &ids[i];
+        }
+   }
+   return *p;
+}
+
+void ProcessData(iv::ultrasonic::ultrasonic xmsg)
+{
+        std::cout<<xmsg.sigobjdist_flside()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flcorner()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flmiddle()<<" ";//1
+        std::cout<<xmsg.sigobjdist_frmiddle()<<" ";//1
+        std::cout<<xmsg.sigobjdist_frcorner()<<" ";//2
+        std::cout<<xmsg.sigobjdist_frside()<<" ";//2
+        std::cout<<xmsg.sigobjdist_rrside()<<" ";//2
+        std::cout<<xmsg.sigobjdist_rrcorner()<<" ";//2
+        std::cout<<xmsg.sigobjdist_rrmiddle()<<" ";//3
+        std::cout<<xmsg.sigobjdist_rlmiddle()<<" ";//3
+        std::cout<<xmsg.sigobjdist_rlcorner()<<" ";//4
+        std::cout<<xmsg.sigobjdist_rlside()<<std::endl;//4
+    iv::ultrasonic::ultraarea areas;
+    areas.set_timestamp(xmsg.timestamp());
+
+    if(xmsg.sigsensor_front_lm()==0 && xmsg.sigsensor_front_rm()==0){
+        iv::ultrasonic::Area area;
+        area.set_id(1);
+        area.set_valid(false);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }else{
+        int tmp[2];
+        tmp[0] = xmsg.sigobjdist_flmiddle();
+        tmp[1] = xmsg.sigobjdist_frmiddle();
+        int min_dist = min(tmp,2);
+        iv::ultrasonic::Area area;
+        area.set_id(1);
+        area.set_dist(min_dist);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }
+    if(xmsg.sigsensor_front_rc()==0 && xmsg.sigsensor_front_rs()==0 && xmsg.sigsensor_rear_rs()==0 && xmsg.sigsensor_rear_rc()==0){
+        iv::ultrasonic::Area area;
+        area.set_id(2);
+        area.set_valid(false);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }else{
+        int tmp[4];
+        tmp[0] = xmsg.sigobjdist_frcorner();
+        tmp[1] = xmsg.sigobjdist_frside();
+        tmp[2] = xmsg.sigobjdist_rrside();
+        tmp[3] = xmsg.sigobjdist_rrcorner();
+        int min_dist = min(tmp,4);
+        iv::ultrasonic::Area area;
+        area.set_id(2);
+        area.set_dist(min_dist);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }
+    if(xmsg.sigsensor_rear_rm()==0 && xmsg.sigsensor_rear_lm()==0){
+        iv::ultrasonic::Area area;
+        area.set_id(3);
+        area.set_valid(false);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }else{
+        int tmp[2];
+        tmp[0] = xmsg.sigobjdist_rrmiddle();
+        tmp[1] = xmsg.sigobjdist_rlmiddle();
+        int min_dist = min(tmp,2);
+        iv::ultrasonic::Area area;
+        area.set_id(3);
+        area.set_dist(min_dist);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }
+    if(xmsg.sigsensor_rear_lc()==0 && xmsg.sigsensor_rear_ls()==0 && xmsg.sigsensor_front_ls()==0 && xmsg.sigsensor_front_lc()==0){
+        iv::ultrasonic::Area area;
+        area.set_id(4);
+        area.set_valid(false);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }else{
+        int tmp[4];
+        tmp[0] = xmsg.sigobjdist_rlcorner();
+        tmp[1] = xmsg.sigobjdist_rlside();
+        tmp[2] = xmsg.sigobjdist_flside();
+        tmp[3] = xmsg.sigobjdist_flcorner();
+        int min_dist = min(tmp,4);
+        iv::ultrasonic::Area area;
+        area.set_id(4);
+        area.set_dist(min_dist);
+        iv::ultrasonic::Area * p = areas.add_area();
+        p->CopyFrom(area);
+    }
+
+    std::cout<<"---------- "<<areas.area_size()<<" ---------"<<std::endl;
+    for(int i=0;i<areas.area_size();i++){
+        iv::ultrasonic::Area area = areas.area(i);
+        std::cout<<area.id()<<": "<<area.dist()<<" "<<area.valid()<<std::endl;
+    }
+    std::string out = areas.SerializeAsString();
+    iv::modulecomm::ModuleSendMsg(gpb,out.data(),out.length());
+}
+
+void ListenUltra(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1) return;
+    iv::ultrasonic::ultrasonic xmsg;
+
+//    givlog->verbose("vbox","rec rsu data");
+    if(false == xmsg.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Listenultra fail."<<std::endl;
+        return;
+    }
+    ProcessData(xmsg);
+//    qDebug("can size is %d",xmsg.rawmsg_size());
+//    xt = QDateTime::currentMSecsSinceEpoch();
+//    qDebug("latence = %ld ",xt-pic.time());
+
+}
+
+void threadstate()
+{
+    while(1)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+//        if(gnRadarState > -100)gnRadarState--;
+//        if(gnRadarState > 0)
+//        {
+//            givfault->SetFaultState(0,0,"OK");
+//        }
+//        else
+//        {
+//            if(gnRadarState > -100)
+//            {
+//                givfault->SetFaultState(1,1,"无CAN数据");
+//            }
+//            else
+//            {
+//                givfault->SetFaultState(2,2,"无CAN数据");
+//            }
+//        }
+
+    }
+}
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    std::string strmemultra = "sonar";
+    std::string strmemsend = "ultra_output";
+
+    givlog = new iv::Ivlog(strmemultra.data());
+    givfault = new iv::Ivfault(strmemultra.data());
+
+    givfault->SetFaultState(1,1,"初始化");
+    givlog->info("ultra","Initialized");
+
+    gpb = iv::modulecomm::RegisterSend(strmemsend.data(),100000,1);
+    iv::modulecomm::RegisterRecv(strmemultra.data(), ListenUltra);
+
+    std::thread threadfault(threadstate);
+
+
+    return a.exec();
+}

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -157,6 +157,8 @@ void CANRecv_Consumer::run()
     }
 }
 
+#define RESULT_OUTPUT_ENABLE
+
 void CANRecv_Consumer::Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID)
 {
     decodeEnableFlag = enableFlag;
@@ -191,16 +193,16 @@ void CANRecv_Consumer::UltrasonicData_Ready_Slot(void)
     xmsg.set_sigobjdist_rlside(objDist_Send[11]);
 
     xmsg.set_sigsensor_front_ls(sensorStatus_Send[0]);
-    xmsg.set_sigsensor_front_l(sensorStatus_Send[1]);
+    xmsg.set_sigsensor_front_lc(sensorStatus_Send[1]);
     xmsg.set_sigsensor_front_lm(sensorStatus_Send[2]);
     xmsg.set_sigsensor_front_rm(sensorStatus_Send[3]);
-    xmsg.set_sigsensor_front_r(sensorStatus_Send[4]);
+    xmsg.set_sigsensor_front_rc(sensorStatus_Send[4]);
     xmsg.set_sigsensor_front_rs(sensorStatus_Send[5]);
     xmsg.set_sigsensor_rear_rs(sensorStatus_Send[6]);
-    xmsg.set_sigsensor_rear_r(sensorStatus_Send[7]);
+    xmsg.set_sigsensor_rear_rc(sensorStatus_Send[7]);
     xmsg.set_sigsensor_rear_rm(sensorStatus_Send[8]);
     xmsg.set_sigsensor_rear_lm(sensorStatus_Send[9]);
-    xmsg.set_sigsensor_rear_l(sensorStatus_Send[10]);
+    xmsg.set_sigsensor_rear_lc(sensorStatus_Send[10]);
     xmsg.set_sigsensor_rear_ls(sensorStatus_Send[11]);
 #ifdef RESULT_OUTPUT_ENABLE
     for(int i=0;i<12;i++)

src/include/proto/ultraarea.proto

@@ -0,0 +1,30 @@
+/*
+		      1  
+ 	        o/--o---o--\o
+		|           |
+ 	       o|	    |o 
+ 		|	    |
+		|	    |
+	     4  |	    |  2
+		|	    |
+               o|	    |o 
+		|	    |
+		o\--o---o--/o
+		      3 	
+ */
+
+syntax = "proto2";
+
+package iv.ultrasonic;
+
+message Area {
+    optional int32 id = 1;   // id of area.
+    optional float dist = 2 [default = 50000]; // min dist in the area. (mm)
+    optional bool valid = 3 [default = true]; // true -> working   |   false -> all ultras in the area don't work.
+};
+
+message ultraarea
+{
+    repeated Area area = 1;	
+    required int64 timestamp = 2;	
+}

src/include/proto/ultrasonic.proto

@@ -1,16 +1,16 @@
 /*
 		 2  3   4  5
  		/o--o---o--o\
-		|			|
- 	 1 o|			|o 6
- 		|			|
-		|			|
-		|			|
-		|			|
-    12 o|			|o 7
-		|			|
+		|	    |
+ 	     1 o|	    |o 6
+ 		|	    |
+		|	    |
+		|	    |
+		|	    |
+            12 o|	    |o 7
+		|	    |
 		\o--o---o--o/
-		11	10  9  8	
+	        11  10  9  8	
  */
 
 syntax = "proto2";
@@ -25,25 +25,25 @@ message ultrasonic
 	optional uint32 sigObjDist_FRMiddle = 4; //前中右
 	optional uint32 sigObjDist_FRCorner = 5; //前右角
 	optional uint32 sigObjDist_FRSide   = 6; //前右侧
-	optional uint32 sigObjDist_RLSide   = 7; //后左侧
-	optional uint32 sigObjDist_RLCorner = 8; //后左角
-	optional uint32 sigObjDist_RLMiddle = 9; //后中左
-	optional uint32 sigObjDist_RRMiddle = 10;//后中右
-	optional uint32 sigObjDist_RRCorner = 11;//后右角
-	optional uint32 sigObjDist_RRSide   = 12;//后右侧
+        optional uint32 sigObjDist_RRSide   = 7;//后右侧
+	optional uint32 sigObjDist_RRCorner = 8;//后右角
+	optional uint32 sigObjDist_RRMiddle = 9;//后中右
+	optional uint32 sigObjDist_RLMiddle = 10; //后中左
+	optional uint32 sigObjDist_RLCorner = 11; //后左角
+	optional uint32 sigObjDist_RLSide   = 12; //后左侧
 
-	optional uint32 sigSensor_Front_LS  = 13;//前左侧运行状态显示
-	optional uint32 sigSensor_Front_L   = 14;//前左角运行状态显示
-	optional uint32 sigSensor_Front_LM  = 15;//前中左
-	optional uint32 sigSensor_Front_RM  = 16;//前中右
-	optional uint32 sigSensor_Front_R   = 17;//前右角
-	optional uint32 sigSensor_Front_RS  = 18;//前右侧
-	optional uint32 sigSensor_Rear_L	= 19;//后左角
-	optional uint32 sigSensor_Rear_LS   = 20;//后左侧
-	optional uint32 sigSensor_Rear_LM   = 21;//后左中
-	optional uint32 sigSensor_Rear_R    = 22;//后右角
-	optional uint32 sigSensor_Rear_RS   = 23;//后右侧
-	optional uint32 sigSensor_Rear_RM   = 24;//后右中
+	optional uint32 sigSensor_Front_LS  = 13;//前左侧运行状态显示 
+        optional uint32 sigSensor_Front_LC  = 14;//前左角运行状态显示  
+        optional uint32 sigSensor_Front_LM  = 15;//前中左 
+        optional uint32 sigSensor_Front_RM  = 16;//前中右 
+        optional uint32 sigSensor_Front_RC  = 17;//前右角 
+        optional uint32 sigSensor_Front_RS  = 18;//前右侧
+        optional uint32 sigSensor_Rear_RS   = 19;//后右侧
+        optional uint32 sigSensor_Rear_RC   = 20;//后右角 
+        optional uint32 sigSensor_Rear_RM   = 21;//后右中 
+        optional uint32 sigSensor_Rear_LM   = 22;//后左中
+        optional uint32 sigSensor_Rear_LC   = 23;//后左角 
+        optional uint32 sigSensor_Rear_LS   = 24;//后左侧 
 
 	optional uint32 sigCANVoltageSt = 25;
 	optional uint32 sigPAVoltageSt = 26;