Add chassis module

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/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;	//总行驶里程
 };