detection ultrasonic

src/detection/detection_ultrasonic/detection_ultrasonic.pro

@@ -0,0 +1,36 @@
+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
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+

src/detection/detection_ultrasonic/main.cpp

@@ -0,0 +1,201 @@
+#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_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()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flside()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flcorner()<<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*1.e-3);
+        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*1.e-3);
+        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*1.e-3);
+        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*1.e-3);
+        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_area";
+
+    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();
+}