Merge branch 'master' of http://118.24.47.114/madesheng/self-driving-robot

src/canbus/canbus.cpp

@@ -1,4 +1,4 @@
-/******************************************************************************
+/******************************************************************************
  * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
  *
  * Licensed under the Apache License, Version 1.0 (the "License");
@@ -79,7 +79,7 @@ int Canbus::Start() {
       LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
   }
 
-  ros::Rate loop_rate(10);
+  ros::Rate loop_rate(2000);
   // 5. set timer to triger publish info periodly
 
   timer_ = CreateTimer(ros::Duration(duration), &Canbus::OnTimer, this);
@@ -94,15 +94,21 @@ int Canbus::Start() {
 
 
 void Canbus::OnTimer(const ros::TimerEvent &) {
-  LOG(INFO)<< Name()<< " timer start ..";
-  //ros::Publisher geometry_pub = node_handle_->advertise<geometry_msgs::Point>("geometry", 200);
-
+ // LOG(INFO)<< Name()<< " timer start ..";
+  if(heatbeat > 15)
+  {
+      heatbeat = 0;
+  }
+  heatbeat ++;
+  icuMonitor.uiICUAlive = heatbeat;
+  MessageCoder::CanMsgPackage((void*)&icuMonitor,0x5FA,&canIcuMonitor);
   MessageCoder::CanMsgPackage((void*)&status1,0x6F1,&canStatus1);
   MessageCoder::CanMsgPackage((void*)&status2,0x6F2,&canStatus2);
   MessageCoder::CanMsgPackage((void*)&status3,0x6F3,&canStatus3);
   write_data(&canStatus1);
   write_data(&canStatus2);
   write_data(&canStatus3);
+  write_data(&canIcuMonitor);
 }
 
 void Canbus::Stop() {
@@ -112,6 +118,8 @@ void Canbus::Stop() {
       funCAN_Close(pcan_handle);
   }
   LOG(INFO)<<Name()<<": closed. ";
+  ros::shutdown();
+    LOG(INFO)<<Name()<<": node shutdown. ";
 }
 
 
@@ -125,6 +133,7 @@ int Canbus::read_loop()
         LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
         return errno;
     }
+    read_data(&(msg.Msg));
      // deal receive data
     // check if a CAN status is pending
     if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
@@ -134,70 +143,125 @@ int Canbus::read_loop()
             LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
             return errno;
         }
-        LOG(INFO)<<Name()<<" receive: pending CAN status "<<(__u16)status<<" read.";
-        read_data(&(msg.Msg));
     }
+
     return ROBOT_SUCCESS;
 }
 
 int Canbus::read_data(TPCANMsg *pMsgBuff)
 {
-    LOG(INFO)<<Name()<<" Receive CanData : ID: "<<pMsgBuff->ID<<",Data: "<<pMsgBuff->DATA;
+//    char strId[4] = {0};
+//    sprintf(strId,"%3xh",pMsgBuff->ID);
+//    char strData[16]={0};
+//    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
+
+//    LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
     if(pMsgBuff->ID == 0x1A1)
     {
+//       char strId1[4] = {0};
+//       char strData1[16]={0};
+//       sprintf(strId1,"%3xh",pMsgBuff->ID);
+//       MessageCoder::ConvertUnCharToStr(strData1,pMsgBuff->DATA,8);
+//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId1)<<",Data: "<<std::string(strData1);
        T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(0x1A1,pMsgBuff->DATA));
        status1.uiAccPsng = tOBDAccPsng.obdAccPsng;
+//       LOG(INFO)<<Name()<<"ID:0x1A1,Data: "<<tOBDAccPsng.obdAccPsng;
     }
     else if(pMsgBuff->ID == 0x0F1)
     {
+//       char strId2[4] = {0};
+//       char strData2[16]={0};
+//       sprintf(strId2,"%3xh",pMsgBuff->ID);
+//       MessageCoder::ConvertUnCharToStr(strData2,pMsgBuff->DATA,8);
+//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId2)<<",Data: "<<std::string(strData2);
        T_OBD_BRK_PSNG tOBDBrkPsng = *((T_OBD_BRK_PSNG*)MessageCoder::CanMsgAnalyse(0x0F1,pMsgBuff->DATA));
        status1.uiBrkPsng = tOBDBrkPsng.obdBrkPsng;
+//       LOG(INFO)<<Name()<<"ID:0x0F1,Data: "<<tOBDBrkPsng.obdBrkPsng;
     }
     else if(pMsgBuff->ID == 0x135)
     {
+//       char strId3[4] = {0};
+//       char strData3[16]={0};
+//       sprintf(strId3,"%3xh",pMsgBuff->ID);
+//       MessageCoder::ConvertUnCharToStr(strData3,pMsgBuff->DATA,8);
+//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId3)<<",Data: "<<std::string(strData3);
        T_OBD_SFT_PSNG tOBDSftPsng = *((T_OBD_SFT_PSNG*)MessageCoder::CanMsgAnalyse(0x135,pMsgBuff->DATA));
        status1.uiSftPsng = tOBDSftPsng.obdSftPsng;
+//      LOG(INFO)<<Name()<<"ID:0x135,Data: "<<tOBDSftPsng.obdSftPsng;
     }
     else if(pMsgBuff->ID == 0x1F1)
     {
+//        char strId4[4] = {0};
+//        char strData4[16]={0};
+//        sprintf(strId4,"%3xh",pMsgBuff->ID);
+//        MessageCoder::ConvertUnCharToStr(strData4,pMsgBuff->DATA,8);
+//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId4)<<",Data: "<<std::string(strData4);
         T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(0x1F1,pMsgBuff->DATA));
         status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
+//        LOG(INFO)<<Name()<<"ID:0x1F1,Data: "<<tOBDVehBrkStat.obdVehHBrkStat;
     }
     else if(pMsgBuff->ID == 0x121)
     {
+//        char strId5[4] = {0};
+//        char strData5[16]={0};
+//        sprintf(strId5,"%3xh",pMsgBuff->ID);
+//        MessageCoder::ConvertUnCharToStr(strData5,pMsgBuff->DATA,8);
+//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId5)<<",Data: "<<std::string(strData5);
         T_OBD_VEH_SW_STAT tOBDVehSwStat = *((T_OBD_VEH_SW_STAT*)MessageCoder::CanMsgAnalyse(0x121,pMsgBuff->DATA));
         status1.uiVehSwStat = tOBDVehSwStat.obdVehSwStat;
+//        LOG(INFO)<<Name()<<"ID:0x121,Data: "<<tOBDVehSwStat.obdVehSwStat;
     }
     else if(pMsgBuff->ID == 0x1E5)
     {
-        T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(0x1E5,pMsgBuff->DATA));
-        status2.iStrAng = tOBDStrAng.obdStrAng;
+//       char strId6[4] = {0};
+//       char strData6[16]={0};
+//       sprintf(strId6,"%3xh",pMsgBuff->ID);
+//       MessageCoder::ConvertUnCharToStr(strData6,pMsgBuff->DATA,8);
+//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId6)<<",Data: "<<std::string(strData6);
+       T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(0x1E5,pMsgBuff->DATA));
+       status2.iStrAng = tOBDStrAng.obdStrAng;
+//        LOG(INFO)<<Name()<<"ID:0x1E5,Data: "<<tOBDStrAng.obdStrAng;
     }
     else if(pMsgBuff->ID == 0x0C9)
     {
+//        char strId7[4] = {0};
+//        char strData7[16]={0};
+//        sprintf(strId7,"%3xh",pMsgBuff->ID);
+//        MessageCoder::ConvertUnCharToStr(strData7,pMsgBuff->DATA,8);
+//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId7)<<",Data: "<<std::string(strData7);
         T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(0x0C9,pMsgBuff->DATA));
         status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
+        //LOG(INFO)<<Name()<<"ID:0x0C9,Data: "<<tOBDEngSpd.obdEngSpd;
     }
-    else
+    else if((pMsgBuff->ID > 0x5F0 && pMsgBuff->ID < 0x7FF) || pMsgBuff->ID ==0x2E0 || pMsgBuff->ID ==0x2E1)
     {
-      //dataAnalysis::CanData canData;
-      //canData.set_id(pMsgBuff->ID);
-      //canData.set_data(reinterpret_cast<const char*>(pMsgBuff->DATA));
-      std_msgs::String ros_msg;
-      std::stringstream ss;
-      ss << "Id:" << pMsgBuff->ID << ",Data:" << pMsgBuff->DATA;
-      ros_msg.data = ss.str();
-      ros::Publisher canData_pub = node_handle_->advertise<std_msgs::String>(robot::common::CanDataAnalysis, 200);
-      canData_pub.publish(ros_msg);
-      LOG(INFO)<<Name()<<" Publish CanData :"<<ros_msg;
-    }
+        //publish_data(pMsgBuff);
 
+    }
     return ROBOT_SUCCESS;
 }
 
+void Canbus::publish_data(TPCANMsg *pMsgBuff)
+{
+    //dataAnalysis::CanData canData;
+    //canData.set_id(pMsgBuff->ID);
+    //canData.set_data(reinterpret_cast<const char*>(pMsgBuff->DATA));
+    std_msgs::String ros_msg;
+    std::stringstream ss;
+    ss << "Id:" << pMsgBuff->ID << ",Data:" << pMsgBuff->DATA;
+    ros_msg.data = ss.str();
+    ros::Publisher canData_pub = node_handle_->advertise<std_msgs::String>(robot::common::CanDataAnalysis, 200);
+    canData_pub.publish(ros_msg);
+    LOG(INFO)<<Name()<<" Publish CanData :"<<ss.str();
+}
+
 int Canbus::write_data(TPCANMsg *pMsgBuff)
 {
-  LOG(INFO)<<Name()<<" send CanData : ID: "<<pMsgBuff->ID<<",Data:"<<pMsgBuff->DATA;
+    char strId[4] = {0};
+    sprintf(strId,"%3xh",pMsgBuff->ID);
+    char strData[16]={0};
+    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
+  LOG(INFO)<<Name()<<" send CanData : ID: "<<std::string(strId)<<",Data:"<<std::string(strData);
   if (funCAN_Write(pcan_handle, pMsgBuff)){
      LOG(ERROR)<<Name()<< " write: LINUX_CAN_write_timeout error. ";
      return errno;

src/canbus/canbus.h

@@ -43,7 +43,7 @@ typedef DWORD   (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
 typedef int     (*funnGetLastError_TYPE)(void);
 
 //the target device name
-#define DEFAULT_NODE "/dev/pcan0"
+#define DEFAULT_NODE "/dev/pcanusb32"
 #define ROBOT_SUCCESS     0
 #define ROBOT_FAILTURE   -1
 
@@ -115,6 +115,11 @@ class Canbus : public robot::common::RobotApp{
   */
   int read_data(TPCANMsg *pMsgBuff);
 
+  /**
+  * @brief publish data from can bus
+  */
+  void publish_data(TPCANMsg *pMsgBuff);
+
   int64_t last_timestamp_ = 0;
   ros::Timer timer_;
 
@@ -124,12 +129,21 @@ class Canbus : public robot::common::RobotApp{
   HANDLE pcan_handle =NULL;//void *pcan_handle
   unsigned short unBaudrate = CAN_BAUD_500K; //set the communicate baud rate of can bus
   int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST
+
   T_VEHICLE_STATUS_1 status1 = {0};    //VehicleStaus1
-  T_VEHICLE_STATUS_2 status2 = {0};     //VehicleStatus2
-  T_VEHICLE_STATUS_3 status3 = {0};     //VehicleStatus3
   TPCANMsg canStatus1; //VehicleStaus1 Can Message
+
+  T_VEHICLE_STATUS_2 status2 = {0};     //VehicleStatus2
   TPCANMsg canStatus2;  //VehicleStatus2 Can Message
+
+  T_VEHICLE_STATUS_3 status3 = {0};     //VehicleStatus3
   TPCANMsg canStatus3;  //VehicleStatus3 Can Message
+
+  T_ICU_MONITOR icuMonitor = {0}; //ICUMonitor
+  TPCANMsg canIcuMonitor; //ICUMonitor Can Message
+
+  int heatbeat = 0;  //ICUAlive
+
 };
 
 }  // namespace canbus

src/canbus/candatatype.h

@@ -2100,11 +2100,31 @@ typedef struct
 typedef struct
 {
     unsigned int : 8;
-    int obdStrAng : 16;		//实际方向盘转角
+    int obdStrAngH : 8;		//实际方向盘转角
+    int obdStrAngL : 8;		//实际方向盘转角
     unsigned int : 8;
     unsigned int : 32;
 }T_OBD_1E5;
 
+typedef struct
+{
+    int obdStrAngL : 8;		//实际方向盘转角
+    int obdStrAngH : 8;		//实际方向盘转角
+    unsigned int : 16;
+}T_OBD_1E5_SRC;
+
+typedef struct
+{
+    int obdStrAng : 16;		//实际方向盘转角
+    unsigned int : 16;
+}T_OBD_1E5_DEST;
+
+typedef union
+{
+    T_OBD_1E5_SRC tobd1e5Src;		//实际方向盘转角
+    T_OBD_1E5_DEST  tobd1e5Dest;
+}T_OBD_1E5_UNION;
+
 //实际方向盘转角
 typedef struct
 {

src/canbus/messagecoder.cpp

@@ -607,7 +607,8 @@ void MessageCoder::OBDAnalyseID_1A1(unsigned char *ucData, T_OBD_ACC_PSNG *tOBDA
     memcpy(ucCanMsgSrc, ucData, 8);
     memcpy(&tOBD1A1, ucCanMsgSrc, 8);
 
-    tOBDAccPsng->obdAccPsng = tOBD1A1.obdAccPsng / OBD_ACC_PSNG_RATIO;
+    tOBDAccPsng->obdAccPsng = (tOBD1A1.obdAccPsng *1000) / OBD_ACC_PSNG_RATIO;
+
 }
 
 //OBD实际制动踏板行程--数据解析
@@ -619,7 +620,7 @@ void MessageCoder::OBDAnalyseID_0F1(unsigned char *ucData, T_OBD_BRK_PSNG *tOBDB
     memcpy(ucCanMsgSrc, ucData, 8);
     memcpy(&tOBD0F1, ucCanMsgSrc, 8);
 
-    tOBDBrkPsng->obdBrkPsng = tOBD0F1.obdBrkPsng / OBD_BRK_PSNG_RATIO;
+    tOBDBrkPsng->obdBrkPsng = (tOBD0F1.obdBrkPsng *1000) / OBD_BRK_PSNG_RATIO;
 }
 
 //OBD实际挡位--数据解析
@@ -700,7 +701,12 @@ void MessageCoder::OBDAnalyseID_1E5(unsigned char *ucData, T_OBD_STR_ANG *tOBDSt
     memcpy(ucCanMsgSrc, ucData, 8);
     memcpy(&tOBD1E5, ucCanMsgSrc, 8);
 
-    tOBDStrAng->obdStrAng = tOBD1E5.obdStrAng / OBD_STR_ANG_RATIO;
+    T_OBD_1E5_UNION tObd1e5Union;
+    memset(&tObd1e5Union,0,sizeof(T_OBD_1E5_UNION));
+    tObd1e5Union.tobd1e5Src.obdStrAngH = tOBD1E5.obdStrAngH;
+    tObd1e5Union.tobd1e5Src.obdStrAngL = tOBD1E5.obdStrAngL;
+
+    tOBDStrAng->obdStrAng = (tObd1e5Union.tobd1e5Dest.obdStrAng *100) / OBD_STR_ANG_RATIO;
 }
 
 //OBD实际发动机转速--数据解析
@@ -1110,6 +1116,71 @@ void MessageCoder::CanMsgPackageID_7F7(T_VEHICLE_START * tVehicleStart, TPCANMsg
     tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
 }
 
+
+//车辆状态1--数据组包
+void MessageCoder::CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg)
+{
+    T_STATE_IN_6F1 tStateIn6F1 = { 0 };
+    unsigned char ucCanMsgSrc[8] = { 0 };
+
+    tStateIn6F1.uiAccPsng = tVehicleStatus1->uiAccPsng;
+    tStateIn6F1.uiBrkPsng = tVehicleStatus1->uiBrkPsng;
+    tStateIn6F1.uiClchPsng = tVehicleStatus1->uiClchPsng;
+    tStateIn6F1.uiDrvSftTrgtGear = tVehicleStatus1->uiDrvSftTrgtGear;
+    tStateIn6F1.uiSftPsng = tVehicleStatus1->uiSftPsng;
+    tStateIn6F1.uiVehHBrkStat = tVehicleStatus1->uiVehHBrkStat;
+    tStateIn6F1.uiVehSftMd = tVehicleStatus1->uiVehSftMd;
+    tStateIn6F1.uiVehSwStat = tVehicleStatus1->uiVehSwStat;
+
+    memcpy(ucCanMsgSrc, &tStateIn6F1, 8);
+
+    memcpy(tCanMsg->DATA, ucCanMsgSrc, 8);
+    tCanMsg->ID = 0x6F1;
+    tCanMsg->LEN = 8;
+    tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
+}
+
+//车辆状态2--数据组包
+void MessageCoder::CanMsgPackageID_6F2(T_VEHICLE_STATUS_2 *tVehicleStatus2, TPCANMsg *tCanMsg)
+{
+    T_STATE_IN_6F2 tStateIn6F2 = { 0 };
+    unsigned char ucCanMsgSrc[8] = { 0 };
+
+    T_STATE_IN_6F2_UNION tStateIn6F2Union;
+    memset(&tStateIn6F2Union, 0, sizeof(T_STATE_IN_6F2_UNION));
+    tStateIn6F2Union.tStateIn6F2Dest.iStrAngGrd = tVehicleStatus2->iStrAngGrd;
+
+    tStateIn6F2.iStrAng = tVehicleStatus2->iStrAng;
+    tStateIn6F2.iStrAngGrdH = tStateIn6F2Union.tStateIn6F2Src.iStrAngGrdH;
+    tStateIn6F2.iStrAngGrdL = tStateIn6F2Union.tStateIn6F2Src.iStrAngGrdL;
+    tStateIn6F2.uiEngSpd = tVehicleStatus2->uiEngSpd;
+    tStateIn6F2.uiVehSpd = tVehicleStatus2->uiVehSpd;
+
+    memcpy(ucCanMsgSrc, &tStateIn6F2, 8);
+    memcpy(tCanMsg->DATA, ucCanMsgSrc, 8);
+    tCanMsg->ID = 0x6F2;
+    tCanMsg->LEN = 8;
+    tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
+}
+
+//车辆状态3--数据组包
+void MessageCoder::CanMsgPackageID_6F3(T_VEHICLE_STATUS_3 *tVehicleStatus3, TPCANMsg *tCanMsg)
+{
+    T_STATE_IN_6F3 tStateIn6F3 = { 0 };
+    unsigned char ucCanMsgSrc[8] = { 0 };
+
+    tStateIn6F3.uiVehBatCur = tVehicleStatus3->uiVehBatCur;
+    tStateIn6F3.uiVehBatQut = tVehicleStatus3->uiVehBatQut;
+    tStateIn6F3.uiVehBatVlt = tVehicleStatus3->uiVehBatVlt;
+
+    memcpy(ucCanMsgSrc, &tStateIn6F3, 8);
+
+    memcpy(tCanMsg->DATA, ucCanMsgSrc, 8);
+    tCanMsg->ID = 0x6F3;
+    tCanMsg->LEN = 8;
+    tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
+}
+
 //数据解析
 void * MessageCoder::CanMsgAnalyse(int id,unsigned char * ucData)
 {
@@ -1368,16 +1439,33 @@ void MessageCoder::CanMsgPackage(void * p, int id, TPCANMsg *tCanMsg)
     case 0x7F5://油门机构位置时间控制
         CanMsgPackageID_7F5((T_ACC_PSTN_CTR_STP *)p,tCanMsg);
         break;
-    case 0x7F6:
+    case 0x7F6://方向盘位置时间序列控制
         CanMsgPackageID_7F6((T_STR_PSTN_TM_CTR *)p,tCanMsg);
         break;
-    case 0x7F7:
+    case 0x7F7://车辆一键启动
         CanMsgPackageID_7F7((T_VEHICLE_START *)p,tCanMsg);
         break;
+    case 0x6F1://车辆状态1
+        CanMsgPackageID_6F1((T_VEHICLE_STATUS_1 *)p,tCanMsg);
+        break;
+    case 0x6F2://车辆状态2
+        CanMsgPackageID_6F2((T_VEHICLE_STATUS_2 *)p,tCanMsg);
+        break;
+    case 0x6F3://车辆状态3
+        CanMsgPackageID_6F3((T_VEHICLE_STATUS_3 *)p,tCanMsg);
+        break;
     default:
         break;
     }
 }
 
+void MessageCoder::ConvertUnCharToStr(char *str,unsigned char *UnChar,int ucLen)
+{
+    for(int i=0;i<ucLen;i++)
+    {
+      sprintf(str+i*2,"%02x",UnChar[i]);
+    }
+}
+
 }// namespace canbus
 }// namespace robot

src/canbus/messagecoder.h

@@ -170,12 +170,23 @@ private:
 
     //车辆一键启动--数据组包
     static void CanMsgPackageID_7F7(T_VEHICLE_START *tVehicleStart,TPCANMsg *tCanMsg);
+
+    //车辆状态1--数据组包
+    static void CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg);
+
+    //车辆状态2--数据组包
+    static void CanMsgPackageID_6F2(T_VEHICLE_STATUS_2 *tVehicleStatus2, TPCANMsg *tCanMsg);
+
+    //车辆状态3--数据组包
+    static void CanMsgPackageID_6F3(T_VEHICLE_STATUS_3 *tVehicleStatus3, TPCANMsg *tCanMsg);
 public:
     //数据解析
     static void * CanMsgAnalyse(int id, unsigned char * ucData);
     //数据组包
     static void CanMsgPackage(void *p,int id, TPCANMsg *tCanMsg);
 
+    static void ConvertUnCharToStr(char *str,unsigned char *UnChar,int ucLen);
+
 };
 }// namespace canbus
 }// namespace robot