添加新的消息和服务5F1,7F7,5FC,6E4,6ED,70F

src/canbus/CMakeLists.txt

@@ -59,6 +59,11 @@ add_message_files(FILES
    SftActCtrReq.srv
    StrActCtrReq.srv
    RBTManager.srv
+   ActuatorCtrLimit.srv
+   PowerOnRequest.srv
+   SlopeMode.srv
+   VehicleStart.srv
+   StrLoopReq.srv
  )
 
 ## Generate actions in the 'action' folder

src/canbus/canbus.cpp

@@ -77,10 +77,10 @@ int Canbus::Start() {
       if (unBaudrate) {
               errno = funCAN_Init(pcan_handle, unBaudrate, nExtended);
               if (errno) {
-                  LOG(ERROR)<<Name()<<": CAN_Init()";
+                  LOG(ERROR)<<Name()<<": CAN_Init() error.";
               }
               else
-                  LOG(INFO)<<" Device Info: " << szDevNode <<"; CAN_BAUD_1M; CAN_INIT_TYPE_ST";
+                  LOG(INFO)<<" Device Info: " << szDevNode <<"Baudrate:"<<unBaudrate;
           }
   }
   else
@@ -94,8 +94,8 @@ int Canbus::Start() {
 
   // this location is necessary
 
-  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 2000);
-  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 2000);
+  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
+  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
 
   ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
                                                                 &Canbus::ActuatorEnableRequest, this);
@@ -109,6 +109,16 @@ int Canbus::Start() {
                                                               &Canbus::PedalActCtrRequest, this);
   ros::ServiceServer RBTManagerService = node_handle_->advertiseService(robot::common::RBTManagerService,
                                                               &Canbus::RBTManager, this);
+  ros::ServiceServer PowerOnService = node_handle_->advertiseService(robot::common::PowerOnService,
+                                                                     &Canbus::PowerOnRequest, this);
+  ros::ServiceServer ActuatorCtrLimitService = node_handle_->advertiseService(robot::common::ActuatorCtrLimitService,
+                                                                              &Canbus::ActuatorCtrLimit,this);
+  ros::ServiceServer VehicleStartService = node_handle_->advertiseService(robot::common::VehicleStartService,
+                                                                           &Canbus::VehicleStart,this);
+  ros::ServiceServer SlopeModeService = node_handle_->advertiseService(robot::common::SlopeModeService,
+                                                                       &Canbus::SlopeMode,this);
+  ros::ServiceServer StrLoopService = node_handle_->advertiseService(robot::common::StrLoopService,
+                                                                       &Canbus::StrLoopReq,this);
   //data receive
   while(ros::ok())
   {
@@ -416,9 +426,9 @@ bool Canbus::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
         request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
-        if (write_data(&canPedalActCtrReqMsg) != ROBOT_SUCCESS)
+        if (write_data(&canPedalActCtrReqMsg)!=ROBOT_SUCCESS)
         {
-            res.uiReturn = 0; // =0
+            res.uiReturn = 0;  //= 0
             return true;
         }
         res.uiReturn = 1;
@@ -521,6 +531,213 @@ bool Canbus::RBTManager(canbus::RBTManager::Request &req,
     return true;
 }
 
+bool Canbus::PowerOnRequest(canbus::PowerOnRequest::Request &req,
+                      canbus::PowerOnRequest::Response &res)
+{
+    LOG(INFO)<< "controlservice start PowerOnRequest";
+    // PowerON request
+    TPCANMsg canPowerOnReqMsg;
+    try{
+        T_POWER_ON_REQUEST request;
+        request.uiAccCtrPowReq = req.uiAccCtrPowReq;
+        request.uiBrkCtrPowReq = req.uiBrkCtrPowReq;
+        request.uiClchCtrPowReq = req.uiClchCtrPowReq;
+        request.uiRCUPowReq = req.uiRCUPowReq;
+        request.uiStrCtrPowReq= req.uiStrCtrPowReq;
+        request.uiXSftArmCtrPowReq = req.uiXSftArmCtrPowReq;
+        request.uiYSftArmCtrPowReq = req.uiYSftArmCtrPowReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::PowerOnReq, &canPowerOnReqMsg);
+
+        if (write_data(&canPowerOnReqMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice PowerOnRequest failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::ActuatorCtrLimit(canbus::ActuatorCtrLimit::Request &req,
+                              canbus::ActuatorCtrLimit::Response &res)
+{
+    LOG(INFO)<< "controlservice start ActuatorCtrLimit";
+    // ActuatorCtrLimit request
+    TPCANMsg canActCtrLimitMsg;
+    try{
+        T_ACT_CTR_LIMT request;
+        request.iStrMaxLimt = req.iStrMaxLimit;
+        request.uiAccMaxLimt = req.uiAccMaxLimit;
+        request.uiBrkMaxLimt = req.uiBrkMaxLimit;
+        request.uiRbtAplySenro = req.uiRbtAplySenro;
+        request.uiRbtCfgMd= req.uiRbtCfgMd;
+        request.uiSftType = req.uiSftType;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorCtrLimit, &canActCtrLimitMsg);
+
+        if (write_data(&canActCtrLimitMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice ActuatorCtrLimit failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::VehicleStart(canbus::VehicleStart::Request &req,
+                           canbus::VehicleStart::Response &res)
+{
+    LOG(INFO)<< "controlservice start VehicleStart";
+    // VehicleStart request
+    TPCANMsg canVechStrtMsg;
+    try{
+        T_VEHICLE_START request;
+        request.uiVehSwReq = req.uiVehSwReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::VehicleStart, &canVechStrtMsg);
+
+        if (write_data(&canVechStrtMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice VehicleStart failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::SlopeMode(canbus::SlopeMode::Request &req,
+                       canbus::SlopeMode::Response &res)
+{
+    LOG(INFO)<< "controlservice start SlopeMode";
+    // SlopeMode request
+    TPCANMsg canSlpMdMsg;
+    try{
+        T_Slope_MODE request;
+        request.iSlopeReq = req.iSlopeRequest;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::SlopeMODE, &canSlpMdMsg);
+
+        if (write_data(&canSlpMdMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice SlopeMode failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
+                       canbus::StrLoopReq::Response &res)
+{
+    LOG(INFO)<< "controlservice start StrLoopReq";
+    // StrLoopReq request
+    TPCANMsg canStrLpReqMsg;
+    try{
+      if(!req.bStart)
+      {
+        if(strTimerState)
+        {
+          str_timer_.stop();
+          strTimerState = false;
+          strLoopNum = 0;
+          tStrActCtrReq = {0};
+        }
+        res.uiReturn = 1;
+        return true;
+      }
+      if(strTimerState)
+      {
+        str_timer_.stop();
+        strTimerState = false;
+        strLoopNum = 0;
+        tStrActCtrReq = {0};
+      }
+      strLpReq = req;
+
+      double str_duration = (double)req.uiStrTimeInterval/1000;
+      str_timer_ = CreateTimer(ros::Duration(str_duration), &Canbus::StrLoopOnTimer, this,false,false);
+      str_timer_.start();
+      strTimerState = true;
+
+      res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice StrLoopReq failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+void Canbus::StrLoopOnTimer(const ros::TimerEvent &event)
+{
+  LOG(INFO)<< Name()<< " StrLoopTimer start ..";
+
+  TPCANMsg canStrActCtrReqMsg;
+  try
+  {
+    if(strLoopNum > strLpReq.uiStrFrequency)  //如果执行次数等于往复次数，则结束定时器
+    {
+      str_timer_.stop();
+      strTimerState = false;
+      strLoopNum = 0;
+      tStrActCtrReq = {0};
+    }
+    tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
+    tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
+    tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
+
+    if(tStrActCtrReq.iStrAngCtrReq > strLpReq.iStrAngEnd) //如果转向位置已经到达终止位置，则表示执行完一套动作，执行次数加一
+    {
+      strLoopNum++;
+      tStrActCtrReq = {0};
+    }
+
+    if(tStrActCtrReq.iStrAngCtrReq == 0)
+    {
+      tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart;
+    }
+    tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart + strLpReq.uiStrSpan;
+
+    MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
+    write_data(&canStrActCtrReqMsg);
+
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
+  }
+
+}
 
 
 

src/canbus/canbus.h

@@ -31,7 +31,11 @@
 #include <canbus/SftActCtrReq.h>
 #include <canbus/StrActCtrReq.h>
 #include <canbus/RBTManager.h>
-
+#include <canbus/PowerOnRequest.h>
+#include <canbus/ActuatorCtrLimit.h>
+#include <canbus/VehicleStart.h>
+#include <canbus/SlopeMode.h>
+#include <canbus/StrLoopReq.h>
 
 /**
  * @namespace robot::canbus
@@ -97,6 +101,8 @@ class Canbus : public robot::common::RobotApp{
   */
   void OnTimer(const ros::TimerEvent &event);
 
+  void StrLoopOnTimer(const ros::TimerEvent &event);  //转向机构循环指令定时器回调函数
+
   //define function pointer,there is a one-to-one mapping between target function and your defined function
   funCAN_Init_TYPE        funCAN_Init;
   funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
@@ -148,11 +154,27 @@ class Canbus : public robot::common::RobotApp{
   bool RBTManager(canbus::RBTManager::Request &req,
                         canbus::RBTManager::Response &res);
 
+  bool PowerOnRequest(canbus::PowerOnRequest::Request &req,
+                      canbus::PowerOnRequest::Response &res);
+
+  bool ActuatorCtrLimit(canbus::ActuatorCtrLimit::Request &req,
+                        canbus::ActuatorCtrLimit::Response &res);
+  bool VehicleStart(canbus::VehicleStart::Request &req,
+                     canbus::VehicleStart::Response &res);
+  bool SlopeMode(canbus::SlopeMode::Request &req,
+                 canbus::SlopeMode::Response &res);
+  bool StrLoopReq(canbus::StrLoopReq::Request &req,
+                 canbus::StrLoopReq::Response &res);
+
 private:
 
   int64_t last_timestamp_ = 0;
   ros::Timer timer_;
 
+  ros::Timer str_timer_;    //转向机构循环指令定时器
+  bool strTimerState = false;  //转向机构循环指令定时器标志位
+  int strLoopNum = 0;   //转向机构循环指令一套动作执行次数
+
   const double duration = 0.01;
 
   const double rate = 0.0001;  //0.0001s
@@ -189,6 +211,9 @@ private:
   // robot manager
   TPCANMsg canRobotManagerMsg;
 
+  canbus::StrLoopReq::Request strLpReq;  //转向机构循环指令
+  T_STR_ACT_CTR_REQ tStrActCtrReq ={0};   //转向机构循环指令中发送的转向执行机构请求
+
 };
 
 }  // namespace canbus

src/canbus/candatatype.h

@@ -1,11 +1,8 @@
-#pragma once
-//============================================================================
-// Name        : canDataType.h
-// Author      :
-// Version     :
-// Copyright   : SIASUN
-// Description : can2.0协议数据结构
-//============================================================================
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 
 #ifndef CAN_DATA_TYPE_H_
 #define CAN_DATA_TYPE_H_
@@ -471,7 +468,11 @@ typedef struct
 typedef struct
 {
     unsigned int uiSftType : 2;  //挡位类型
-    unsigned int : 11;
+    unsigned int : 1;
+    unsigned int uiRbtCfgMd:1;  //机器人配置模式
+    unsigned int:1;
+    unsigned int uiRbtAplySenro:1;  //机器人应用场景
+    unsigned int:7;
     int iStrMaxLimt : 19;					//转向极限值
     unsigned int : 6;
     unsigned int uiBrkMaxLimt : 10;		//制动行程最大限值
@@ -482,10 +483,12 @@ typedef struct
 //执行机构限值设置
 typedef struct
 {
-    unsigned int uiSftType;   //挡位类型
     unsigned int uiAccMaxLimt;		//油门行程最大限值
     unsigned int uiBrkMaxLimt;		//制动行程最大限值
     int iStrMaxLimt;				//转向极限值
+    unsigned int uiRbtAplySenro; //机器人应用场景
+    unsigned int uiRbtCfgMd;  //机器人配置模式
+    unsigned int uiSftType;   //挡位类型
 }T_ACT_CTR_LIMT;
 /*************力矩保护设置*******/
 typedef struct
@@ -500,8 +503,8 @@ typedef struct
 //力矩保护设置
 typedef struct
 {
-    unsigned int uiStrTorqSwitch;					//转向力矩限制开关
     unsigned int uiStrTorqLimit;		//转向力矩限值
+    unsigned int uiStrTorqSwitch;		//转向力矩限制开关
 }T_TRQ_PRTCT_STP;
 
 /*************制动机构力时间控制设置*******/
@@ -518,9 +521,9 @@ typedef struct
 //制动机构力时间控制设置
 typedef struct
 {
-    unsigned int uiBrkTm;					//控制时间
-    unsigned int uiBscFrc;     //力的基准值
     unsigned int uiFrcSpd;		//力增长速率
+    unsigned int uiBscFrc;     //力的基准值
+    unsigned int uiBrkTm;		//控制时间
 }T_BRK_FRC_CTR_STP;
 
 /*************转向三角波*******/
@@ -561,12 +564,12 @@ typedef union
 //转向三角波
 typedef struct
 {
-    unsigned int uiBrkTm;   //控制时间
-    int uiStrPstBsc;   //方向盘位置基准值
-    int uiStrSpdk;  //方向盘转速
-    unsigned int uiStrNum;  //个数
-    unsigned int uiStrTT;		//脉宽
     int uiStrTA;		//峰值
+    unsigned int uiStrTT;		//脉宽
+    unsigned int uiStrNum;  //个数
+    int uiStrSpdk;  //方向盘转速
+    int uiStrPstBsc;   //方向盘位置基准值
+    unsigned int uiBrkTm;   //控制时间
 }T_STR_TRNGL_LN_CTR;
 
 /*************转向正弦控制*******/
@@ -588,12 +591,12 @@ typedef struct
 //转向正弦控制
 typedef struct
 {
-    unsigned int uiBrkTm;   //控制时间
-    unsigned int uiStrDt;   //迟滞时间
-    unsigned int uiStrA;  //幅值
-    int uiStrFi;  //相位差
-    unsigned int uiStrFrtb;		//频率基值
     unsigned int uiStrFrtk;		//频率斜率
+    unsigned int uiStrFrtb;		//频率基值
+    int uiStrFi;  //相位差
+    unsigned int uiStrA;  //幅值
+    unsigned int uiStrDt;   //迟滞时间
+    unsigned int uiBrkTm;   //控制时间
 }T_STR_SIN_CTR;
 
 /*************油门机构位置时间控制*******/
@@ -610,9 +613,9 @@ typedef struct
 //油门机构位置时间控制
 typedef struct
 {
-    unsigned int uiBrkTm : 11;   //控制时间
-    unsigned int uiAccPstBsc : 11;   //油门位置基准值
     unsigned int uiAccSpdP : 10;  //油门速度
+    unsigned int uiAccPstBsc : 11;   //油门位置基准值
+    unsigned int uiBrkTm : 11;   //控制时间
 }T_ACC_PSTN_CTR_STP;
 
 /*************方向盘位置时间序列控制*******/
@@ -649,11 +652,11 @@ typedef union
 //方向盘位置时间序列控制
 typedef struct
 {
-    int uiStrAngReq;   //方向盘转角
-    unsigned int uiRelTime;   //相对时间
-    unsigned int uiNum;  //实际点序号
-    unsigned int uiTotalNum;  //总个数
     unsigned int uiSqncID;  //序列ID
+    unsigned int uiTotalNum;  //总个数
+    unsigned int uiNum;  //实际点序号
+    unsigned int uiRelTime;   //相对时间
+    int uiStrAngReq;   //方向盘转角
 }T_STR_PSTN_TM_CTR;
 
 /*************车辆一键启动*******/
@@ -670,6 +673,19 @@ typedef struct
     unsigned int uiVehSwReq;   //车辆一键启动
 }T_VEHICLE_START;
 
+/****坡度模板****/
+typedef struct
+{
+    unsigned int : 32;
+    unsigned int : 21;
+    int iSlopeReq : 11;   //坡度电压请求
+}T_CONTROL_CMD_70F;
+
+//坡度模板
+typedef struct
+{
+   int iSlopeReq;   //坡度电压请求
+}T_Slope_MODE;
 
 /******************************ICU-RCU状态输入******************************/
 /*************车辆状态1*******技术规范添加了部分定义******/
@@ -1001,13 +1017,13 @@ typedef struct
 //控制器上电状态
 typedef struct
 {
-    unsigned int uiRCUPowStat;			//RCU上电状态
-    unsigned int uiAccCtrPowStat;		//油门踏板电机控制器上电状态
-    unsigned int uiBrkCtrPowStat;		//制动电机控制器上电状态
     unsigned int uiClchCtrPowStat;		//离合电机控制器上电状态
-    unsigned int uiStrCtrPowStat;		//转向电机控制器上电状态
-    unsigned int uiXSftArmCtrPowStat;	//换挡臂X电机控制器上电状态
+    unsigned int uiBrkCtrPowStat;		//制动电机控制器上电状态
+    unsigned int uiAccCtrPowStat;		//油门踏板电机控制器上电状态
+    unsigned int uiRCUPowStat;			//RCU上电状态
     unsigned int uiYSftArmCtrPowStat;	//换挡臂Y电机控制器上电状态
+    unsigned int uiXSftArmCtrPowStat;	//换挡臂X电机控制器上电状态
+    unsigned int uiStrCtrPowStat;		//转向电机控制器上电状态
 }T_CTR_POW_STATUS;
 
 #define	POW_STAT_OFF	0	//未上电
@@ -1042,14 +1058,14 @@ typedef struct
 //执行机构使能状态
 typedef struct
 {
-    unsigned int uiAccMotEnStat;		//油门踏板电机使能状态
-    unsigned int uiBrkMotEnStat;		//制动电机使能状态
-    unsigned int uiStrMotEnStat;		//转向电机使能状态
     unsigned int uiXSftMotEnStat;		//换挡臂X电机使能状态
-    unsigned int uiYSftMotEnStat;		//换挡臂Y电机使能状态
-    unsigned int uiClchMotEnStat;		//离合电机使能状态
-    unsigned int uiAccClchCtrStat;		//油门踏板离合器通电状态
+    unsigned int uiStrMotEnStat;		//转向电机使能状态
+    unsigned int uiBrkMotEnStat;		//制动电机使能状态
+    unsigned int uiAccMotEnStat;		//油门踏板电机使能状态
     unsigned int uiPauseEnStat;			//系统软急停使能状态
+    unsigned int uiAccClchCtrStat;		//油门踏板离合器通电状态
+    unsigned int uiClchMotEnStat;		//离合电机使能状态
+    unsigned int uiYSftMotEnStat;		//换挡臂Y电机使能状态
     unsigned int uiBrkHVFFStat;			//制动机构手阀状态
 }T_ACT_ENABLE_STATUS;
 
@@ -1333,26 +1349,29 @@ typedef struct
     unsigned int uiRCUAlive : 4;			//系统RCU心跳
     unsigned int : 4;
     unsigned int uiCtrMdStat : 3;			//实际系统控制状态
-    unsigned int : 5;
+    unsigned int : 1;
+    unsigned int uiRbtCfgMdStat:1;          //机器人实际配置模式
+    unsigned int:3;
 }T_STATE_OUT_6ED;
 
 //系统运行状态参数反馈2
 typedef struct
 {
+    unsigned int uiRbtCfgMdStat;     //机器人实际配置模式
     unsigned int uiCtrMdStat;		//实际系统控制状态
     unsigned int uiRCUAlive;		//系统RCU心跳
-    unsigned int uiAccCtrAlive;		//油门踏板控制器心跳
     unsigned int uiAccMotClbr;		//油门踏板控制电机标零状态
-    unsigned int uiBrkCtrAlive;		//制动控制器心跳
+    unsigned int uiAccCtrAlive;		//油门踏板控制器心跳
     unsigned int uiBrkMotClbr;		//制动控制电机标零状态
-    unsigned int uiClchCtrAlive;	//离合控制器心跳
+    unsigned int uiBrkCtrAlive;		//制动控制器心跳
     unsigned int uiClchMotClbr;		//离合控制电机标零状态
-    unsigned int uiStrCtrAlive;		//转向控制器心跳
+    unsigned int uiClchCtrAlive;	//离合控制器心跳
     unsigned int uiStrMotClbr;		//转向控制电机标零状态
-    unsigned int uiXSftArmCtrAlive;	//换挡臂X控制器心跳
+    unsigned int uiStrCtrAlive;		//转向控制器心跳
     unsigned int uiXSftMotClbr;		//换挡臂X控制标零状态
-    unsigned int uiYSftArmCtrAlive;	//换挡臂Y控制器心跳
+    unsigned int uiXSftArmCtrAlive;	//换挡臂X控制器心跳
     unsigned int uiYSftMotClbr;		//换挡臂Y控制标零状态
+    unsigned int uiYSftArmCtrAlive;	//换挡臂Y控制器心跳
 }T_SYS_INFO_2;
 
 #define	CTR_MD_STAT_AUTO_LEARN		0	//实际系统控制状态-自动学习模式

src/canbus/messagecoder.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "messagecoder.h"
 #include"candatatype.h"
 #include"string.h"
@@ -23,10 +28,10 @@ void MessageCoder::CharReverse(unsigned char * s, int n)
 void MessageCoder::CanMsgAnalyseID_5F2(unsigned char * ucData, T_ACTUATOR_ENABLE_REQUEST *tActEnableReq)
 {
     T_CONTROL_CMD_5F2 tCtrCmd5F2 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd5F2, ucCanMsgSrc, CAN_MSG_LEN);
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd5F2, ucData, CAN_MSG_LEN);
     tActEnableReq->uiAccClchCtrReq = tCtrCmd5F2.uiAccClchCtrReq;
     tActEnableReq->uiAccMotEnReq = tCtrCmd5F2.uiAccMotEnReq;
     tActEnableReq->uiBrkMotEnReq = tCtrCmd5F2.uiBrkMotEnReq;
@@ -46,10 +51,10 @@ void MessageCoder::CanMsgAnalyseID_5F5(unsigned char * ucData, T_STR_ACT_CTR_REQ
     T_CONTROL_CMD_5F5 tCtrCmd5F5 = { 0 };
     T_CONTROL_CMD_5F5_UNION tCtrCmd5F5Union;
 
-    unsigned char ucCanMsgSrc[8] = { 0 };
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd5F5, ucCanMsgSrc, CAN_MSG_LEN);
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd5F5, ucData, CAN_MSG_LEN);
 
     memset(&tCtrCmd5F5Union, 0, sizeof(T_CONTROL_CMD_5F5_UNION));
     tCtrCmd5F5Union.tCtrCmd5F5Src.iStrSpdCtrReqL = tCtrCmd5F5.iStrSpdCtrReqL;
@@ -65,11 +70,11 @@ void MessageCoder::CanMsgAnalyseID_5F5(unsigned char * ucData, T_STR_ACT_CTR_REQ
 void MessageCoder::CanMsgAnalyseID_6F1(unsigned char * ucData, T_VEHICLE_STATUS_1 * tVehicleStat1)
 {
     T_STATE_IN_6F1 tStatIn6F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F1, ucData, CAN_MSG_LEN);
 
     tVehicleStat1->uiAccPsng = tStatIn6F1.uiAccPsng;
     tVehicleStat1->uiBrkPsng = tStatIn6F1.uiBrkPsng;
@@ -86,11 +91,11 @@ void MessageCoder::CanMsgAnalyseID_6F2(unsigned char * ucData, T_VEHICLE_STATUS_
 {
     T_STATE_IN_6F2 tStatIn6F2 = { 0 };
     T_STATE_IN_6F2_UNION tStatIn6F2Union;
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F2, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F2, ucData, CAN_MSG_LEN);
 
     memset(&tStatIn6F2Union, 0, sizeof(T_STATE_IN_6F2_UNION));
     tStatIn6F2Union.tStateIn6F2Src.iStrAngGrdL = tStatIn6F2.iStrAngGrdL;
@@ -106,11 +111,11 @@ void MessageCoder::CanMsgAnalyseID_6F2(unsigned char * ucData, T_VEHICLE_STATUS_
 void MessageCoder::CanMsgAnalyseID_6F3(unsigned char * ucData, T_VEHICLE_STATUS_3 * tVehicleStat3)
 {
     T_STATE_IN_6F3 tStatIn6F3 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F3, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F3, ucData, CAN_MSG_LEN);
 
     tVehicleStat3->uiVehBatQut = tStatIn6F3.uiVehBatQut;
     tVehicleStat3->uiVehBatCur = tStatIn6F3.uiVehBatCur;
@@ -121,11 +126,11 @@ void MessageCoder::CanMsgAnalyseID_6F3(unsigned char * ucData, T_VEHICLE_STATUS_
 void MessageCoder::CanMsgAnalyseID_6F4(unsigned char * ucData, T_COMPUTER_STATUS * tComputerStat)
 {
     T_STATE_IN_6F4 tStatIn6F4 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F4, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F4, ucData, CAN_MSG_LEN);
 
     tComputerStat->uiCmpStat = tStatIn6F4.uiCmpStat;
 }
@@ -134,11 +139,11 @@ void MessageCoder::CanMsgAnalyseID_6F4(unsigned char * ucData, T_COMPUTER_STATUS
 void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG * tRbtActPsng)
 {
     T_STATE_OUT_6E0 tStatout6E0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E0, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E0_UNION tStateOut6E0Union;
     memset(&tStateOut6E0Union, 0, sizeof(T_STATE_OUT_6E0_UNION));
@@ -155,11 +160,11 @@ void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E1(unsigned char * ucData, T_ACC_BRK_PSNG * tAccBrkPsng)
 {
     T_STATE_OUT_6E1 tStatout6E1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E1, ucData, CAN_MSG_LEN);
 
     tAccBrkPsng->uiClbtAccPsng0 = tStatout6E1.uiClbtAccPsng0;
     tAccBrkPsng->uiClbtAccPsngMax = tStatout6E1.uiClbtAccPsngMax;
@@ -171,11 +176,11 @@ void MessageCoder::CanMsgAnalyseID_6E1(unsigned char * ucData, T_ACC_BRK_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E2(unsigned char * ucData, T_STR_LRN_PSNG * tStrLrnPsng)
 {
     T_STATE_OUT_6E2 tStatout6E2 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E2, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E2, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E2_UNION tStateOut6E2Union;
     memset(&tStateOut6E2Union,0,sizeof(T_STATE_OUT_6E2_UNION));
@@ -191,11 +196,11 @@ void MessageCoder::CanMsgAnalyseID_6E2(unsigned char * ucData, T_STR_LRN_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E3(unsigned char * ucData, T_SFT_LRN_PST_1 * tSftLrnPst1)
 {
     T_STATE_OUT_6E3 tStatout6E3 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E3, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E3, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E3_UNION tStateOut6E3Union;
     memset(&tStateOut6E3Union,0,sizeof(T_STATE_OUT_6E3_UNION));
@@ -215,11 +220,11 @@ void MessageCoder::CanMsgAnalyseID_6E3(unsigned char * ucData, T_SFT_LRN_PST_1 *
 void MessageCoder::CanMsgAnalyseID_6E4(unsigned char * ucData, T_CTR_POW_STATUS * tCtrPowStatus)
 {
     T_STATE_OUT_6E4 tStatout6E4 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E4, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E4, ucData, CAN_MSG_LEN);
 
     tCtrPowStatus->uiAccCtrPowStat = tStatout6E4.uiAccCtrPowStat;
     tCtrPowStatus->uiBrkCtrPowStat = tStatout6E4.uiBrkCtrPowStat;
@@ -234,11 +239,11 @@ void MessageCoder::CanMsgAnalyseID_6E4(unsigned char * ucData, T_CTR_POW_STATUS
 void MessageCoder::CanMsgAnalyseID_6E5(unsigned char * ucData, T_ACT_ENABLE_STATUS * tActEnableStatus)
 {
     T_STATE_OUT_6E5 tStatout6E5 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E5, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E5, ucData, CAN_MSG_LEN);
 
     tActEnableStatus->uiAccClchCtrStat = tStatout6E5.uiAccClchCtrStat;
     tActEnableStatus->uiAccMotEnStat = tStatout6E5.uiAccMotEnStat;
@@ -255,11 +260,11 @@ void MessageCoder::CanMsgAnalyseID_6E5(unsigned char * ucData, T_ACT_ENABLE_STAT
 void MessageCoder::CanMsgAnalyseID_6E6(unsigned char * ucData, T_ACC_CTR_STATUS * tAccCtrStatus)
 {
     T_STATE_OUT_6E6 tStatout6E6 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E6, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E6, ucData, CAN_MSG_LEN);
 
     tAccCtrStatus->iAccDspSpd = tStatout6E6.iAccDspSpd;
     tAccCtrStatus->iAccMotSpd = tStatout6E6.iAccMotSpd;
@@ -270,11 +275,11 @@ void MessageCoder::CanMsgAnalyseID_6E6(unsigned char * ucData, T_ACC_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E7(unsigned char * ucData, T_BRK_CTR_STATUS * tBrkCtrStatus)
 {
     T_STATE_OUT_6E7 tStatout6E7 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E7, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E7, ucData, CAN_MSG_LEN);
 
     tBrkCtrStatus->iBrkDspSpd = tStatout6E7.iBrkDspSpd;
     tBrkCtrStatus->iBrkFoc = tStatout6E7.iBrkFoc;
@@ -288,11 +293,11 @@ void MessageCoder::CanMsgAnalyseID_6E7(unsigned char * ucData, T_BRK_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E8(unsigned char * ucData, T_CLCH_CTR_STATUS * tClchCtrStatus)
 {
     T_STATE_OUT_6E8 tStatout6E8 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E8, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E8, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E8_UNION tStateOut6E8Union;
     memset(&tStateOut6E8Union,0,sizeof(T_STATE_OUT_6E8_UNION));
@@ -309,11 +314,11 @@ void MessageCoder::CanMsgAnalyseID_6E8(unsigned char * ucData, T_CLCH_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E9(unsigned char * ucData, T_STR_STATUS * tStrStatus)
 {
     T_STATE_OUT_6E9 tStatout6E9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E9, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E9_UNION tStateOut6E9Union;
     memset(&tStateOut6E9Union,0,sizeof(T_STATE_OUT_6E9_UNION));
@@ -331,11 +336,11 @@ void MessageCoder::CanMsgAnalyseID_6E9(unsigned char * ucData, T_STR_STATUS * tS
 void MessageCoder::CanMsgAnalyseID_6EA(unsigned char * ucData, T_SFT_STATUS * tSftStatus)
 {
     T_STATE_OUT_6EA tStatout6EA = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EA, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EA, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6EA_UNION tStateOut6EAUnion;
     memset(&tStateOut6EAUnion, 0, sizeof(T_STATE_OUT_6EA_UNION));
@@ -357,11 +362,11 @@ void MessageCoder::CanMsgAnalyseID_6EA(unsigned char * ucData, T_SFT_STATUS * tS
 void MessageCoder::CanMsgAnalyseID_6EB(unsigned char * ucData, T_SYS_INFO_1 * tSysInfo1)
 {
     T_STATE_OUT_6EB tStatout6EB = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EB, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EB, ucData, CAN_MSG_LEN);
 
     tSysInfo1->uiAccMotCur = tStatout6EB.uiAccMotCur;
     tSysInfo1->uiBrkMotCur = tStatout6EB.uiBrkMotCur;
@@ -376,11 +381,11 @@ void MessageCoder::CanMsgAnalyseID_6EB(unsigned char * ucData, T_SYS_INFO_1 * tS
 void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tSysInfo2)
 {
     T_STATE_OUT_6ED tStatout6ED = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6ED, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6ED, ucData, CAN_MSG_LEN);
 
     tSysInfo2->uiAccCtrAlive = tStatout6ED.uiAccCtrAlive;
     tSysInfo2->uiAccMotClbr = tStatout6ED.uiAccMotClbr;
@@ -396,6 +401,7 @@ void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tS
     tSysInfo2->uiXSftMotClbr = tStatout6ED.uiXSftMotClbr;
     tSysInfo2->uiYSftArmCtrAlive = tStatout6ED.uiYSftArmCtrAlive;
     tSysInfo2->uiYSftMotClbr = tStatout6ED.uiYSftMotClbr;
+    tSysInfo2->uiRbtCfgMdStat = tStatout6ED.uiRbtCfgMdStat;
 
 }
 
@@ -403,11 +409,11 @@ void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tS
 void MessageCoder::CanMsgAnalyseID_6EE(unsigned char * ucData, T_SYS_LRN * tSysLrn)
 {
     T_STATE_OUT_6EE tStatout6EE = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EE, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EE, ucData, CAN_MSG_LEN);
 
     tSysLrn->uiAcc0LrnStat = tStatout6EE.uiAcc0LrnStat;
     tSysLrn->uiAccLrnStat = tStatout6EE.uiAccLrnStat;
@@ -428,11 +434,11 @@ void MessageCoder::CanMsgAnalyseID_6EE(unsigned char * ucData, T_SYS_LRN * tSysL
 void MessageCoder::CanMsgAnalyseID_6EF(unsigned char * ucData, T_SFT_LRN_PST_2 * tSFtLrnPst2)
 {
     T_STATE_OUT_6EF tStatout6EF = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EF, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EF, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6EF_UNION tStateOut6EFUnion;
     memset(&tStateOut6EFUnion, 0, sizeof(T_STATE_OUT_6EF_UNION));
@@ -452,11 +458,11 @@ void MessageCoder::CanMsgAnalyseID_6EF(unsigned char * ucData, T_SFT_LRN_PST_2 *
 void MessageCoder::CanMsgAnalyseID_6D0(unsigned char * ucData, T_SYS_MONITOR * tSysMonitor)
 {
     T_STATE_OUT_6D0 tStatout6D0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6D0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6D0, ucData, CAN_MSG_LEN);
 
     tSysMonitor->iCtrTmp = tStatout6D0.iCtrTmp;
     tSysMonitor->uiAccClchAbnStat = tStatout6D0.uiAccClchAbnStat;
@@ -483,11 +489,11 @@ void MessageCoder::CanMsgAnalyseID_6D0(unsigned char * ucData, T_SYS_MONITOR * t
 void MessageCoder::CanMsgAnalyseID_6D1(unsigned char * ucData, T_SYS_ERR * tSysErr)
 {
     T_STATE_OUT_6D1 tStatOut6D1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatOut6D1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatOut6D1, ucData, CAN_MSG_LEN);
 
     tSysErr->uiSysErra = tStatOut6D1.uiSysErra;
     tSysErr->uiSysErrb = tStatOut6D1.uiSysErrb;
@@ -521,11 +527,11 @@ void MessageCoder::CanMsgAnalyseID_6D1(unsigned char * ucData, T_SYS_ERR * tSysE
 void MessageCoder::CanMsgAnalyseID_6D7(unsigned char * ucData, T_STEER_PST_TIME_REQ * tSteerPstTimeReq)
 {
     T_CONTROL_CMD_6D7 tCtrCmd6D7 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd6D7, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd6D7, ucData, CAN_MSG_LEN);
 
     tSteerPstTimeReq->uiNumReq = tCtrCmd6D7.uiNumReq;
     tSteerPstTimeReq->uiSeqIdReq = tCtrCmd6D7.uiSeqIdReq;
@@ -535,11 +541,11 @@ void MessageCoder::CanMsgAnalyseID_6D7(unsigned char * ucData, T_STEER_PST_TIME_
 void MessageCoder::CanMsgAnalyseID_2E0(unsigned char * ucData, T_ESTOP_INFO * tEstopInfo)
 {
     T_ESTOP_2E0 tEstop2E0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tEstop2E0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tEstop2E0, ucData, CAN_MSG_LEN);
 
     tEstopInfo->uiEstpAbnStat = tEstop2E0.uiEstpAbnStat;
     tEstopInfo->uiEstpAlive = tEstop2E0.uiEstpAlive;
@@ -551,11 +557,11 @@ void MessageCoder::CanMsgAnalyseID_2E0(unsigned char * ucData, T_ESTOP_INFO * tE
 void MessageCoder::CanMsgAnalyseID_2E1(unsigned char * ucData, T_ESTOP_ERR * tEstopErr)
 {
     T_ESTOP_2E1 tEstop2E1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tEstop2E1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tEstop2E1, ucData, CAN_MSG_LEN);
 
     tEstopErr->uiEspErra = tEstop2E1.uiEspErra;
     tEstopErr->uiEspErrb = tEstop2E1.uiEspErrb;
@@ -573,11 +579,11 @@ void MessageCoder::CanMsgAnalyseID_2E1(unsigned char * ucData, T_ESTOP_ERR * tEs
 void MessageCoder::CanMsgAnalyseErr(unsigned char * ucData, T_MOT_CTR_ERR * tMotCtrErr)
 {
     T_CTR_ERR_CODE tCtrErrCode = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrErrCode, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrErrCode, ucData, CAN_MSG_LEN);
 
     tMotCtrErr->uiErra = tCtrErrCode.a;
     tMotCtrErr->uiErraa = tCtrErrCode.aa;
@@ -620,10 +626,10 @@ void MessageCoder::CanMsgAnalyseErr(unsigned char * ucData, T_MOT_CTR_ERR * tMot
 void MessageCoder::OBDAnalyseID_1A1(unsigned char *ucData, T_OBD_ACC_PSNG *tOBDAccPsng)
 {
     T_OBD_1A1 tOBD1A1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1A1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1A1, ucData, CAN_MSG_LEN);
 
     tOBDAccPsng->obdAccPsng = (tOBD1A1.obdAccPsng *1000) / OBD_ACC_PSNG_RATIO;
 
@@ -633,10 +639,10 @@ void MessageCoder::OBDAnalyseID_1A1(unsigned char *ucData, T_OBD_ACC_PSNG *tOBDA
 void MessageCoder::OBDAnalyseID_0F1(unsigned char *ucData, T_OBD_BRK_PSNG *tOBDBrkPsng)
 {
     T_OBD_0F1 tOBD0F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD0F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD0F1, ucData, CAN_MSG_LEN);
 
     tOBDBrkPsng->obdBrkPsng = (tOBD0F1.obdBrkPsng *1000) / OBD_BRK_PSNG_RATIO;
 }
@@ -645,10 +651,10 @@ void MessageCoder::OBDAnalyseID_0F1(unsigned char *ucData, T_OBD_BRK_PSNG *tOBDB
 void MessageCoder::OBDAnalyseID_135(unsigned char *ucData, T_OBD_SFT_PSNG *tOBDSftPsng)
 {
     T_OBD_135 tOBD135 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD135, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD135, ucData, CAN_MSG_LEN);
 
     switch(tOBD135.obdSftPsng)
     {
@@ -674,10 +680,10 @@ void MessageCoder::OBDAnalyseID_135(unsigned char *ucData, T_OBD_SFT_PSNG *tOBDS
 void MessageCoder::OBDAnalyseID_1F1(unsigned char *ucData, T_OBD_VEH_BRK_STAT *tOBDVehBrkStat)
 {
     T_OBD_1F1 tOBD1F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1F1, ucData, CAN_MSG_LEN);
 
     tOBDVehBrkStat->obdVehHBrkStat = tOBD1F1.obdVehHBrkStat;
 }
@@ -686,10 +692,10 @@ void MessageCoder::OBDAnalyseID_1F1(unsigned char *ucData, T_OBD_VEH_BRK_STAT *t
 void MessageCoder::OBDAnalyseID_121(unsigned char *ucData, T_OBD_VEH_SW_STAT *tOBDVehSwStat)
 {
     T_OBD_121 tOBD121 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD121, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD121, ucData, CAN_MSG_LEN);
 
     switch (tOBD121.obdVehSwStat) {
     case 4:
@@ -714,10 +720,10 @@ void MessageCoder::OBDAnalyseID_121(unsigned char *ucData, T_OBD_VEH_SW_STAT *tO
 void MessageCoder::OBDAnalyseID_1E5(unsigned char *ucData, T_OBD_STR_ANG *tOBDStrAng)
 {
     T_OBD_1E5 tOBD1E5 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1E5, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1E5, ucData, CAN_MSG_LEN);
 
     T_OBD_1E5_UNION tObd1e5Union;
     memset(&tObd1e5Union,0,sizeof(T_OBD_1E5_UNION));
@@ -739,10 +745,10 @@ void MessageCoder::OBDAnalyseID_0C9(unsigned char *ucData, T_OBD_ENG_SPD *tOBDEn
 //    tOBDEngSpd->obdEngSpd = tOBD0C9.obdEngSpd / OBD_ENG_SPD_RATIO;
 
     T_OBD_0C9 tOBD0C9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD0C9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD0C9, ucData, CAN_MSG_LEN);
 
     T_OBD_0C9_UNION tObd0c9Union;
     memset(&tObd0c9Union,0,sizeof(T_OBD_0C9_UNION));
@@ -764,10 +770,10 @@ void MessageCoder::OBDAnalyseID_3E9(unsigned char *ucData, T_OBD_VEH_SPD *tOBDVe
 //    tOBDVehSpd->obdVehSpd = tOBD3E9.obdVehSpd / OBD_VEH_SPD_RATIO;
 
     T_OBD_3E9 tOBD3E9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD3E9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD3E9, ucData, CAN_MSG_LEN);
 
     T_OBD_3E9_UNION tObd3e9Union;
     memset(&tObd3e9Union,0,sizeof(T_OBD_3E9_UNION));
@@ -1023,6 +1029,8 @@ void MessageCoder::CanMsgPackageID_5FC(T_ACT_CTR_LIMT * tActCtrLimt, TPCANMsg *t
     tCtrCmd5FC.uiAccMaxLimt = tActCtrLimt->uiAccMaxLimt;
     tCtrCmd5FC.uiBrkMaxLimt = tActCtrLimt->uiBrkMaxLimt;
     tCtrCmd5FC.uiSftType = tActCtrLimt->uiSftType;
+    tCtrCmd5FC.uiRbtAplySenro = tActCtrLimt->uiRbtAplySenro;
+    tCtrCmd5FC.uiRbtCfgMd = tActCtrLimt->uiRbtCfgMd;
 
     memcpy(ucCanMsgSrc, &tCtrCmd5FC, CAN_MSG_LEN);
     CharReverse(ucCanMsgSrc,8);
@@ -1174,6 +1182,22 @@ void MessageCoder::CanMsgPackageID_7F7(T_VEHICLE_START * tVehicleStart, TPCANMsg
     tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
 }
 
+//坡度模板--数据组包
+void MessageCoder::CanMsgPackageID_70F(T_Slope_MODE *tSlopeMODE,TPCANMsg *tCanMsg)
+{
+    T_CONTROL_CMD_70F tCtrCmd70F = {0};
+    unsigned char ucCanMsgSrc[8] = {0};
+
+    tCtrCmd70F.iSlopeReq = tSlopeMODE->iSlopeReq;
+
+    memcpy(ucCanMsgSrc, &tCtrCmd70F, CAN_MSG_LEN);
+    CharReverse(ucCanMsgSrc,8);
+    memcpy(tCanMsg->DATA, ucCanMsgSrc, CAN_MSG_LEN);
+    tCanMsg->ID = robot::common::SlopeMODE;
+    tCanMsg->LEN = 8;
+    tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
+}
+
 
 //车辆状态1--数据组包
 void MessageCoder::CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg)
@@ -1509,6 +1533,9 @@ void MessageCoder::CanMsgPackage(void * p, int id, TPCANMsg *tCanMsg)
     case robot::common::VehicleStart://车辆一键启动
         CanMsgPackageID_7F7((T_VEHICLE_START *)p,tCanMsg);
         break;
+    case robot::common::SlopeMODE:  //坡度模板
+        CanMsgPackageID_70F((T_Slope_MODE *)p,tCanMsg);
+        break;
     case robot::common::VehicleStatus1://车辆状态1
         CanMsgPackageID_6F1((T_VEHICLE_STATUS_1 *)p,tCanMsg);
         break;

src/canbus/messagecoder.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef MESSAGECODER_H
 #define MESSAGECODER_H
 
@@ -175,6 +180,9 @@ private:
     //车辆一键启动--数据组包
     static void CanMsgPackageID_7F7(T_VEHICLE_START *tVehicleStart,TPCANMsg *tCanMsg);
 
+    //坡度模板--数据组包
+    static void CanMsgPackageID_70F(T_Slope_MODE *tSlopeMODE,TPCANMsg *tCanMsg);
+
     //车辆状态1--数据组包
     static void CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg);
 

src/canbus/srv/ActuatorCtrLimit.srv

@@ -0,0 +1,8 @@
+uint32 uiAccMaxLimit		#油门行程最大限值
+uint32 uiBrkMaxLimit		#制动行程最大限值
+int32 iStrMaxLimit		#转向极限值
+uint32 uiSftType	        #档位类型
+uint32 uiRbtCfgMd	        #机器人配置模式
+uint32 uiRbtAplySenro	        #机器人应用场景
+---
+uint32 uiReturn

src/canbus/srv/PowerOnRequest.srv

@@ -0,0 +1,9 @@
+uint32 uiRCUPowReq		#RCU上电
+uint32 uiAccCtrPowReq		#油门机构使用请求
+uint32 uiBrkCtrPowReq		#制动机构使用请求
+uint32 uiClchCtrPowReq	        #离合机构使用请求
+uint32 uiStrCtrPowReq	        #转向机构使用请求
+uint32 uiXSftArmCtrPowReq	#换挡臂X电机使用请求
+uint32 uiYSftArmCtrPowReq	#换挡臂Y电机使用请求
+---
+uint32 uiReturn

src/canbus/srv/SlopeMode.srv

@@ -0,0 +1,3 @@
+int32 iSlopeRequest		#坡度电压请求
+---
+uint32 uiReturn

src/canbus/srv/StrLoopReq.srv

@@ -0,0 +1,11 @@
+int32 iStrAngStart		#方向盘转角开始位置
+int32 iStrAngEnd		#方向盘转角终止位置
+uint32 uiStrCtrMdReq		#方向盘控制模式请求
+int32 iStrSpdCtrReq		#方向盘转速控制请求
+int32 iStrTrqCtrReq		#方向盘转矩控制请求
+uint32 uiStrTimeInterval        #时间间隔
+uint32 uiStrSpan                #跨度间隔
+uint32 uiStrFrequency           #往复次数
+bool   bStart                   #启动标志位
+---
+uint32 uiReturn

src/canbus/srv/VehicleStart.srv

@@ -0,0 +1,3 @@
+uint32 uiVehSwReq		#车辆一键启动请求
+---
+uint32 uiReturn

src/common/canmsgobdmsgid.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef CANMSGOBDMSGID_H
 #define CANMSGOBDMSGID_H
 
@@ -46,6 +51,8 @@ const int AccPositionCtrStp = 0x7F5;
 const int SteerPositionTimeCtr = 0x7F6;
 //车辆一键启动
 const int VehicleStart = 0x7F7;
+//坡度模板
+const int SlopeMODE = 0x70F;
 //车辆状态1
 const int VehicleStatus1 = 0x6F1;
 //车辆状态2

src/common/message.h

@@ -1,7 +1,6 @@
-
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 
@@ -52,6 +51,8 @@ const std::string StreData = "streData";
 const std::string EiData = "eiData";
 
 const std::string EeData = "eeData";
+
+const std::string PowerData = "powerData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------
@@ -68,6 +69,16 @@ const std::string PedalActCtrService = "pedalactctrservice";
 const std::string CommandModeCmdService = "commandmodecmdservice";
 
 const std::string RBTManagerService = "rbtmanagerservice";
+
+const std::string PowerOnService = "poweronservice";
+
+const std::string ActuatorCtrLimitService = "actuatorctrlimitservice";
+
+const std::string VehicleStartService = "vehiclestartservice";
+
+const std::string SlopeModeService = "slopemodeservice";
+
+const std::string StrLoopService = "strloopservice";
 // -----------------services ----------------------------
 
 

src/decision/decision.cpp

@@ -44,19 +44,19 @@ int Decision::Start(){
     ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
                                                                 &Decision::CommandRequestService, this);
     // under ros subscriber
-    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 200,
+    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
                                                           &Decision::RTKLocationCallback, this);
 
-    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 200, &Decision::OBDCallback, this);
+    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 1, &Decision::OBDCallback, this);
 
-    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 200, &Decision::RCUModeCallback, this);
+    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 1, &Decision::RCUModeCallback, this);
 
     // under ros publish
-    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
+    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 100);
 
-    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 200);
+    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 100);
 
-    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 200);
+    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 100);
 
     // under ros service of client
     clientSftActCtrReq = node_handle_->serviceClient<canbus::SftActCtrReq>(robot::common::ShiftActCtrService);
@@ -65,7 +65,7 @@ int Decision::Start(){
 
     clientStrActCtrReq = node_handle_->serviceClient<canbus::StrActCtrReq>(robot::common::SteerActCtrService);
 
-    ros::AsyncSpinner spinner(2); // thread_count The number of threads to use.  A value of 0 means to use the number of processor cores
+    ros::AsyncSpinner spinner(3); // thread_count The number of threads to use.  A value of 0 means to use the number of processor cores
     spinner.start();
     ros::waitForShutdown();
     LOG(INFO)<<Name()<<"  start() end.";
@@ -86,24 +86,26 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
     if (req.Cmd == CMD_AUTODRIE_START)
     {
         LoadPlanRoute();
-        double breakCtr = -50; // 踩刹车到50%
+        throttle_Last = -70; // 踩刹车到70%
         ros::Rate loop_rate(1);
         while(ros::ok())
         {
-            if (SpeedControl(breakCtr) == 1)
+            if (SpeedControl(throttle_Last) == 1)
             {
-                LOG(INFO)<<Name()<<"CommandRequestService break control 50% excute success.";
+                LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
                 if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                     break;
             }
             else
             {
-                LOG(ERROR)<<Name()<<"CommandRequestService break control 50% excute error.";
+                LOG(ERROR)<<Name()<<"CommandRequestService break control 70% excute error.";
                 return false;
             }
             ros::spinOnce();
             loop_rate.sleep();
         }
+        ptsx.clear();
+        ptsy.clear();
         ptsx.push_back(0);
         ptsy.push_back(0);
         ptsx.push_back(0);
@@ -155,29 +157,29 @@ void Decision::LoadPlanRoute()
 // 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
 void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 {
-     switch (iCommandCmd) {
-     case CMD_PLAN_START:
-         ExcutePlanRoute(msg);
-         break;
-     case CMD_AUTODRIE_START:
-         ExcuteSelfDriving(msg);
-         break;
-     case CMD_PLAN_END:
-         StopPlanRoute(msg);
-         break;
-     case CMD_AUTODRIE_END:
-         CarAutoDriveStop();
-         break;
-     default:
-         break;
-     }
+    switch (iCommandCmd) {
+    case CMD_PLAN_START:
+        ExcutePlanRoute(msg);
+        break;
+    case CMD_AUTODRIE_START:
+        ExcuteSelfDriving(msg);
+        break;
+    case CMD_PLAN_END:
+        StopPlanRoute(msg);
+        break;
+    case CMD_AUTODRIE_END:
+        CarAutoDriveStop();
+        break;
+    default:
+        break;
+    }
 }
 
 
 void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
 {
     fCarNowSpeed = msg->VehSpd;
-    //LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
+    LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
 }
 
 void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
@@ -189,7 +191,7 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
 //自动生成测试用例
 void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-    if (iAbandon< 20){
+    if (iAbandon< 40){ // 2 second excute
         iAbandon ++;
         return;
     }
@@ -205,16 +207,13 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
         if (iRulesNowNumber == 1)
             strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
         else{
-            if (fCarNowSpeed > fCarLastSpeed)
-            {
+            if (fCarNowSpeed > fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
             }
-            else if (fCarNowSpeed < fCarLastSpeed)
-            {
+            else if (fCarNowSpeed < fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
             }
-            else if (fCarNowSpeed == fCarLastSpeed)
-            {
+            else if (fCarNowSpeed == fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
             }
        }
@@ -225,8 +224,7 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
             iLocalNowNumber--;
             iRulesNowNumber--;
        }
-       else
-       {
+       else{
            decision::localizationMsg localMsg;
            localMsg.Locationid = iLocalNowNumber;
            localMsg.Longitude = msg->Longitude;
@@ -253,16 +251,13 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
      iLocalNowNumber++;
      iRulesNowNumber++;
      std::stringstream strRuleContent;
-     if (fCarNowSpeed > fCarLastSpeed)
-     {
+     if (fCarNowSpeed > fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
      }
-     else if (fCarNowSpeed < fCarLastSpeed)
-     {
+     else if (fCarNowSpeed < fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
      }
-     else if (fCarNowSpeed == fCarLastSpeed)
-     {
+     else if (fCarNowSpeed == fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
      }
     int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
@@ -279,8 +274,7 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 {
    // 判断当前RCU是否为自动驾驶模式
-    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
-    {
+    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE){
         LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
         return;
     }
@@ -294,8 +288,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
             (((ptsx[1] - ptsx[0])< 0)&&((px - ptsx[1])< 0));
     bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
             (((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
-    if (bRetLon|| bRetLat)
-    {       
+    if (bRetLon|| bRetLat){       
         std::vector<LOCATION_RULE_INFO> tempInfo;
         tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
         ptsx.clear();
@@ -310,8 +303,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
         UpdataCarRuleResults();       
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
     }
-    else
-    {
+    else{
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
     }
     double psi = deg2rad(msg->AngleDeviated + 90);
@@ -344,8 +336,8 @@ bool  Decision::CarAutoDriveStop()
 
         if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
             iCommandCmd = CMD_AUTODRIE_END;
-            double breakCtr = -50; // 踩刹车到50%
-            iLocalNowNumber ++;
+            int breakCtr = -70; // 踩刹车到70%
+            //iLocalNowNumber ++;
             while(true){
                 if (SpeedControl(breakCtr) == 1)
                 {
@@ -353,6 +345,9 @@ bool  Decision::CarAutoDriveStop()
                         break;
                 }
             }
+            if (iRulesNowNumber == iRulesTotalNumbers)
+                return true;
+            iRulesNowNumber ++;
             decision::rulesMsg ruleMsg;
             if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
             {
@@ -427,21 +422,15 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      // 这个地方有延迟，取第三个
      auto vars = mpc.Solve(state, coeffs);
      // 展示MPC预测的路径
-//     vector<double> mpc_x_vals=result[0];
-//     vector<double> mpc_y_vals=result[1];
-//     std::vector<double>::iterator iter_start = mpc_x_vals.begin();
-//     std::vector<double>::iterator iter_end = mpc_x_vals.end();
-//     std::vector<double>::iterator iter;
-//     int i = 0;
-//     for ( iter = iter_start; iter != iter_end; iter++)
-//     {
-//       LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
-//             <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
-//       i++;
-//     }
-     double Lf = 2.67;
-     double steer_value = (vars[0]*1300/25/(deg2rad(25)*Lf));
-     double throttle_value = vars[1]*100;
+     int steer_value = rad2deg(vars[0]);
+
+     double acc = vars[1]; // 加速度
+     int throttle_value = throttle_Last + acc*0.1*3.6*100;
+//     if (throttle_Last>=0)
+//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last -15: throttle_Now;
+//     else
+//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last +15: throttle_Now;
+     throttle_Last = throttle_value;
      LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
      StreeControl(steer_value);
      SpeedControl(throttle_value);
@@ -454,16 +443,16 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      Control_pub.publish(control);
 }
 
-unsigned int Decision::SpeedControl(double &speed)
+unsigned int Decision::SpeedControl(int &speed)
 {
      canbus::PedalActCtrReq srv;
      if (speed >0){
-         srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
+         srv.request.uiAccPsngCtrReq = speed *10;
          srv.request.uiBrkPsngCtrReq =0;
      }
      else{
          srv.request.uiAccPsngCtrReq = 0;
-         srv.request.uiBrkPsngCtrReq =(unsigned int)(-speed *10);
+         srv.request.uiBrkPsngCtrReq = -speed *10;
      }
      srv.request.iAccSpdCtrReq = 20;
      srv.request.iBrkSpdCtrReq = 50;
@@ -481,11 +470,11 @@ unsigned int Decision::SpeedControl(double &speed)
 }
 
 
-unsigned int Decision::StreeControl(double &streer)
+unsigned int Decision::StreeControl(int &streer)
 {
 
      canbus::StrActCtrReq srv;
-     srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
+     srv.request.iStrAngCtrReq = streer *100;
      srv.request.uiStrCtrMdReq =1;
      srv.request.iStrSpdCtrReq = 200;
      srv.request.iStrTrqCtrReq = 50;

src/decision/decision.h

@@ -118,9 +118,9 @@ private:
 
     void TestMPC();
     //速度控制
-    unsigned int SpeedControl(double &speed);
+    unsigned int SpeedControl(int &speed);
     //方向控制
-    unsigned int StreeControl(double &streer);
+    unsigned int StreeControl(int &streer);
     //档位控制
     unsigned int BreakCtrControl(unsigned int breakmode);
     //加载规划路径
@@ -162,7 +162,10 @@ private:
     Eigen::MatrixXd transformGlobal2Vehicle(double x, double y, double psi,
            const vector<double> & ptsx, const vector<double> & ptsy);
 
-
+    /**
+    * @brief timer callback function
+    */
+    void OnTimer(const ros::TimerEvent &event);
 
 private:
 
@@ -208,8 +211,8 @@ private:
     std::vector<double> vPx;
     std::vector<double> vPy;
 
-    double throttle_Last = 0;
-    double steer_Last = 0;
+    int throttle_Last = 0;
+    int steer_Last = 0;
 
 };
 

src/localization/localization.cpp

@@ -40,7 +40,7 @@ int Localization::Start() {
  LOG(INFO)<<Name()<<" start start...";
  try
  {
-     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 200);
+     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
      //设置串口属性，并打开串口
      serial_gnss.setPort(serial_port);
      serial_gnss.setBaudrate(baudrate);
@@ -50,7 +50,7 @@ int Localization::Start() {
      if (IsOpen())
      {
          //指定循环的频率
-         ros::Rate loop_rate(2000);
+         ros::Rate loop_rate(rate);
          while(ros::ok())
          {
 

src/localization/localization.h

@@ -108,12 +108,14 @@ private:
 
   uint32_t baudrate = 0;
 
-  uint32_t timeout = 10;
+  uint32_t timeout = 50;
 
   serial::Serial serial_gnss; //声明串口对象
 
   ros::Publisher geometry_pub;
 
+  const double rate = 20;  //20HZ
+
 };
 
 

src/perception/CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
   cecMsg.msg
   eiMsg.msg
   eeMsg.msg
+  powerMsg.msg
  )
 
 ## Generate services in the 'srv' folder

src/perception/msg/powerMsg.msg

@@ -0,0 +1,7 @@
+uint32 RCUPowStat
+uint32 AccCtrPowStat
+uint32 BrkCtrPowStat
+uint32 ClchCtrPowStat
+uint32 StrCtrPowStat
+uint32 XSftArmCtrPowStat
+uint32 YSftArmCtrPowStat

src/perception/perception.cpp

@@ -8,6 +8,7 @@
 #include "perception/cecMsg.h"
 #include "perception/eiMsg.h"
 #include "perception/eeMsg.h"
+#include "perception/powerMsg.h"
 #include "canbus/ActuatorEnableRequest.h"
 
 namespace  robot {
@@ -33,22 +34,23 @@ int Perception::Start()
     LOG(INFO) << Name() << " Start start...";
     try
     {
-        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
+        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1, &Perception::CanTopicCallback, this);
 
-        bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
-        bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 2000);
-        bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
+        bus_pub_6E4 = node_handle_->advertise<::perception::powerMsg>(robot::common::PowerData, 1);
+        bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 1);
+        bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 1);
+        bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 1);
 
-        bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
+        bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 1);
 
-        bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 2000);
-        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 2000);
-        bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 2000);
-        bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 2000);
-        bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 2000);
+        bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 1);
+        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 1);
+        bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 1);
+        bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 1);
+        bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 1);
 
-        bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
-        bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
+        bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 1);
+        bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 1);
 
         clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
 
@@ -58,7 +60,7 @@ int Perception::Start()
     {
          LOG(ERROR) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
-    }  
+    }
     LOG(INFO) << Name() << " Start end.";
     return ROBOT_SUCCESS;
 }
@@ -115,6 +117,9 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     case robot::common::EstopErr:
         PublishState_2E1(ucCanMsgSrc);
         break;
+    case robot::common::CtrPowerStatus:
+        PublishState_6E4(ucCanMsgSrc);
+        break;
     default:
         break;
     }
@@ -170,6 +175,47 @@ bool Perception::PublishState_6E5(unsigned char* msg)
     }
 }
 
+bool Perception::PublishState_6E4(unsigned char* msg)
+{
+    robot::canbusSpace::T_STATE_OUT_6E4 state_6E4;
+    memset(&state_6E4, 0 ,sizeof(state_6E4));
+    memcpy(&state_6E4, msg, CAN_MSG_LEN);
+    if(state_6E4.uiAccCtrPowStat != s_state_6E4.uiAccCtrPowStat || state_6E4.uiBrkCtrPowStat != s_state_6E4.uiBrkCtrPowStat ||
+            state_6E4.uiClchCtrPowStat != s_state_6E4.uiClchCtrPowStat || state_6E4.uiRCUPowStat != s_state_6E4.uiRCUPowStat ||
+            state_6E4.uiStrCtrPowStat != s_state_6E4.uiStrCtrPowStat || state_6E4.uiXSftArmCtrPowStat != s_state_6E4.uiXSftArmCtrPowStat||
+            state_6E4.uiYSftArmCtrPowStat != s_state_6E4.uiYSftArmCtrPowStat)
+    {
+
+        memcpy(&s_state_6E4, msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6E5))
+//            memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
+        if(bus_pub_6E4)
+        {
+            ::perception::powerMsg msg;
+            msg.AccCtrPowStat = state_6E4.uiAccCtrPowStat;
+            msg.BrkCtrPowStat =  state_6E4.uiBrkCtrPowStat;
+            msg.RCUPowStat = state_6E4.uiRCUPowStat;
+            msg.StrCtrPowStat = state_6E4.uiStrCtrPowStat;
+            msg.XSftArmCtrPowStat = state_6E4.uiXSftArmCtrPowStat;
+            msg.YSftArmCtrPowStat = state_6E4.uiYSftArmCtrPowStat;
+            msg.ClchCtrPowStat = state_6E4.uiClchCtrPowStat;
+
+            bus_pub_6E4.publish(msg);
+        }
+        else
+        {
+            LOG(ERROR) << Name() << " PublishState_6E4 publisher is unvalid";
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_6E4 data is repeat";
+        return false;
+    }
+}
+
 bool Perception::PublishState_6EE(unsigned char* msg)
 {
 

src/perception/perception.h

@@ -56,6 +56,12 @@ private:
 
     bool PublishState_6E5(unsigned char* msg);
 
+    /**
+    * @brief Publish State_6E4 topic
+    */
+
+    bool PublishState_6E4(unsigned char* msg);
+
     /**
     * @brief Publish State_6ED topic
     */
@@ -134,6 +140,8 @@ private:
 private:
      robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
 
+     robot::canbusSpace::T_STATE_OUT_6E4 s_state_6E4;
+
      robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
 
      robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
@@ -151,6 +159,7 @@ private:
      robot::canbusSpace::T_STR_ERR s_state_6D6;
 
      ros::Publisher bus_pub_6E5;
+     ros::Publisher bus_pub_6E4;
      ros::Publisher bus_pub_6ED;
      ros::Publisher bus_pub_6EE;