add ultra logic

src/detection/detection_chassis/decodechassis.cpp

@@ -29,219 +29,23 @@ static void ShareChassis(void * pa,iv::chassis  * pchassis)
 
 }
 
-/**
-  * @brief ProcGE3Chassis
-  * Process GE3 Chassis State
-  * @param pa                pointer to module communication handle
-  * @param pmsg              pointer to CAN Message
-  * @retval                  0 No Chassis Message   1 Have Chassis Message And Update
-**/
-int ProcGE3Chassis(void * pa, iv::can::canmsg * pmsg)
-{
-    int i;
-    int nRtn = 0;
-
-//    mygivlog = new iv::Ivlog("chassis");
- //   mygivfault = new iv::Ivfault("chassis");
-
-    for(i=0;i<pmsg->rawmsg_size();i++)
-    {
-        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
-        unsigned char data[8];
-        if(praw->id() == 0x13)
-        {
-            std::cout<<"receive chassis."<<std::endl;
-            memcpy(data,praw->data().data(),8);
-            unsigned char EPSMode,EPBFault,DriveMode,Shift,AEBAvailable,CDDAvailable;
-            unsigned short angle_feedback;
-
-            EPSMode = (data[1] & 0x60)>>5;     //EPS_mode,00：失效；01：人工；10自动
-            EPBFault = (data[1] & 0x10)>>4;    //EPB故障状态，0：无故障  1：故障
-            DriveMode = (data[1] & 0x0C)>>2;   //驾驶模式，1：Manual  3:Auto
-            Shift = data[1] & 0x03;   //Gear,00N,01D，10R,11p
-
-            angle_feedback = data[2] * 256 + data[3];
-            angle_feedback = angle_feedback * 0.1 - 780;
-
-
-            //制动的两个状态为反馈，AEBAvailable 和CDDAvailable，这两个都为1的时候制动有效，有一个为0，制动失效
-            AEBAvailable = data[4] & 0x01; //0=Not available,1=Available
-            CDDAvailable = (data[4] & 0x02) >> 1;  //0=Not available,1=Available
-
-            iv::chassis xchassis;
-
-            //xchassis.set_epsmode(EPSMode);
-            if(EPSMode == 0)
-            {
-                std::cout<<"eps mode"<<std::endl;
- //               mygivlog->warn("chassis"," Warning!!  NOW GE3 chassis triggers a epsmode！！");
-            }
-            if(EPSMode == 1)
-            {
-//                mygivlog->info("chassis","OK!!  NOW GE3 chassis epsmode is Manual！");
-            }
-            if(EPSMode == 2)
-            {
-  //              mygivlog->info("chassis","OK!! NOW GE3 chassis epsmode is Auto！");
-            }
-
-//            xchassis.set_epsmode(2);//为了不退出自动驾驶，经讨论，在这个地方写定值。
-
-            xchassis.set_epsmode(EPSMode);
-            xchassis.set_epbfault(EPBFault);
-            xchassis.set_drivemode(DriveMode);
-            xchassis.set_shift(Shift);
-
-            xchassis.set_aebavailable(AEBAvailable);
-            xchassis.set_cddavailable(CDDAvailable);
-            xchassis.set_angle_feedback(angle_feedback);
-
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-    }
-
-    return nRtn;
-
-
-}
-
-
-//struct Command_Response_Bit_0x14
-//{
-//    unsigned char feedback_angle_H;
-//    unsigned char feedback_angle_L;
-//    unsigned char feedback_angle_speed;
-//    unsigned char feedback_torque;
-//    unsigned char feedback_speed_H;
-//    unsigned char feedback_speed_L;
-//    unsigned char feedback_battery_power;
-//    unsigned char feedback_door_state:2;
-//    unsigned char feedback_EPB_state:1;
-//    unsigned char feedback_reserved:3;
-//    unsigned char feedback_dangwei:2;
-//};
-
-
-//ServiceCarStatus.torque_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_torque;
-//ServiceCarStatus.dangwei_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_dangwei;
-//ServiceCarStatus.battery_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_battery_power*0.4;
-
-//ServiceCarStatus.vehSpeed_st= (ServiceControlStatus.command_reponse_0x14.bit.feedback_speed_H*256+
-//                              ServiceControlStatus.command_reponse_0x14.bit.feedback_speed_L)*0.05625;
-
-int ProcPROBLUEChassis(void * pa, iv::can::canmsg * pmsg)
-{
-    int i;
-    int nRtn = 0;
-    for(i=0;i<pmsg->rawmsg_size();i++)
-    {
-
-        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
-        unsigned char data[8];
-        if(praw->id() == 0x14)
-        {
-            memcpy(data,praw->data().data(),8);
-            double torque,dangwei,soc,vehspeed;
-            torque = data[3];
-            dangwei = data[7]>>6;
-            soc = data[6]; soc = soc *0.4;
-            vehspeed = (data[4]*256.0 + data[5])*0.05625;
-
-            iv::chassis xchassis;
-            xchassis.set_torque(torque);
-            xchassis.set_shift(dangwei);
-            xchassis.set_soc(soc);
-            xchassis.set_vel(vehspeed);
-
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-    }
-
-    return nRtn;
-}
-
-int ProcMIDCARChassis(void * pa, iv::can::canmsg * pmsg)
+inline float getfourmotovalue(unsigned char * strdata, float fratio, float foff)
 {
-    int i;
-    int nRtn = 0;
-    for(i=0;i<pmsg->rawmsg_size();i++)
-    {
-
-        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
-        unsigned char data[8];
-        if(praw->id() == 0x14)
-        {
-            memcpy(data,praw->data().data(),8);
-            double torque,dangwei,soc,vehspeed;
-            torque = data[3];
-            dangwei = data[7]>>6;
-            soc = data[6]; soc = soc *0.4;
-            vehspeed = (data[4]*256.0 + data[5])*0.05625;
-
-            iv::chassis xchassis;
-            xchassis.set_torque(torque);
-            xchassis.set_shift(dangwei);
-            xchassis.set_soc(soc);
-            xchassis.set_vel(vehspeed);
-
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-        else if(praw->id() == 0xC2)
-        {
-            memcpy(data,praw->data().data(),8);
-            float soc;
-            soc = data[0];
-
-            iv::chassis xchassis;
-            xchassis.set_soc(soc);
-
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-    }
-
-    return nRtn;
-
-}
-
-int ProcVV7Chassis(void * pa, iv::can::canmsg * pmsg)
-{
-    int i;
-    int nRtn = 0;
-    for(i=0;i<pmsg->rawmsg_size();i++)
-    {
-
-        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
-        unsigned char data[8];
-        if(praw->id() == 0x13)
-        {
-            memcpy(data,praw->data().data(),8);
-            double engine_speed;
-
-            engine_speed = (data[5]*256.0 + data[6])*0.125;
-
-            iv::chassis xchassis;
-            xchassis.set_engine_speed(engine_speed);
-
-
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-    }
-
-    return nRtn;
 
+    int a,b,c,d;
+    a = strdata[0];
+    b = strdata[1];
+    c = strdata[2];
+    d = strdata[3];
+    float fvalue = a<<24 + b<<16 + c<<8 +d;
+    fvalue = fvalue * fratio + foff;
+    return fvalue;
 }
-
-
 inline float gettwomotovalue(unsigned char * strdata,float fratio,float foff)
 {
     int a,b;
-    a = strdata[0];
-    b = strdata[1];
+    a = strdata[0] & 0xFF;
+    b = strdata[1] & 0xFF;
     float fvalue = a*256+ b;
     fvalue = fvalue * fratio + foff;
     return fvalue;
@@ -257,79 +61,19 @@ inline float getonevalue(unsigned char * strdata,float fratio,float foff)
     return fvalue;
 }
 
-
 static bool  ghave0x13= false;
 static bool  ghave0x14= false;
 static bool  ghave0x15= false;
-int ProcMIDBUSChassis(void * pa, iv::can::canmsg * pmsg)
-{
-    int i;
-    int nRtn = 0;
-    static iv::chassis xchassis;
-    for(i=0;i<pmsg->rawmsg_size();i++)
-    {
-
-        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
-        unsigned char data[8];
-        memcpy(data,praw->data().data(),8);
-        if(praw->id() == 0x13)
-        {           
-            xchassis.set_angle_feedback(gettwomotovalue(data+0,0.1,-870));
-            xchassis.set_torque(getonevalue(data+2,1.0,0));
-            xchassis.set_brake_feedback(getonevalue(data+3,0.4,0));
-            xchassis.set_vel(gettwomotovalue(data+4,0.05625,0));
-            xchassis.set_epb_feedback(data[6]&0x03);
-            xchassis.set_shift((data[6]&0xf0)>>4);
-            xchassis.set_drivemode(data[7]&0x01);
-            xchassis.set_emergencystop_feedback((data[7]&06)>>1);
-            xchassis.set_brakelight_feedback((data[7]&0x08)>>3);
-
-
-
-            ghave0x13 = true;
-
-        }
-        if(praw->id() == 0x14)
-        {
-            xchassis.set_range_feedback(data[0]);
-            xchassis.set_soc(getonevalue(data+1,0.4,0));
-            xchassis.set_drivectrltype_feedback(data[5]&0x0f);
-            xchassis.set_brakectrltype_feedback((data[5]&0xf0)>>4);
-            xchassis.set_epsctrltype_feedback(data[6]&0x0f);
-            ghave0x14 = true;
-        }
-        if(praw->id() == 0x15)
-        {
-
-            xchassis.set_frontleftwheel_feedback(gettwomotovalue(data,0.05625,0));
-            xchassis.set_frontrightwheel_feedback(gettwomotovalue(data+2,0.05625,0));
-            xchassis.set_rearleftwheel_feedback(gettwomotovalue(data+4,0.05625,0));
-            xchassis.set_rearrightwheel_feedback(gettwomotovalue(data+6,0.05625,0));
-            ghave0x15 = true;
-        }
+static bool  ghave0x28b= false;
+static bool  ghave0x255= false;
 
-        if(ghave0x13&&ghave0x14&&ghave0x15)
-        {
-            ghave0x13 = false;
-            ghave0x14 = false;
-            ghave0x15 = false;
-            ShareChassis(pa,&xchassis);
-            nRtn = 1;
-        }
-    }
-
-    return nRtn;
-
-}
-
-int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg)
+int ProcJSRunlegs(void * pa, iv::can::canmsg * pmsg)
 {
     int i;
     int nRtn = 0;
     static iv::chassis xchassis;
     for(i=0;i<pmsg->rawmsg_size();i++)
     {
-
         const iv::can::canraw * praw = &(pmsg->rawmsg(i));
         unsigned char data[8];
         memcpy(data,praw->data().data(),8);
@@ -367,28 +111,47 @@ int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg)
             xchassis.set_rearrightwheel_feedback(gettwomotovalue(data+6,0.05625,0));
             ghave0x15 = true;
         }
-
-        if(ghave0x13&&ghave0x14&&ghave0x15)
+        //ErrorCode FB---HaoMo
+        if(praw->id() == 0x28B)
+        {
+            int a;
+            a = data[0] & 0x03;
+            xchassis.set_veherrind(a);
+            a = data[0] & 0x0C;
+            xchassis.set_brakesyserr(a);
+            a = data[0] & 0x30;
+            xchassis.set_epserr(a);
+            a = data[0] & 0xC0;
+            xchassis.set_motorerr(a);
+            a = data[0] & 0x03;
+            xchassis.set_epberr(a);
+            a = data[0] & 0x0C;
+            xchassis.set_hivolbatteryerr(a);
+            ghave0x28b = true;
+        }
+        //XingshiLiCheng
+        if(praw->id() == 0x255)
+        {
+            int a;
+            float fvalue;
+            a = data[1]<<16 + data[2]<<8 + data[3];
+            fvalue = a * 0.1;
+            xchassis.set_submileage(fvalue);
+            a = (data[4]<<8 +data[5])>>2;
+            fvalue = a * 0.1;
+            xchassis.set_totalmileage(fvalue);
+            ghave0x255 = true;
+        }
+        if(ghave0x13 && ghave0x14 && ghave0x15 && ghave0x255 && ghave0x28b)
         {
             ghave0x13 = false;
             ghave0x14 = false;
             ghave0x15 = false;
+            ghave0x255 = false;
+            ghave0x28b = false;
             ShareChassis(pa,&xchassis);
             nRtn = 1;
         }
     }
-
     return nRtn;
-
-}
-
-
-int ProcYUHESENChassis(void * pa, iv::can::canmsg * pmsg)
-{
-    int i;
-    int nRtn = 0;
-
-
-    return nRtn;
-
 }

src/detection/detection_chassis/decodechassis.h

@@ -6,19 +6,5 @@
 #include "modulecomm.h"
 #include "canmsg.pb.h"
 #include "chassis.pb.h"
-
-int ProcGE3Chassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcPROBLUEChassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcMIDCARChassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcVV7Chassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcMIDBUSChassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg);
-
-int ProcYUHESENChassis(void * pa, iv::can::canmsg * pmsg);
-
+int ProcJSRunlegs(void * pa, iv::can::canmsg * pmsg);
 #endif // DECODECHASSIS_H

src/detection/detection_chassis/main.cpp

@@ -26,7 +26,7 @@ QTime gTime;
 
 namespace  iv {
 
-enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN} gVehicleType;  //车辆类型
+enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN, JSRunlegs} gVehicleType;  //车辆类型
 
 
 }
@@ -59,32 +59,15 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
 
     int nRtn = 0;
     switch (iv::gVehicleType) {
-    case iv::GE3:
-        nRtn = ProcGE3Chassis(gpa,&xmsg);
-        break;
-    case iv::VV7:
-        nRtn = ProcVV7Chassis(gpa,&xmsg);
-        break;
-    case iv::PROBLUE:
-        nRtn = ProcPROBLUEChassis(gpa,&xmsg);
-        break;
-    case iv::MIDCAR:
-        nRtn = ProcMIDCARChassis(gpa,&xmsg);
-        break;
-    case iv::MIDBUS:
-        nRtn = ProcMIDBUSChassis(gpa,&xmsg);
-        break;
-    case iv::HAPO:
-        nRtn = ProcHAPOChassis(gpa,&xmsg);
-        break;
-    case iv::YUHESEN:
-        nRtn = ProcYUHESENChassis(gpa,&xmsg);
+    case iv::JSRunlegs:
+        nRtn = ProcJSRunlegs(gpa,&xmsg);
         break;
     default:
         break;
     }
 
-    if(nRtn == 1)gnRadarState = 100;
+    if(nRtn == 1)
+        gnRadarState = 100;
 //    DecodeRadar(xmsg);
 
 //    qDebug("can size is %d",xmsg.rawmsg_size());
@@ -104,7 +87,8 @@ void threadstate()
     while(1)
     {
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
-        if(gnRadarState > -100)gnRadarState--;
+        if(gnRadarState > -100)
+            gnRadarState--;
         if(gnRadarState > 0)
         {
             givfault->SetFaultState(0,0,"OK");
@@ -120,7 +104,6 @@ void threadstate()
                 givfault->SetFaultState(1,2,"底盘无响应");
             }
         }
-
     }
 }
 
@@ -147,41 +130,12 @@ int main(int argc, char *argv[])
     std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
 
     iv::gVehicleType = iv::UNKNOWN;
-    if(strncmp(strvehicletype.data(),"GE3",255) == 0)
-    {
-        iv::gVehicleType = iv::GE3;
-    }
-
-    if(strncmp(strvehicletype.data(),"VV7",255) == 0)
-    {
-        iv::gVehicleType = iv::VV7;
-    }
 
-    if(strncmp(strvehicletype.data(),"PROBLUE",255) == 0)
+    if(strncmp(strvehicletype.data(),"JSRunlegs",255) == 0)
     {
-        iv::gVehicleType = iv::PROBLUE;
+        iv::gVehicleType = iv::JSRunlegs;
     }
 
-    if(strncmp(strvehicletype.data(),"MIDCAR",255) == 0)
-    {
-        iv::gVehicleType = iv::MIDCAR;
-    }
-
-    if(strncmp(strvehicletype.data(),"MIDBUS",255) == 0)
-    {
-        iv::gVehicleType = iv::MIDBUS;
-    }
-    if(strncmp(strvehicletype.data(),"HAPO",255) == 0)
-    {
-        iv::gVehicleType = iv::HAPO;
-    }
- if(strncmp(strvehicletype.data(),"YUHESEN",255) == 0)
-    {
-        iv::gVehicleType = iv::YUHESEN;
-    }
-
-//iv::gVehicleType = iv::MIDBUS;
-
     givlog = new iv::Ivlog(strmodulename.data());
     givfault = new iv::Ivfault(strmodulename.data());
 
@@ -193,6 +147,5 @@ int main(int argc, char *argv[])
 
     std::thread threadfault(threadstate);
 
-
     return a.exec();
 }

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/chassis.proto

@@ -29,5 +29,12 @@ message chassis
   optional float rearleftwheel_feedback = 23;
   optional float rearrightwheel_feedback = 24;
   optional float engine_speed = 25;
-  
+  optional int32 vehErrInd = 26;	//整车线控故障状态
+  optional int32 brakeSysErr = 27;	//制动系统故障状态
+  optional int32 EPSErr = 28;		//转向系统故障状态
+  optional int32 motorErr = 29;		//驱动电机故障状态
+  optional int32 EPBErr = 30;		//电子驻车系统故障状态
+  optional int32 hiVolBatteryErr = 31;	//高压电池系统故障状态
+  optional int32 subMileage = 32;	//小计行驶里程
+  optional int32 totalMileage = 33;	//总行驶里程
 };

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;