|
@@ -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;
|