配适生成TSY的vehicle相关文件（2）

标签：12月   去掉   tool   第一个   details   base   直接   目录   prot   

上接20201217文档中的内容，也就是这篇博客：https://www.cnblogs.com/shenqiren/p/14151072.html 。

强调一下，这里主要集中在第三部分的内容，其内容为如何修改我手头的现有DBC文件以及如何配适xx_controller.cc中的具体代码，关于devkit_controller.cc的解析可以看我之前的一篇文章：https://www.cnblogs.com/shenqiren/p/14070235.html 。

目录

2.    配适代码合入Apollo内    1

（1）    2

（2）    3

3.    实现新的车辆控制逻辑    5

点火开关    7

轮速反馈    10

车速反馈    11

油门反馈    11

制动反馈    12

挡位反馈    13

转向反馈    13

驻车EPB反馈    14

iECU有效性设定（ENABLE）    16

退出自动驾驶（DISABLE）    17

仅自动转向模式    17

仅自动速度模式    17

挡位控制    17

制动请求    19

驱动请求    20

加速度请求    21

转向请求（仅是转向角度）    22

转向请求（转角、角速度）    22

车辆自检（CheckChassisError）    23

反馈检测（CheckResponse）    25

1. 配适代码合入Apollo内

按照技术文档中的指示，接下来就是复制粘贴的工作了。

### 2. 适配代码合入apollo文件内

下面以添加ge3车型为例，将该代码添加至apollo内：

可以用下面这个条指令来移动文件夹，最好是在容器内，因为output/文件夹是被保护的。

（1）

将`apollo/modules/tools/gen_vehicle_protocol/output/proto` 文件夹内`ge3.proto`文件拷贝至`apollo/modules/canbus/proto` 文件夹内，并在该文件夹内修改chassis_detail.proto，在该文件头部添加头文件`import "modules/canbus/proto/ge3.proto"`

import "modules/canbus/proto/tsy.proto";  //参照技术文档的修改

这一步还是比较简单的，按照技术文档的描述就可以实现。

  optional Neolix_edu neolix_edu = 26;

  // Reserve fields for other vehicles

  optional Tsy tsy = 27;  //根据技术文档添加底盘信息。

  optional apollo.common.VehicleID vehicle_id = 101;

PS：【这里有一个知识点，如何理解每个enum下面的optional呢？optional在C++中是定义了一个集成了T模板和bool类型的关键字，用它来声明的量如果没有初始化的话，就不能有效使用，因为bool的标志会是false，表示没有初始化；如果以模板T初始化了之后，bool会为true，这样声明的变量就可以使用了。参考博客：https://blog.csdn.net/leapmotion/article/details/108046138和https://blog.csdn.net/zzhongcy/article/details/87623430 】

在`message ChassisDetail{}` 结构体内的最后一行添加要增加的新车型变量定义： `Ge3 ge3 = 21`；

在`pollo/modules/canbus/proto`目录的`BUILD`文件内添加上述新`proto`的依赖：`"ge3.proto"`；

这一步遇到问题了，根据技术文档中的图片来看，我们并没有找到他添加的代码位置，所以只好按照devkit的代码来添加一个只有name和src的proto_library。

上面这张是官方给的图片。

下面这是我自己修改的代码，不一定好用，后续可能要修改。

package(default_visibility = ["//visibility:public"])

proto_library(

    name = "tsy_proto",

    srcs = ["tsy.proto"],

)

##参考技术文档并没有找到合适的proto_library叫做canbus_proto_lib

cc_proto_library(

    name = "devkit_cc_proto",

    deps = [

        ":devkit_proto",

    ],

)

（2）

将`apollo/modules/tools/gen_vehicle_protocol/output/vehicle/` 内的ge3文件夹拷贝至`apollo/modules/canbus/vehicle/` 文件夹下；

1. 实现新的车辆控制逻辑

实现新的车辆控制逻辑，在`apollo/modules/canbus/vehicle/ge3/ge3_controller.cc` 文件编写控制逻辑代码，主要包含将解析的底盘反馈报文的信息，通过`chassis`和`chassis_detail`广播出车辆底盘信息。`chassis`主要包括获取底盘的车速、轮速、发动机转速、踏板反馈、转角反馈等信息， `chassis_detail`是每一帧报文的实际信息，这一部分编写的代码如下所示：

从这里开始，我们将和技术文档中对照，一点一点地修改tsy.controller.cc的内容，下面的主要格式将是一段技术文档的代码，一段我自己修改的代码。

先要注意一点，这个在我上一篇博文提到过，就是需要修改DBC中的transmitter，所以这里的sender part后面的内容是不对的。

1. ErrorCode TsyController::Init(  
2.     const VehicleParameter& params,  
3.     CanSender<::apollo::canbus::ChassisDetail> *const can_sender,  
4.     MessageManager<::apollo::canbus::ChassisDetail> *const message_manager) {  
5.   if (is_initialized_) {  
6.     AINFO << "TsyController has already been initiated.";  
7.     return ErrorCode::CANBUS_ERROR;  
8.   }  
9.     
10.   params_.CopyFrom(params);  
11.   if (!params_.has_driving_mode()) {  
12.     AERROR << "Vehicle conf pb not set driving_mode.";  
13.     return ErrorCode::CANBUS_ERROR;  
14.   }  
15.     
16.   if (can_sender == nullptr) {  
17.     return ErrorCode::CANBUS_ERROR;  
18.   }  
19.   can_sender_ = can_sender;  
20.     
21.   if (message_manager == nullptr) {  
22.     AERROR << "protocol manager is null.";  
23.     return ErrorCode::CANBUS_ERROR;  
24.   }  
25.   message_manager_ = message_manager;  
26.     
27.   // sender part  
28.   vcu_dbsf_request_154_ = dynamic_cast<Vcudbsfrequest154*>  
29.           (message_manager_->GetMutableProtocolDataById(Vcudbsfrequest154::ID));  
30.   if (vcu_dbsf_request_154_ == nullptr) {  
31.      AERROR << "Vcudbsfrequest154 does not exist in the TsyMessageManager!";  
32.      return ErrorCode::CANBUS_ERROR;  
33.   }  
34.     
35.   vcu_epsf_control_request_469_ = dynamic_cast<Vcuepsfcontrolrequest469*>  
36.           (message_manager_->GetMutableProtocolDataById(Vcuepsfcontrolrequest469::ID));  
37.   if (vcu_epsf_control_request_469_ == nullptr) {  
38.      AERROR << "Vcuepsfcontrolrequest469 does not exist in the TsyMessageManager!";  
39.      return ErrorCode::CANBUS_ERROR;  
40.   }  
41.     
42.   vcu_epsr_control_request_101_ = dynamic_cast<Vcuepsrcontrolrequest101*>  
43.           (message_manager_->GetMutableProtocolDataById(Vcuepsrcontrolrequest101::ID));  
44.   if (vcu_epsr_control_request_101_ == nullptr) {  
45.      AERROR << "Vcuepsrcontrolrequest101 does not exist in the TsyMessageManager!";  
46.      return ErrorCode::CANBUS_ERROR;  
47.   }  
48.     
49.   vcu_vehicle_diagnosis_301_ = dynamic_cast<Vcuvehiclediagnosis301*>  
50.           (message_manager_->GetMutableProtocolDataById(Vcuvehiclediagnosis301::ID));  
51.   if (vcu_vehicle_diagnosis_301_ == nullptr) {  
52.      AERROR << "Vcuvehiclediagnosis301 does not exist in the TsyMessageManager!";  
53.      return ErrorCode::CANBUS_ERROR;  
54.   }  
55.     
56.   vcu_vehicle_status_1_303_ = dynamic_cast<Vcuvehiclestatus1303*>  
57.           (message_manager_->GetMutableProtocolDataById(Vcuvehiclestatus1303::ID));  
58.   if (vcu_vehicle_status_1_303_ == nullptr) {  
59.      AERROR << "Vcuvehiclestatus1303 does not exist in the TsyMessageManager!";  
60.      return ErrorCode::CANBUS_ERROR;  
61.   }  
62.     
63.   can_sender_->AddMessage(Vcudbsfrequest154::ID, vcu_dbsf_request_154_, false);  
64.   can_sender_->AddMessage(Vcuepsfcontrolrequest469::ID, vcu_epsf_control_request_469_, false);  
65.   can_sender_->AddMessage(Vcuepsrcontrolrequest101::ID, vcu_epsr_control_request_101_, false);  
66.   can_sender_->AddMessage(Vcuvehiclediagnosis301::ID, vcu_vehicle_diagnosis_301_, false);  
67.   can_sender_->AddMessage(Vcuvehiclestatus1303::ID, vcu_vehicle_status_1_303_, false);  
68.     
69.   // need sleep to ensure all messages received  
70.   AINFO << "TsyController is initialized.";  
71.     
72.   is_initialized_ = true;  
73.   return ErrorCode::OK;  
74. }  

点火开关

  // 3

  chassis_.set_engine_started(true);

  // check if there is not ge3, no chassis detail can be retrieved and return

  if (!chassis_detail.has_ge3()) {

    AERROR << "NO GE3 chassis information!";

    return chassis_;

  }

  Ge3 ge3 = chassis_detail.ge3();

1. // 3  
2. chassis_.set_engine_started(true);  
3. /* ADD YOUR OWN CAR CHASSIS OPERATION 
4. */  
5. if (!chassis_detail.has_ge3()) {  
6.   AERROR << "NO TSY chassis information!";  
7.   return chassis_;  
8. }  
9. TSY tsy = chassis_detail.tsy();  

接下来我遇到了一点问题，官方技术文档中的ge3和实际酷黑devkit的//5是不一致的，具体看下面的代码。

第一个是ge3的：

  if (ge3.has_scu_bcs_3_308()) {

    Scu_bcs_3_308 scu_bcs_3_308 = ge3.scu_bcs_3_308();

    if (scu_bcs_3_308.has_bcs_rrwheelspd()) {

      if (chassis_.has_wheel_speed()) {

        chassis_.mutable_wheel_speed()->set_is_wheel_spd_rr_valid(

            scu_bcs_3_308.bcs_rrwheelspdvd());

        chassis_.mutable_wheel_speed()->set_wheel_direction_rr(

            (WheelSpeed::WheelSpeedType)scu_bcs_3_308.bcs_rrwheeldirection());

        chassis_.mutable_wheel_speed()->set_wheel_spd_rr(

            scu_bcs_3_308.bcs_rrwheelspd());

      }

    }

第二个是devkit的：

  if (chassis_detail.devkit().has_wheelspeed_report_506()) {

    if (chassis_detail.devkit().wheelspeed_report_506().has_rr()) {

      chassis_.mutable_wheel_speed()->set_wheel_spd_rr(

          chassis_detail.devkit().wheelspeed_report_506().rr());

    }

可以看到这两个在set是出现了不一致，ge3多了两个set。我从devkit_controller.h中的头文件找到了一个文件：

#include "modules/canbus/proto/chassis.pb.h"

我在这个文件夹下找到了一个文件：

这个文件里面有一段代码

message WheelSpeed {

  enum WheelSpeedType {

    FORWARD = 0;

    BACKWARD = 1;

    STANDSTILL = 2;

    INVALID = 3;

  }

  optional bool is_wheel_spd_rr_valid = 1 [default = false];

  optional WheelSpeedType wheel_direction_rr = 2 [default = INVALID];

  optional double wheel_spd_rr = 3 [default = 0.0];

  optional bool is_wheel_spd_rl_valid = 4 [default = false];

  optional WheelSpeedType wheel_direction_rl = 5 [default = INVALID];

  optional double wheel_spd_rl = 6 [default = 0.0];

  optional bool is_wheel_spd_fr_valid = 7 [default = false];

  optional WheelSpeedType wheel_direction_fr = 8 [default = INVALID];

  optional double wheel_spd_fr = 9 [default = 0.0];

  optional bool is_wheel_spd_fl_valid = 10 [default = false];

  optional WheelSpeedType wheel_direction_fl = 11 [default = INVALID];

  optional double wheel_spd_fl = 12 [default = 0.0];

}

看这里可以发现是不一样的，valid是转速信号的有效位，wheel_direction是转向，估计是正反向的，wheel_spd_rr是转速信号。

轮速反馈

现在回到正题，我们可以认为按照devkit的信号来是可以的，所以我们参考devkit的代码而不是ge3的代码。但是我们和酷黑不同的地方是，酷黑的DBC中专门有一个反馈信号506用来存放四个车轮的转速反馈，我们是分散在4个反馈信号10/20/30/40里。

  // 4 engine rpm ch has no engine rpm

  // chassis_.set_engine_rpm(0);

  // 5 wheel spd

  if (chassis_detail.devkit().has_wheelspeed_report_506()) {

    if (chassis_detail.devkit().wheelspeed_report_506().has_rr()) {

      chassis_.mutable_wheel_speed()->set_wheel_spd_rr(

          chassis_detail.devkit().wheelspeed_report_506().rr());

    }

    if (chassis_detail.devkit().wheelspeed_report_506().has_rl()) {

      chassis_.mutable_wheel_speed()->set_wheel_spd_rl(

          chassis_detail.devkit().wheelspeed_report_506().rl());

    }

    if (chassis_detail.devkit().wheelspeed_report_506().has_fr()) {

      chassis_.mutable_wheel_speed()->set_wheel_spd_fr(

          chassis_detail.devkit().wheelspeed_report_506().fr());

    }

    if (chassis_detail.devkit().wheelspeed_report_506().has_fl()) {

      chassis_.mutable_wheel_speed()->set_wheel_spd_fl(

          chassis_detail.devkit().wheelspeed_report_506().fl());

    }

  }

1. //4 Tsy:tsy 没有发动机转速信号  
2. //5 wheel spd  
3. if (tsy.has_mcufl_torque_feedback_10()) {  
4.   if (tsy.mcufl_torque_feedback_10().has_fl_speed()) {  
5.     chassis_.mutable_wheel_speed()->set_wheel_spd_fl(  
6.         tsy.mcufl_torque_feedback_10().fl_speed());  
7.   }  
8. }  
9. if (tsy.has_mcufr_torque_feedback_20()) {  
10.   if (tsy.mcufr_torque_feedback_20().has_fr_speed()) {  
11.     chassis_.mutable_wheel_speed()->set_wheel_spd_fr(  
12.         tsy.mcufr_torque_feedback_20().fr_speed());  
13.   }  
14. }  
15. if (tsy.has_mcurl_torque_feedback_30()) {  
16.   if (tsy.mcurl_torque_feedback_30().has_rl_speed()) {  
17.     chassis_.mutable_wheel_speed()->set_wheel_spd_rl(  
18.         tsy.mcurl_torque_feedback_30().rl_speed());  
19.   }  
20. }  
21. if (tsy.has_mcurr_torque_feedback_40()) {  
22.   if (tsy.mcurr_torque_feedback_40().has_rr_speed()) {  
23.     chassis_.mutable_wheel_speed()->set_wheel_spd_rr(  
24.         tsy.mcurr_torque_feedback_40().rr_speed());  
25.   }  
26. }  

车速反馈

  if (ge3.has_scu_bcs_2_307() && ge3.scu_bcs_2_307().has_bcs_vehspd()) {

    chassis_.set_speed_mps(

        static_cast<float>(ge3.scu_bcs_2_307().bcs_vehspd()));

  } else {

    chassis_.set_speed_mps(0);

  }

1. //6 speed_mps 这个是米每秒的车速  
2. if (tsy.has_vcu_vehicle_status_2_304() && tsy.vcu_vehicle_status_2_304().has_vehicle_speed()) {  
3.   chassis_.set_speed_mps(  
4.       static_cast<float>(tsy.vcu_vehicle_status_2_304().vehicle_speed()));  
5. } else {  
6.   chassis_.set_speed_mps(0);  
7. }  

油门反馈

  // 9 throttle

  if (chassis_detail.devkit().has_throttle_report_500() &&

      chassis_detail.devkit()

          .throttle_report_500()

          .has_throttle_pedal_actual()) {

    chassis_.set_throttle_percentage(static_cast<float>(

        chassis_detail.devkit().throttle_report_500().throttle_pedal_actual()));

  } else {

    chassis_.set_throttle_percentage(0);

  }

1. // 9 throttle 这里有一个问题，这是油门的实际反馈，我们缺少这部分的DBC和定义  
2. /* 
3. if (tsy().has_throttle_report_500() && 
4.     tsy().throttle_report_500().has_throttle_pedal_actual()) { 
5.   chassis_.set_throttle_percentage(static_cast<float>( 
6.       tsy().throttle_report_500().throttle_pedal_actual())); 
7. } else { 
8.   chassis_.set_throttle_percentage(0); 
9. } 
10. */  

制动反馈

  // 10 brake

  if (chassis_detail.devkit().has_brake_report_501() &&

      chassis_detail.devkit().brake_report_501().has_brake_pedal_actual()) {

    chassis_.set_brake_percentage(static_cast<float>(

        chassis_detail.devkit().brake_report_501().brake_pedal_actual()));

  } else {

    chassis_.set_brake_percentage(0);

  }

1. // 10 brake 这里也有一个问题，要求的是制动百分比的反馈，所以在制动压力反馈的DBC  
2. //          中应该除以一下最大的制动压力  
3. if (tsy().has_dbsf_status_142() &&  
4.     tsy().dbsf_status_142().has_dbsf_hp_pressure()) {  
5.   chassis_.set_brake_percentage(static_cast<float>(  
6.       tsy().dbsf_status_142().has_dbsf_hp_pressure()));  
7. } else {  
8.   chassis_.set_brake_percentage(0);  
9. }  

挡位反馈

  // 23, previously 11 gear

  if (chassis_detail.devkit().has_gear_report_503() &&

      chassis_detail.devkit().gear_report_503().has_gear_actual()) {

    Chassis::GearPosition gear_pos = Chassis::GEAR_INVALID;

    if (chassis_detail.devkit().gear_report_503().gear_actual() ==

        Gear_report_503::GEAR_ACTUAL_INVALID) {

      gear_pos = Chassis::GEAR_INVALID;

    }

    if (chassis_detail.devkit().gear_report_503().gear_actual() ==

        Gear_report_503::GEAR_ACTUAL_NEUTRAL) {

      gear_pos = Chassis::GEAR_NEUTRAL;

    }

    if (chassis_detail.devkit().gear_report_503().gear_actual() ==

        Gear_report_503::GEAR_ACTUAL_REVERSE) {

      gear_pos = Chassis::GEAR_REVERSE;

    }

    if (chassis_detail.devkit().gear_report_503().gear_actual() ==

        Gear_report_503::GEAR_ACTUAL_DRIVE) {

      gear_pos = Chassis::GEAR_DRIVE;

    }

    if (chassis_detail.devkit().gear_report_503().gear_actual() ==

        Gear_report_503::GEAR_ACTUAL_PARK) {

      gear_pos = Chassis::GEAR_PARKING;

    }

    chassis_.set_gear_location(gear_pos);

  } else {

    chassis_.set_gear_location(Chassis::GEAR_NONE);

  }

存在一些的问题，目前我们是没有挡位反馈的，所以我初步打算在0x303这个报文内，用8位来反馈这个数据。

转向反馈

  // 12 steering

  if (chassis_detail.devkit().has_steering_report_502() &&

      chassis_detail.devkit().steering_report_502().has_steer_angle_actual()) {

    chassis_.set_steering_percentage(static_cast<float>(

        chassis_detail.devkit().steering_report_502().steer_angle_actual() *

        100.0 / vehicle_params_.max_steer_angle() * M_PI / 180));

  } else {

    chassis_.set_steering_percentage(0);

  }

1. // 12 steering  
2. if (tsy.has_vcu_vehicle_status_2_304() &&  
3.     tsy.vcu_vehicle_status_2_304().has_f_tire_angle()) {  
4.   chassis_.set_steering_percentage(static_cast<float>(  
5.       tsy.vcu_vehicle_status_2_304().f_tire_angle() *  
6.       100.0 / vehicle_params_.max_steer_angle() * M_PI / 180));  
7. } else {  
8.   chassis_.set_steering_percentage(0);  
9. }  

感觉前面的制动反馈可以借鉴这里的用法，在vehicl_params_里加一个最大制动压力的参数。

驻车EPB反馈

  // 13 parking brake

  if (chassis_detail.devkit().has_park_report_504() &&

      chassis_detail.devkit().park_report_504().has_parking_actual()) {

    if (chassis_detail.devkit().park_report_504().parking_actual() ==

        Park_report_504::PARKING_ACTUAL_PARKING_TRIGGER) {

      chassis_.set_parking_brake(true);

    } else {

      chassis_.set_parking_brake(false);

    }

  } else {

    chassis_.set_parking_brake(false);

  }

1. // 13 parking brake  
2. if (tsy.has_vcu_vehicle_status_2_304() &&  
3.     tsy.vcu_vehicle_status_2_304().has_epb_final_state()) {  
4.   if (tsy.vcu_vehicle_status_2_304().epb_final_state() ==  
5.       Vcu_vehicle_status_2_304::PARKED) {  
6.     chassis_.set_parking_brake(true);  
7.   } else {  
8.     chassis_.set_parking_brake(false);  
9.   }  
10. } else {  
11.   chassis_.set_parking_brake(false);  
12. }  

这里直接用EPB_status_375或许不太妥，因为实际EPB的反馈是有四个状态的，而Apollo中只定义了两种状态，所以我想在303中用8位来指代这个EPB状态。PARKED代表加紧，另外的代表放松，去掉保留这个状态，用上升和下降的阶跃来判断。

到这里为止，反馈报文的修改以及修改意见就结束了，下面是请求报文的部分了。

iECU有效性设定（ENABLE）

  // set enable

  brake_command_101_->set_brake_en_ctrl(

      Brake_command_101::BRAKE_EN_CTRL_ENABLE);

  throttle_command_100_->set_throttle_en_ctrl(

      Throttle_command_100::THROTTLE_EN_CTRL_ENABLE);

  steering_command_102_->set_steer_en_ctrl(

      Steering_command_102::STEER_EN_CTRL_ENABLE);

  gear_command_103_->set_gear_en_ctrl(Gear_command_103::GEAR_EN_CTRL_ENABLE);

  park_command_104_->set_park_en_ctrl(Park_command_104::PARK_EN_CTRL_ENABLE);

1. // set enable  
2. iecu_control_flag_501_->set_iecu_control_request_flag(  
3.     Iecu_control_flag_501::ENABLE)  
4. iecu_control_steering_502_->set_iecu_steering_valid(  
5.     Iecu_control_steering_502::ENABLE);  
6. iecu_control_ibc_503_->set_iecu_ibc_valid(  
7.     Iecu_control_ibc_503::ENABLE);  
8. iecu_control_power_504_->set_iecu_power_valid(  
9.     Iecu_control_power_504::ENABLE);  
10. gear_command_103_->set_gear_en_ctrl(Gear_command_103::GEAR_EN_CTRL_ENABLE);  
11. park_command_104_->set_park_en_ctrl(Park_command_104::PARK_EN_CTRL_ENABLE);  

这里存在一些问题，后续需要去查看一下Apollo对驻车和挡位的要求，是否需要一个直接的EPB以及挡位控制请求。

从代码中可以看到devkit的DBC是有一个en_ctrl的位置的，但是新的DBC中好像没有提到这个事：

同样需要注意的是，我们将EPB和驻车挡结合了，在0x504里有一个关于挡位的信息：

退出自动驾驶（DISABLE）

这部分使用自动生成代码就可以，实现的就是退出自动驾驶改为人工驾驶。

仅自动转向模式

这里和devkit一致，直接采用不能仅自动转向的方案：

  AFATAL << "SteerOnlyMode is not supported in devkit!";

  return ErrorCode::CANBUS_ERROR;

但是在模板中我们可以看到Apollo的方法是其他都DISABLE，只有转向ENABLE就是仅自动转向模式：

1. brake_60_->set_disable();  
2. throttle_62_->set_disable();  
3. steering_64_->set_enable();  

这里提出一个后续人机共驾的改进方法，把驱动、制动、转向分开来做，希望可以实现人工油门但是自动转向。

仅自动速度模式

同自动转向。

挡位控制

  switch (gear_position) {

    case Chassis::GEAR_NEUTRAL: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_NEUTRAL);

      break;

    }

    case Chassis::GEAR_REVERSE: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_REVERSE);

      break;

    }

    case Chassis::GEAR_DRIVE: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_DRIVE);

      break;

    }

    case Chassis::GEAR_PARKING: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_PARK);

      break;

    }

    case Chassis::GEAR_INVALID: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_NEUTRAL);

      break;

    }

    default: {

      gear_command_103_->set_gear_target(Gear_command_103::GEAR_TARGET_NEUTRAL);

      break;

    }

  }

1. switch (gear_position) {  
2.   case Chassis::GEAR_NEUTRAL: {  
3.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::NEUTRAL);  
4.     break;  
5.   }  
6.   case Chassis::GEAR_REVERSE: {  
7.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::REVERSE);  
8.     break;  
9.   }  
10.   case Chassis::GEAR_DRIVE: {  
11.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::DRIVE);  
12.     break;  
13.   }  
14.   case Chassis::GEAR_PARKING: {  
15.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::PARKING);  
16.     break;  
17.   }  
18.   /* 
19.   case Chassis::GEAR_LOW: { 
20.     gear_66_->set_gear_low(); 
21.     break; 
22.   } 
23.   并没有设计低速档，所以这段不用。 
24.   */  
25.   case Chassis::GEAR_NONE: {  
26.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::NEUTRAL);  
27.     break;  
28.   }  
29.   case Chassis::GEAR_INVALID: {  
30.     AERROR << "Gear command is invalid!";  
31.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::NEUTRAL);  
32.     break;  
33.   }  
34.   default: {  
35.     iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::NEUTRAL);  
36.     break;  
37.   }  
38. }  

这里提出一点需要改进的地方，在0x504里的挡位，不仅需要在comment里解释各个挡位的含义，还需要再value table里对应上，为这里Iecu_control_power_504::NEUTRAL之类的代码实现提供可能，因为并不确定这里是否可以直接给0或者1.

制动请求

void DevkitController::Brake(double pedal) {

  // double real_value = params_.max_acc() * acceleration / 100;

  // TODO(All) :  Update brake value based on mode

  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&

      driving_mode() != Chassis::AUTO_SPEED_ONLY) {

    AINFO << "The current drive mode does not need to set brake pedal.";

    return;

  }

  brake_command_101_->set_brake_pedal_target(pedal);

}

1. void TsyController::Brake(double pedal) {  
2.   // double real_value = params_.max_acc() * acceleration / 100;  
3.   // TODO(All) :  Update brake value based on mode  
4.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
5.       driving_mode() != Chassis::AUTO_SPEED_ONLY) {  
6.     AINFO << "The current drive mode does not need to set brake pedal.";  
7.     return;  
8.   }  
9.   iecu_control_ibc_503_->set_iecu_breakpressure_cmd(pedal);  
10. }  

同样也有需要注意的问题，这里的制动应该是百分比，而不是Mpa，所以需要改一下。

驱动请求

void DevkitController::Throttle(double pedal) {

  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&

      driving_mode() != Chassis::AUTO_SPEED_ONLY) {

    AINFO << "The current drive mode does not need to set throttle pedal.";

    return;

  }

  throttle_command_100_->set_throttle_pedal_target(pedal);

}

1. void TsyController::Throttle(double pedal) {  
2.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
3.       driving_mode() != Chassis::AUTO_SPEED_ONLY) {  
4.     AINFO << "The current drive mode does not need to set throttle pedal.";  
5.     return;  
6.   }  
7.   iecu_control_power_504_->set_iecu_total_or_distribute(  
8.       Iecu_control_power_504::TOTAL);  
9.   iecu_control_power_504_->set_torque_or_speed_or_acc(  
10.       Iecu_control_power_504::TORQUE);  
11.   iecu_control_power_504_->set_iecu_torque_control(pedal);  
12. }  

这里需要注意的是我们的iecu设计是分了驱动模式，以及集中式或者分布式驱动的：

我想我们应该将切换驱动模式的量都用value table来定义，我这里先给出一个初版：转矩驱动：TORQUE；转速驱动：SPEED；加速度驱动：ACC；集中驱动：TOTAL；分布式：DISTRIBUTE。

加速度请求

可以看到酷黑是没有加速度控制的。

void DevkitController::Acceleration(double acc) {

  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE ||

      driving_mode() != Chassis::AUTO_SPEED_ONLY) {

    AINFO << "The current drive mode does not need to set acceleration.";

    return;

  }

  // None

}

但是我们是有的，所以这样做：

1. void TsyController::Acceleration(double acc) {  
2.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE ||  
3.       driving_mode() != Chassis::AUTO_SPEED_ONLY) {  
4.     AINFO << "The current drive mode does not need to set acceleration.";  
5.     return;  
6.   }  
7.   iecu_control_power_504_->set_iecu_total_or_distribute(  
8.       Iecu_control_power_504::TOTAL);  
9.   iecu_control_power_504_->set_torque_or_speed_or_acc(  
10.       Iecu_control_power_504::ACC);  
11.   iecu_control_power_504_->set_iecu_acc_or_de_control(acc);  
12. }  

转向请求（仅是转向角度）

可以看到酷黑的请求并不能直接是实际转角，而是有轮端传动比。这是因为Apollo自己规定了这一点，并不知道是为什么。

  // const double real_angle = params_.max_steer_angle() * angle / 100.0;

  // reverse sign

  /* ADD YOUR OWN CAR CHASSIS OPERATION

  steering_64_->set_steering_angle(real_angle)->set_steering_angle_speed(200);

  */

void DevkitController::Steer(double angle) {

  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&

      driving_mode() != Chassis::AUTO_STEER_ONLY) {

    AINFO << "The current driving mode does not need to set steer.";

    return;

  }

  const double real_angle =

      vehicle_params_.max_steer_angle() / M_PI * 180 * angle / 100.0;

  steering_command_102_->set_steer_angle_target(real_angle)

      ->set_steer_angle_spd(250);

}

我们默认的转角速度是200，而酷黑是250。

1. void TsyController::Steer(double angle) {  
2.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
3.       driving_mode() != Chassis::AUTO_STEER_ONLY) {  
4.     AINFO << "The current driving mode does not need to set steer.";  
5.     return;  
6.   }  
7.   const double real_angle = params_.max_steer_angle() * angle / 100.0;  
8.   // reverse sign  
9.   iecu_control_steering_502_->set_iecu_ftire_angle_cmd(real_angle);  
10.   iecu_control_steering_502_->set_iecu_ftire_speed_cmd(200);  
11. }  

这里我还是要提一点问题，我们的底盘转角是左负右正的，而Apollo要求左正右负，这一点需要修改。

转向请求（转角、角速度）

酷黑的转角速度居然不会变。

void DevkitController::Steer(double angle, double angle_spd) {

  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&

      driving_mode() != Chassis::AUTO_STEER_ONLY) {

    AINFO << "The current driving mode does not need to set steer.";

    return;

  }

  const double real_angle =

      vehicle_params_.max_steer_angle() / M_PI * 180 * angle / 100.0;

  steering_command_102_->set_steer_angle_target(real_angle)

      ->set_steer_angle_spd(250);

}

1. void TsyController::Steer(double angle, double angle_spd) {  
2.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
3.       driving_mode() != Chassis::AUTO_STEER_ONLY) {  
4.     AINFO << "The current driving mode does not need to set steer.";  
5.     return;  
6.   }  
7.   const double real_angle = params_.max_steer_angle() * angle / 100.0;  
8.   const double real_angle_spd = ProtocolData::BoundedValue(  
9.       params_.min_steer_angle_spd(), params_.max_steer_angle_spd(),  
10.       params_.max_steer_angle_spd() * angle_spd / 100.0);  
11.   // reverse sign  
12.   iecu_control_steering_502_->set_iecu_ftire_angle_cmd(real_angle);  
13.   iecu_control_steering_502_->set_iecu_ftire_speed_cmd(real_angle_spd);  
14. }  

这个地方居然转角速度也是百分比的，看来是为了统一不同的车型，使用无量纲的方法。

转向灯控制

虽然再Apollo6.0中酷黑的程序里没有给出转向灯控制：

void DevkitController::SetTurningSignal(const ControlCommand& command) {

  // Set Turn Signal do not support on devkit

}

但是在新的DBC中是有这一项的：

我觉得我们后续也可以把这一项加进来，不过这并不是很要紧的信号。

车辆自检（CheckChassisError）

参考酷黑的代码进行修改：

1. bool TsyController::CheckChassisError() {  
2.   ChassisDetail chassis_detail;  
3.   message_manager_->GetSensorData(&chassis_detail);  
4.   if (!chassis_detail.has_tsy()) {  
5.     AERROR_EVERY(100) << "ChassisDetail has no tsy vehicle info."  
6.                       << chassis_detail.DebugString();  
7.     return false;  
8.   }  
9.     
10.   Tsy tsy = chassis_detail.tsy();  
11.     
12.   // steer fault  
13.   if (tsy.has_vcu_vehicle_diagnosis_301()) {  
14.     if (Vcu_vehicle_diagnosis_301::FSTEER_FAULT ==  
15.         tsy.vcu_vehicle_diagnosis_301().fsteering_state()) {  
16.       return true;  
17.     }  
18.   }  
19.   // drive fault  
20.   if (tsy.has_vcu_vehicle_diagnosis_301()) {  
21.     if (Vcu_vehicle_diagnosis_301::FL_FAULT ==  
22.        tsy.vcu_vehicle_diagnosis_301().flmotor_state()) {  
23.       return true;  
24.     }  
25.     if (Vcu_vehicle_diagnosis_301::FR_FAULT ==  
26.        tsy.vcu_vehicle_diagnosis_301().frmotor_state()) {  
27.       return true;  
28.     }  
29.     if (Vcu_vehicle_diagnosis_301::RL_FAULT ==  
30.        tsy.vcu_vehicle_diagnosis_301().rlmotor_state()) {  
31.       return true;  
32.     }  
33.     if (Vcu_vehicle_diagnosis_301::RR_FAULT ==  
34.        tsy.vcu_vehicle_diagnosis_301().rrmotor_state()) {  
35.       return true;  
36.     }  
37.   }  
38.   // brake fault  
39.   if (tsy.has_vcu_vehicle_diagnosis_301()) {  
40.     if (Vcu_vehicle_diagnosis_301::BRAKE_FAULT ==  
41.         tsy.vcu_vehicle_diagnosis_301().dbsf_state()) {  
42.       return true;  
43.     }  
44.   }  
45.   // gear fault  
46.   /* 
47.   挡位并没有错误码，也不知道酷黑是哪里来的这个信号。 
48.   */  
49.   // park fault  
50.   if (tsy.has_vcu_vehicle_diagnosis_301()) {  
51.     if (Vcu_vehicle_diagnosis_301::EPB_FAULT ==  
52.         tsy.vcu_vehicle_diagnosis_301().epb_state()) {  
53.       return true;  
54.     }  
55.   }  
56.   return false;  
57. }  

这里建议对0x301这个报文的内容提出修改，将0/1改为XX_FAULT/XX_OK，在value table中体现。

反馈检测（CheckResponse）

这部分可能我在之前酷黑的代码分析上写的不是很对，这里引用技术文档中的内容。

添加`CheckResponse`逻辑，Apollo程序内增加了对车辆底层是否在自动驾驶模式的监控，即车辆横向、驱动、制动模块的驾驶模式反馈是否处于自动驾驶状态，如果在一个`CheckResponse`周期内，车辆某个模块驾驶模块反馈处于接管或者手动驾驶模式，则Apollo会控制车辆使能为紧急停车模式（`Emergency`），即各模块均控制为手动模式，确保控制车辆时的安全。不同的车辆`CheckResponse`周期可能不同，需要开发者根据情况通过设置`retry_num`设定`check`周期。

开发者可以不改原check代码方案，将3个驾驶模式反馈报文与apollo内`chassis_detail`做映射：

`is_eps_online->转向模式反馈信号` 

`is_vcu_online->驱动模式反馈信号` 

`is_esp_online->制动模式反馈信号`

在`apollo/modules/canbus/vehicle/ge3/protocol/scu_eps_311.cc`文件内，增加以下代码：

```

chassis->mutable_check_response()->set_is_eps_online(eps_drvmode(bytes, length) == 3);

```

在`apollo/modules/canbus/vehicle/ge3/protocol/scu_vcu_1_312.cc`文件内，增加以下代码：

```

chassis->mutable_check_response()->set_is_vcu_online(vcu_drvmode(bytes, length) == 3);

```

在`apollo/modules/canbus/vehicle/ge3/protocol/scu_bcs_1_306.cc`文件内，增加以下代码：

```

chassis->mutable_check_response()->set_is_esp_online(bcs_drvmode(bytes, length) == 3);

```

所以从这里看，需要对代码进行改动，包括本小节引用的技术文档中黄色高光的部分。

这部分的代码直接复制粘贴技术文档中的代码，出于规范，这里还是贴出来我们在tsy_controller.cc中的配适代码。

1. bool TsyController::CheckResponse(const int32_t flags, bool need_wait) {  
2.   int32_t retry_num = 20;  
3.   ChassisDetail chassis_detail;  
4.   bool is_eps_online = false;  
5.   bool is_vcu_online = false;  
6.   bool is_esp_online = false;  
7.     
8.   do {  
9.     if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) {  
10.       AERROR_EVERY(100) << "get chassis detail failed.";  
11.       return false;  
12.     }  
13.     bool check_ok = true;  
14.     if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {  
15.       is_eps_online = chassis_detail.has_check_response() &&  
16.                       chassis_detail.check_response().has_is_eps_online() &&  
17.                       chassis_detail.check_response().is_eps_online();  
18.       check_ok = check_ok && is_eps_online;  
19.     }  
20.     
21.     if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {  
22.       is_vcu_online = chassis_detail.has_check_response() &&  
23.                       chassis_detail.check_response().has_is_vcu_online() &&  
24.                       chassis_detail.check_response().is_vcu_online();  
25.       is_esp_online = chassis_detail.has_check_response() &&  
26.                       chassis_detail.check_response().has_is_esp_online() &&  
27.                       chassis_detail.check_response().is_esp_online();  
28.       check_ok = check_ok && is_vcu_online && is_esp_online;  
29.     }  
30.     if (check_ok) {  
31.       return true;  
32.     } else {  
33.       AINFO << "Need to check response again.";  
34.     }  
35.     if (need_wait) {  
36.       --retry_num;  
37.       std::this_thread::sleep_for(  
38.           std::chrono::duration<double, std::milli>(20));  
39.     }  
40.   } while (need_wait && retry_num);  
41.     
42.   AINFO << "check_response fail: is_eps_online:" << is_eps_online  
43.         << ", is_vcu_online:" << is_vcu_online  
44.         << ", is_esp_online:" << is_esp_online;  
45.             
46.   return false;  
47. }  

本篇完

2020年12月21日 17：42

于宁波天尚元振狮路365号工厂二楼

配适生成TSY的vehicle相关文件（2）

标签：12月   去掉   tool   第一个   details   base   直接   目录   prot   

原文地址：https://www.cnblogs.com/shenqiren/p/14169216.html