2020年12月22日星期二

上接上一篇文章https://www.cnblogs.com/shenqiren/p/14169216.html ，这篇是参考上一篇文章的步骤进行一次尝试，如果最后可以执行的话我会再系统地写一遍所有步骤（大概）。

这篇文章除了是测试自己上一篇文章对不对，同时也是对其中一些问题进行了修改，又提出了一些新的问题和思考。

目录

2020年12月22日星期二    1

1.    DBC修改    1

轮速反馈    1

油门反馈    1

制动反馈    1

挡位反馈    1

EPB驻车反馈    2

iECU有效性设定（ENABLE）    2

挡位控制    3

制动请求    3

驱动请求    4

转向请求    4

车辆自检    4

统计有用的DBCmessage    5

2.    代码修改（tsy_controller.cc）    5

轮速反馈    6

油门反馈    6

制动反馈    7

挡位反馈    7

EPB驻车反馈    8

iECU有效性（ENABLE）    8

挡位控制    10

制动请求    11

转向请求    12

车辆自检    12

 

 

  1. DBC修改

轮速反馈

在DBC中新加一个0x305的报文，名字叫VCU_Vehicle_Status_3。在这个报文里放一些需要的信息在里面，主要的想法是在里面加入一些由VCU整合后的反馈信息。

在里面我打算加入轮速反馈以及油门反馈。

油门反馈

这个打算将0x304中的信号重新写一下，将原来的制动反馈改为8bits，然后在16-24位插入一个throttle_rpt的信号，作为油门反馈。

制动反馈

在DBC中将制动压力反馈改为了百分比，需要在VCU中除以一下最大制动压力。

挡位反馈

在0x303中加入8位挡位反馈，value table如下所示：

这样就可以在代码中加入挡位反馈的内容了。

EPB驻车反馈

在0x303中加入8位的状态反馈，value table如下所示：

这样就可以在代码中加入挡位反馈的内容了。

iECU有效性设定（ENABLE）

这里先不对gear/park/_en_ctrl进行设定了，对应代码中应该删除这部分。但是我们需要把其他几个信号的0/1在value table中设定为XX_DISABLE/XX_ENABLE。

需要修改代码中的ENABLE。

挡位控制

需要修改代码中关于value table的使用。

制动请求

这里本来就是百分比的控制方法，但是需要说一下，所有的comment中不能有汉字或者换行和回车，这是Apollo中规定的。

驱动请求

Value table已经按照之前的设想修改了，如图所示：

需要修改代码中的value table的表达。

转向请求

关于转向请求左正右负的问题，我觉得可以先不修改，因为这只是模拟而已。不过还是需要注意一点，因为我看转向反馈在304里是左正右负的，这可能会导致一些问题的出现，后续需要统一一下。

车辆自检

对0x301中的0/1进行修改，新建的value table如下所示：

统计有用的DBCmessage

发送信号：0x501/0x502/0x503/0x504/0x505

接收信号：    1）    电机的反馈：0x10/0x20/0x30/0x40/0x60/0x70/0x80/0x90

2）    状态的VCU反馈：0x301/0x303/0x304或许还会有0x305

 

本节完

2020年12月22日 16：47

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

  1. 代码修改（tsy_controller.cc）

参考之前的博客：https://www.cnblogs.com/shenqiren/p/14169216.html 进行修改，可能直接复制粘贴网站上的代码会有TAB不对齐的问题，百度一下Vscode自动对齐就好了。不过在修改过程中我发现了一个问题，那就是如果自己按照DBC中的名字一个一个的敲实在是费时费力，而且还容易出错。

这里开始对之前遗留的一些问题做了修改，算是配适tsy_controller.cc的第二版吧。

轮速反馈

这里改用305的唯一信号了。

  1. //5 wheel spd  
  2. if (tsy.has_vcu_vehicle_status_3_305()) {  
  3.   if (tsy.vcu_vehicle_status_3_305().has_rpm_fl_rpt()) {  
  4.     chassis_.mutable_wheel_speed()->set_wheel_spd_fl(  
  5.         tsy.vcu_vehicle_status_3_305().rpm_fl_rpt());  
  6.   }  
  7.   if (tsy.vcu_vehicle_status_3_305().has_rpm_fr_rpt()) {  
  8.     chassis_.mutable_wheel_speed()->set_wheel_spd_fr(  
  9.         tsy.vcu_vehicle_status_3_305().rpm_fr_rpt());  
  10.   }  
  11.   if (tsy.vcu_vehicle_status_3_305().has_rpm_rl_rpt()) {  
  12.     chassis_.mutable_wheel_speed()->set_wheel_spd_rl(  
  13.         tsy.vcu_vehicle_status_3_305().rpm_rl_rpt());  
  14.   }  
  15.   if (tsy.vcu_vehicle_status_3_305().has_rpm_rr_rpt()) {  
  16.     chassis_.mutable_wheel_speed()->set_wheel_spd_rr(  
  17.         tsy.vcu_vehicle_status_3_305().rpm_rr_rpt());  
  18.   }  
  19. }  

油门反馈

这里突然想到一个问题，我们的DBC中0x304油门反馈的数据范围好像不是0-100。

  1. // 10 brake 0x304  
  2. if (tsy.has_vcu_vehicle_status_2_304() &&  
  3.     tsy.vcu_vehicle_status_2_304().has_vehicle_brake_pressure()) {  
  4.   chassis_.set_brake_percentage(static_cast<float>(  
  5.       tsy.vcu_vehicle_status_2_304().vehicle_brake_pressure()));  
  6. } else {  
  7.   chassis_.set_brake_percentage(0);  
  8. }  
  9. /*  下面是原来用0x142的传感器反馈的，这里为了测试就改用0x304了 
  10. if (tsy.has_dbsf_status_142() && 
  11.     tsy.dbsf_status_142().has_dbsf_hp_pressure()) { 
  12.   chassis_.set_brake_percentage(static_cast<float>( 
  13.       tsy.dbsf_status_142().has_dbsf_hp_pressure())); 
  14. } else { 
  15.   chassis_.set_brake_percentage(0); 
  16. } 
  17. */  

制动反馈

问题同上，DBC好像还是0-8，而不是0-100。

  1. // 10 brake 0x304  
  2. if (tsy.has_vcu_vehicle_status_2_304() &&  
  3.     tsy.vcu_vehicle_status_2_304().has_vehicle_brake_pressure()) {  
  4.   chassis_.set_brake_percentage(static_cast<float>(  
  5.       tsy.vcu_vehicle_status_2_304().vehicle_brake_pressure()));  
  6. } else {  
  7.   chassis_.set_brake_percentage(0);  
  8. }  

挡位反馈

这里在0x303中定义了一个挡位反馈，仅用于调试使用，VCU中暂时没有这个逻辑。

  1. // 23, previously 11 gear 0x303中定义挡位反馈  
  2. if (tsy.has_vcu_vehicle_status_1_303() &&  
  3.     tsy.vcu_vehicle_status_1_303().has_gear_rpt()) {  
  4.   Chassis::GearPosition gear_pos = Chassis::GEAR_INVALID;  
  5.     
  6.   if (tsy.vcu_vehicle_status_1_303().gear_rpt() ==  
  7.       Vcu_vehicle_status_1_303::GEAR_RPT_INVALID) {  
  8.     gear_pos = Chassis::GEAR_INVALID;  
  9.   }  
  10.   if (tsy.vcu_vehicle_status_1_303().gear_rpt() ==  
  11.       Vcu_vehicle_status_1_303::GEAR_RPT_NEUTRAL) {  
  12.     gear_pos = Chassis::GEAR_NEUTRAL;  
  13.   }  
  14.   if (tsy.vcu_vehicle_status_1_303().gear_rpt() ==  
  15.       Vcu_vehicle_status_1_303::GEAR_RPT_REVERSE) {  
  16.     gear_pos = Chassis::GEAR_REVERSE;  
  17.   }  
  18.   if (tsy.vcu_vehicle_status_1_303().gear_rpt() ==  
  19.       Vcu_vehicle_status_1_303::GEAR_RPT_DRIVE) {  
  20.     gear_pos = Chassis::GEAR_DRIVE;  
  21.   }  
  22.   if (tsy.vcu_vehicle_status_1_303().gear_rpt() ==  
  23.       Vcu_vehicle_status_1_303::GEAR_RPT_PARKING) {  
  24.     gear_pos = Chassis::GEAR_PARKING;  
  25.   }  
  26.   chassis_.set_gear_location(gear_pos);  
  27. } else {  
  28.   chassis_.set_gear_location(Chassis::GEAR_NONE);  
  29. }  

EPB驻车反馈

  1. // 13 parking brake  
  2. if (tsy.has_vcu_vehicle_status_1_303() &&  
  3.     tsy.vcu_vehicle_status_1_303().has_epb_rpt()) {  
  4.   if (tsy.vcu_vehicle_status_1_303().epb_rpt() ==  
  5.       Vcu_vehicle_status_1_303::EPB_RPT_EPB_ON) {  
  6.     chassis_.set_parking_brake(true);  
  7.   } else {  
  8.     chassis_.set_parking_brake(false);  
  9.   }  
  10. } else {  
  11.   chassis_.set_parking_brake(false);  
  12. }  

这里是只有true与false，所以把EPB的0和2的状态都当作false，这在具体信号中规定了。

// config detail: {'bit': 24, 'enum': {0: 'EPB_RPT_EPB_NONE', 1: 'EPB_RPT_EPB_ON', 2: 'EPB_RPT_EPB_OFF'}, 'is_signed_var': False, 'len': 8, 'name': 'epb_rpt', 'offset': 0.0, 'order': 'intel', 'physical_range': '[0|2]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'}

Vcu_vehicle_status_1_303::Epb_rptType Vcuvehiclestatus1303::epb_rpt(const std::uint8_t* bytes, int32_t length) const {

  Byte t0(bytes + 3);

  int32_t x = t0.get_byte(0, 8);

 

  Vcu_vehicle_status_1_303::Epb_rptType ret =  static_cast<Vcu_vehicle_status_1_303::Epb_rptType>(x);

  return ret;

}

iECU有效性（ENABLE）

这里有一点点小问题，怎么我的value table里的名字变得这么长了？

// config detail: {'bit': 0, 'enum': {0: 'IECU_CONTROL_REQUEST_FLAG_IECU_DISABLE', 1: 'IECU_CONTROL_REQUEST_FLAG_IECU_ENABLE'}, 'is_signed_var': False, 'len': 8, 'name': 'iECU_Control_Request_Flag', 'offset': 0.0, 'order': 'intel', 'physical_range': '[0|1]', 'physical_unit': 'N/A', 'precision': 1.0, 'type': 'enum'}

void Iecucontrolflag501::set_p_iecu_control_request_flag(uint8_t* data,

    Iecu_control_flag_501::Iecu_control_request_flagType iecu_control_request_flag) {

  int x = iecu_control_request_flag;

 

  Byte to_set(data + 0);

  to_set.set_value(x, 0, 8);

}

  1. ErrorCode TsyController::EnableAutoMode() {  
  2.   if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) {  
  3.     AINFO << "already in COMPLETE_AUTO_DRIVE mode";  
  4.     return ErrorCode::OK;  
  5.   }  
  6.   return ErrorCode::OK;  
  7.   // set enable  
  8.   iecu_control_flag_501_->set_iecu_control_request_flag(  
  9.       Iecu_control_flag_501::IECU_CONTROL_REQUEST_FLAG_IECU_ENABLE)  
  10.   iecu_control_steering_502_->set_iecu_steering_valid(  
  11.       Iecu_control_steering_502::IECU_STEERING_VALID_STR_ENABLE);  
  12.   iecu_control_ibc_503_->set_iecu_ibc_valid(  
  13.       Iecu_control_ibc_503::IECU_IBC_VALID_BRK_ENABLE);  
  14.   iecu_control_power_504_->set_iecu_power_valid(  
  15.       Iecu_control_power_504::IECU_POWER_VALID_POW_ENABLE);  
  16.           
  17.   can_sender_->Update();  
  18.   const int32_t flag =  
  19.       CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;  
  20.   if (!CheckResponse(flag, true)) {  
  21.     AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode.";  
  22.     Emergency();  
  23.     set_chassis_error_code(Chassis::CHASSIS_ERROR);  
  24.     return ErrorCode::CANBUS_ERROR;  
  25.   }  
  26.   set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);  
  27.   AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";  
  28.     
  29.   return ErrorCode::OK;  
  30. }  

这和我前面DBC中定义的完全不对劲啊。

挡位控制

  1. // NEUTRAL, REVERSE, DRIVE  
  2. void TsyController::Gear(Chassis::GearPosition gear_position) {  
  3.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
  4.       driving_mode() != Chassis::AUTO_SPEED_ONLY) {  
  5.     AINFO << "This drive mode no need to set gear.";  
  6.     return;  
  7.   }  
  8.   // ADD YOUR OWN CAR CHASSIS OPERATION  
  9.   switch (gear_position) {  
  10.     case Chassis::GEAR_NEUTRAL: {  
  11.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_NEUTRAL_CMD);  
  12.       break;  
  13.     }  
  14.     case Chassis::GEAR_REVERSE: {  
  15.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_REVERSE_CMD);  
  16.       break;  
  17.     }  
  18.     case Chassis::GEAR_DRIVE: {  
  19.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_DRIVE_CMD);  
  20.       break;  
  21.     }  
  22.     case Chassis::GEAR_PARKING: {  
  23.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_PARK_CMD);  
  24.       break;  
  25.     }  
  26.     /* 
  27.     case Chassis::GEAR_LOW: { 
  28.       gear_66_->set_gear_low(); 
  29.       break; 
  30.     } 
  31.     并没有设计低速档，所以这段不用。 
  32.     */  
  33.     case Chassis::GEAR_NONE: {  
  34.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_NEUTRAL_CMD);  
  35.       break;  
  36.     }  
  37.     case Chassis::GEAR_INVALID: {  
  38.       AERROR << "Gear command is invalid!";  
  39.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_NEUTRAL_CMD);  
  40.       break;  
  41.     }  
  42.     default: {  
  43.       iecu_control_power_504_->set_iecu_power_gear(Iecu_control_power_504::IECU_POWER_GEAR_NEUTRAL_CMD);  
  44.       break;  
  45.     }  
  46.   }  
  47. }  

制动请求

不需要修改。

驱动请求

油门控制

  1. // drive with old acceleration  
  2. // gas:0.00~99.99 unit:  
  3. void TsyController::Throttle(double pedal) {  
  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 throttle pedal.";  
  7.     return;  
  8.   }  
  9.   iecu_control_power_504_->set_iecu_total_or_distribute(  
  10.       Iecu_control_power_504::IECU_TOTAL_OR_DISTRIBUTE_POW_TOTAL);  
  11.   iecu_control_power_504_->set_torque_or_speed_or_acc(  
  12.       Iecu_control_power_504::IECU_TORQUE_OR_SPEED_OR_ACC_POW_TORQUE);  
  13.   iecu_control_power_504_->set_iecu_torque_control(pedal);  
  14. }  

这就离谱，我只担心会不会字符太长超出了限制。

加速度控制

  1. // confirm the car is driven by acceleration command or throttle/brake pedal  
  2. // drive with acceleration/deceleration  
  3. // acc:-7.0 ~ 5.0, unit:m/s^2  
  4. void TsyController::Acceleration(double acc) {  
  5.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE ||  
  6.       driving_mode() != Chassis::AUTO_SPEED_ONLY) {  
  7.     AINFO << "The current drive mode does not need to set acceleration.";  
  8.     return;  
  9.   }  
  10.   iecu_control_power_504_->set_iecu_total_or_distribute(  
  11.       Iecu_control_power_504::IECU_TOTAL_OR_DISTRIBUTE_POW_TOTAL);  
  12.   iecu_control_power_504_->set_torque_or_speed_or_acc(  
  13.       Iecu_control_power_504::IECU_TORQUE_OR_SPEED_OR_ACC_POW_ACC);  
  14.   iecu_control_power_504_->set_iecu_acc_or_de_control(acc);  
  15. }  

转向请求

我突然明白为什么devkit的这段里面存在一个M_PI了，应该是他原始的单位是弧度rad，并非角度°。

M_PI * 180

  1. // tsy default, -470 ~ 470, left:+, right:-  
  2. // need to be compatible with control module, so reverse  
  3. // steering with old angle speed  
  4. // angle:-99.99~0.00~99.99, unit:, left:-, right:+  
  5. void TsyController::Steer(double angle) {  
  6.   if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&  
  7.       driving_mode() != Chassis::AUTO_STEER_ONLY) {  
  8.     AINFO << "The current driving mode does not need to set steer.";  
  9.     return;  
  10.   }  
  11.   const double real_angle = params_.max_steer_angle() * angle / 100.0;  
  12.   // reverse sign  
  13.   iecu_control_steering_502_->set_iecu_ftire_angle_cmd(real_angle);  
  14.   iecu_control_steering_502_->set_iecu_ftire_speed_cmd(200);  
  15. }  

车辆自检

  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::FSTEERING_STATE_FSTR_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::FLMOTOR_STATE_FL_FAULT ==  
  22.        tsy.vcu_vehicle_diagnosis_301().flmotor_state()) {  
  23.       return true;  
  24.     }  
  25.     if (Vcu_vehicle_diagnosis_301::FRMOTOR_STATE_FR_FAULT ==  
  26.        tsy.vcu_vehicle_diagnosis_301().frmotor_state()) {  
  27.       return true;  
  28.     }  
  29.     if (Vcu_vehicle_diagnosis_301::RLMOTOR_STATE_RL_FAULT ==  
  30.        tsy.vcu_vehicle_diagnosis_301().rlmotor_state()) {  
  31.       return true;  
  32.     }  
  33.     if (Vcu_vehicle_diagnosis_301::RRMOTOR_STATE_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::DBSF_STATE_DBSF_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_STATE_EPB_FAULT ==  
  52.         tsy.vcu_vehicle_diagnosis_301().epb_state()) {  
  53.       return true;  
  54.     }  
  55.   }  
  56.   return false;  
  57. }  

本节完

2020年12月23日 11:41

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