添加cte值初始化使能和设备管理

src/canbus/canbus.cpp

@@ -51,7 +51,7 @@ int Canbus::Init() {
   status1.uiVehHBrkStat = 1;
 
 
-  InitObd();
+  InitServiceAndMsg();
 
   LOG(INFO)<<Name()<<" init end.";
   return ROBOT_SUCCESS;
@@ -176,7 +176,7 @@ void Canbus::Stop() {
 }
 
 
-void Canbus::InitObd()
+void Canbus::InitServiceAndMsg()
 {
      obd.AccPsng = 0;
      obd.BrkPsng = 0;
@@ -185,6 +185,31 @@ void Canbus::InitObd()
      obd.VehHBrkStat = 0;
      obd.VehSpd = 0;
      obd.VehSwStat = 0;
+     T_ACTUATOR_ENABLE_REQUEST request;
+     request.uiAccClchCtrReq = MOT_EN_REQ_ENABLE;
+     request.uiAccMotEnReq = MOT_EN_REQ_ENABLE;
+     request.uiBrkMotEnReq = MOT_EN_REQ_ENABLE;
+     request.uiClchMotEnReq = ACC_CLCH_CTR_REQ_RELEASE;
+     request.uiCtrMdReq= CTR_MD_REQ_HAND_DRIVE;
+     request.uiEstpEnReq = ESTP_EN_REQ_DISABLE;
+     request.uiPauseEnReq = PAUSE_EN_REQ_DISABLE;
+     request.uiPmpEnReq = PMP_EN_REQ_DISABLE;
+     request.uiStrMotEnReq = MOT_EN_REQ_ENABLE;
+     request.uiXSftMotEnReq = MOT_EN_REQ_ENABLE;
+     request.uiYSftMotEnReq = MOT_EN_REQ_ENABLE;
+
+     MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
+
+     T_RBT_MANAGER manager;
+     manager.uiAccMotInt = FLT_CLR_RESET;
+     manager.uiBrkMotInt = FLT_CLR_RESET;
+     manager.uiClchMotInt = FLT_CLR_RESET;
+     manager.uiFltClr = FLT_CLR_RESET;
+     manager.uiStrMotInt= FLT_CLR_RESET;
+     manager.uiXSftMotInt = FLT_CLR_RESET;
+     manager.uiYSftMotInt = FLT_CLR_RESET;
+
+     MessageCoder::CanMsgPackage((void*)&manager, robot::common::RobotManager, &canRobotManagerMsg);
 }
 
 //read from CAN forever - until manual break

src/canbus/canbus.h

@@ -91,7 +91,7 @@ class Canbus : public robot::common::RobotApp{
 
  private:
 
-  void InitObd();
+  void InitServiceAndMsg();
   /**
   * @brief timer callback function
   */

src/canbus/candatatype.h

@@ -88,11 +88,11 @@ typedef struct
     unsigned int uiXSftMotEnReq;	//换挡臂X电机使能
     unsigned int uiYSftMotEnReq;	//换挡臂Y电机使能
     unsigned int uiClchMotEnReq;	//离合电机使能
-    unsigned int uiAccClchCtrReq;	//油门离合器控	制
+    unsigned int uiAccClchCtrReq;	//油门离合器控制
     unsigned int uiEstpEnReq;		//机器人Estop使能
     unsigned int uiPmpEnReq;		//制动急停气泵使能
-    unsigned int uiPauseEnReq;		//系统软急停使	能
-    unsigned int uiCtrMdReq;		//实际系统控制	模式请求
+    unsigned int uiPauseEnReq;		//系统软急停使能
+    unsigned int uiCtrMdReq;		//实际系统控制模式请求
 }T_ACTUATOR_ENABLE_REQUEST;
 
 #define	MOT_EN_REQ_DISABLE		0		//电机禁能
@@ -108,7 +108,7 @@ typedef struct
 #define	PMP_EN_REQ_ENABLE	1			//制动急停气泵使能
 
 #define	PAUSE_EN_REQ_DISABLE	0		//系统软急停禁能
-#define	PAUSE_EN_REQ_ENABLE		1		//系统软急停使	能
+#define	PAUSE_EN_REQ_ENABLE		1		//系统软急停使能
 
 #define	CTR_MD_REQ_AUTO_LEARN		0	//控制模式请求-自动学习模式
 #define	CTR_MD_REQ_HAND_LEARN		1	//控制模式请求-手动学习模式
@@ -423,7 +423,7 @@ typedef struct
 }T_RBT_MANAGER;
 
 #define	MOT_INT_ZERO		1		//电机初始化-标零
-#define	FLT_CLR_RESET		1		//故障消除-复位
+#define	FLT_CLR_RESET		0		//故障消除-无效
 
 
 /*************上位机监控******设计汇总未定义*******/

src/decision/decision.cpp

@@ -378,6 +378,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      control.refSpeed = mpc.ref_speed;
      control.steer =  mpc.delta_prev;
      control.throttle = mpc.a_prev;
+     control.cte = cte;
      Control_pub.publish(control);
 }
 

src/decision/msg/controlMsg.msg

@@ -2,3 +2,4 @@ float64 steer
 float64 throttle
 float64 refSpeed
 float64 nowSpeed
+float64 cte