2020年12月1日星期二
酷黑小车devkit中的devkit_controller.cc
酷黑小车devkit中的devkit_controller.cc
文件路径在apollo/modules/canbus/vehicle/devkit/devkit_controller.cc
using ::apollo::common::ErrorCode;
using ::apollo::control::ControlCommand;
using ::apollo::drivers::canbus::ProtocolData;
开头的三行代码，分别调用了错误码判断，控制命令和二进制代码数据三个东西。
ErrorCode DevkitController::Init(
const VehicleParameter& params,
CanSender<::apollo::canbus::ChassisDetail>* const can_sender,
MessageManager<::apollo::canbus::ChassisDetail>* const message_manager){…}
这段是实现初始化，前面的errorcode不知道是什么意思，我猜测是后面这个Init{}的函数是以他为类型的声明。这里面初始化了can_sender的指针，我想这个是指向CAN信号发送的地址，后面还有个message_manager的指针，这个应该是从message_manager.cc中指向不同message的一种指针吧。
PS：{*const和const*的区别，*const是说声明了一个固定地址的指针，指向的对象的内容可以变，而const*相当于const (*a)，即指向的对象是一个常数，但是指针的地址是可以变的。
以下内容来自https://blog.csdn.net/qq_32623363/article/details/87813484
比如int const*a;，实际上可以看成是int const (*a)，这表示指针a所指向的地址可以变，但是所指向的那个值不能变。
而int *const a;，可以看成int* (const a);，我们都知道a的值其实是一个地址，这就表示a所保存的地址是不可以变的，但是这个地址对应的值是可以变的。}
if (is_initialized_) {
AINFO << "DevkitController has already been initiated.";
return ErrorCode::CANBUS_ERROR;
}
这段是判读是否初始化了。
vehicle_params_.CopyFrom(
common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
这段是一个vehicle_params_的子函数，但是并没有找到vehicle_params_的定义在哪里。我的猜测是这样的，因为这里出现的GetConfig().vehicle_param()所以猜测是和devkit_controller.h中的两条include有关：
#include "modules/canbus/proto/canbus_conf.pb.h"
#include "modules/canbus/proto/vehicle_parameter.pb.h"
但是这两条include并没有找到，我认为是需要在linux下bash dev_start和bash apollo build 之后才可以有这些文件。
params_.CopyFrom(params);
if (!params_.has_driving_mode()) {
AERROR << "Vehicle conf pb not set driving_mode.";
return ErrorCode::CANBUS_ERROR;
}
这条就是查看是否设置了驾驶模式，如果没设置就会报错。
if (can_sender == nullptr) {
return ErrorCode::CANBUS_ERROR;
}
can_sender_ = can_sender;
这条是来确定之前初始化的时候，can_sender的指针是否为空，目前可以大概率确定的是，can_sender是定义或者传入的数据，而can_sender_是在随后的.cc中使用的对象。
if (message_manager == nullptr) {
AERROR << "protocol manager is null.";
return ErrorCode::CANBUS_ERROR;
}
message_manager_ = message_manager;
message_manager和message_manager_同上的can_sender和can_sender_。
// sender part
brake_command_101_ = dynamic_cast<Brakecommand101*>(
message_manager_->GetMutableProtocolDataById(Brakecommand101::ID));
if (brake_command_101_ == nullptr) {
AERROR << "Brakecommand101 does not exist in the DevkitMessageManager!";
return ErrorCode::CANBUS_ERROR;
}
这段代码的注释就很明显了，说的是信号发送。其实不止101这一个信号，下面还有几个，但是结构都是一样的，这里以101来进行解释。brake_command_101_ = dynamic_cast<Brakecommand101*>(…)这部分的含义是子类指针转父类指针，括号中的…就是被转换的子类指针，父类的类型是Brakecommand101*，然后再把这个指针的指向对象地址赋值给brake_command_101_。message_manager_->GetMutableProtocolDataById(Brakecommand101::ID)应该可以用字面意思来理解，这里->的意思是有个指针叫message_manager_，它指向的对象有一个附属函数叫做GetMutableProtocolDataById，这个函数从字面来理解，就是通过ID来获得二进制数据，从dynamic_cast可以看出来这个通过ID获得数据的函数返回的是一个指针。
PS：{dynamic_cast是防止父类指针转子类的一种转换限制，如果父类转子类，就会转成空的NULL，详细请见https://www.bilibili.com/video/BV1FZ4y1p78h}
所以也就有了后面的if (brake_command_101_ == nullptr)，NULLptr的情况就是类型转换失败了，也就是父类子类出现了问题。
101是制动请求，如果查看apollo\modules\canbus\vehicle\devkit\protocol\ brake_command_101.h，就可以发现这个是和DBC文件的内容是对应的。
class Brakecommand101 : public ::apollo::drivers::canbus::ProtocolData<
::apollo::canbus::ChassisDetail>{
public:
…
// config detail: {'name': 'Brake_Dec', 'offset': 0.0, 'precision': 0.01,
// 'len': 10, 'is_signed_var': False, 'physical_range': '[0|10.00]', 'bit':
// 15, 'type': 'double', 'order': 'motorola', 'physical_unit': 'm/s^2'}
Brakecommand101* set_brake_dec(double brake_dec);
…}
里面规定了这个信号的起始位，长度，信号格式是摩托罗拉，偏移、精度等等信息。
void Brakecommand101::UpdateData(uint8_t* data) {
set_p_brake_dec(data, brake_dec_);
set_p_brake_pedal_target(data, brake_pedal_target_);
set_p_brake_en_ctrl(data, brake_en_ctrl_);
checksum_101_ =
data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6];
set_p_checksum_101(data, checksum_101_);
}
上面这段代码是apollo\modules\canbus\vehicle\devkit\protocol\ brake_command_101.cc中的一部分。brake_command_101.cc这个文件中实现了DBC信号的解析与编码，因为他调用了一个叫做Byte的函数，这个函数在modules/drivers/canbus/common/byte.h，如果去看就会发现这是一个十进制和二进制转换的。这部分的主要内容是确定了checksum_101_的数值是多少，这个数值的含义是与非门，用data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]来判断。
PS：{^也可以表示特殊的二元运算符——逐位逻辑运算符（用于对数据的位进行操作），它表示的含义是逐位非或，要求两个整型操作数。从最小（即最右）的位开始，对操作数逐位操作；如果只有其中一个位为1，那么结果为1；否则为0。以上内容来自https://zhidao.baidu.com/question/531865126.html}
can_sender_->AddMessage(Brakecommand101::ID, brake_command_101_, false);
这句话可以看出来是将发送的请求数据添加到了发送队列，但是这个false不知道是什么意思。
DevkitController::~DevkitController() {}
这句话是析构函数，把之前DevkitController函数的内存进行释放。
bool DevkitController::Start() {
if (!is_initialized_) {
AERROR << "DevkitController has NOT been initiated.";
return false;
}
const auto& update_func = [this] { SecurityDogThreadFunc(); };
thread_.reset(new std::thread(update_func));
return true;
}
void DevkitController::Stop() {
if (!is_initialized_) {
AERROR << "DevkitController stops or starts improperly!";
return;
}
if (thread_ != nullptr && thread_->joinable()) {
thread_->join();
thread_.reset();
AINFO << "DevkitController stopped.";
}
}
上面这段代码就是将进程释放，这是很标准的析构函数所拥有的start(){}和stop(){}函数。
Chassis DevkitController::chassis(){…}
这个函数主要是讲的具体有哪些信号需要下发。下面将具体介绍：
chassis_.Clear();
ChassisDetail chassis_detail;
message_manager_->GetSensorData(&chassis_detail);
// 21, 22, previously 1, 2
if (driving_mode() == Chassis::EMERGENCY_MODE) {
set_chassis_error_code(Chassis::NO_ERROR);
}
chassis_.set_driving_mode(driving_mode());
chassis_.set_error_code(chassis_error_code());
// 3
chassis_.set_engine_started(true);
clear()的作用猜测是初始化并清空底盘信息chassis。接下来声明了一个chassis_detail，继承的是ChassisDetail 。message_manager_是一个指针，它指向的对象中有一个子函数GetSensorData，需要以地址&chassis_detail为输入，这句话可以字面理解，就是通过底盘细节这个地址get data，获得底盘信息。后面的"21，22"逻辑有点乱，可以理解如果是紧急模式的时候，需要屏蔽错误码，所以用的是no_error，可是在后面还是set了底盘的error_code，这很迷惑啊，前面的if (…){…}是不是就没用了。"3"就是点火信号。
// 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());
}
}
"4"没有发动机。"5"轮速，因为是分布式电机，所以需要对四个电机单独进行转速指令的下发，这里以rr电机为例进行解读：第一步是判断是否有这个轮速的请求信号，用的是chassis_detail的子内容devkit()中包含的has_wheelspeed_report_506()来作为有506这个轮速信号的标志位，后面用chassis_detail.devkit()的子内容wheelspeed_report_506()的子内容has_rr()作为rr电机请求的标志位，具体的rr电机转速请求在chassis_detail.devkit().wheelspeed_report_506().rr()，设定或者叫下发这个请求指令的函数在chassis_.mutable_wheel_speed()->set_wheel_spd_rr(…)。从这里可以看出来很明显的分级，最高的是chassis_detail，他下面包括的是品牌，本车为devkit()，然后对于每个信号都有一个类似信号是否接收到的标志位has_wheelspeed_report_506()，同样平级的还有一个信号的内容wheelspeed_report_506()，这种内容下面或许还有子内容rr()，但是这一切的具体实现并非使用的chassis_detail，因为他只是一个底盘信息的载体，并非底盘控制的载体，后者的载体是chassis_，他的子内容mutable_wheel_speed()指向的对象有子内容set_wheel_spd_rr()，这样子就完成了一个底盘控制信息的执行。
// 6 speed_mps
if (chassis_detail.devkit().has_vcu_report_505() &&
chassis_detail.devkit().vcu_report_505().has_speed()) {
chassis_.set_speed_mps(
static_cast<float>(chassis_detail.devkit().vcu_report_505().speed()));
} else {
chassis_.set_speed_mps(0);
}
// 7 no odometer
// chassis_.set_odometer_m(0);
// 8 no fuel. do not set;
// chassis_.set_fuel_range_m(0);
"6"mps不知道是什么，但是结构和前面的rpm是类似的，这里将505的判断位和has_speed的判断位一起if了而已，没什么区别。"7，8"，没有里程计和燃料。
// 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);
}
"9"油门，和前面一样，不过在else后面有一个没有信号就把油门置0的逻辑，而且if中涉及了一个数据类型转换的static_cast<float>()强行把后面的数据类型变换成了浮点数。
// 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);
}
"10"刹车，和油门是一样的。
// 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);
}
"23"挡位，和前面一样，但是为什么不用case？
// 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);
}
"12"转向。
// 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);
}
"13"驻车。
return chassis_;
最后这个函数返回的是指针chassis_，猜测这个指针经过前面一系列的set指令，已经将指向的对象设置好了，他指向的对象就是要发送的车辆底盘信号。
void DevkitController::Emergency() {
set_driving_mode(Chassis::EMERGENCY_MODE);
ResetProtocol();
}
字面意思，在前面应该是规定了这个函数要override重写的，所以这里重写了，内容就是设置车辆进入紧急模式。
ErrorCode DevkitController::EnableAutoMode(){…}
下面来看这个函数，很明显这个函数的类型是ErrorCode，并且实现的是自动驾驶各个模块的使能（enable）操作，用ErrorCode来判断是否可以赋予自动驾驶的能力，所以函数内的return都是类似的类型。
if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) {
AINFO << "already in COMPLETE_AUTO_DRIVE mode";
return ErrorCode::OK;
}
首先如果已经进入自动驾驶， 那么就不需要后续的判断以及转换了，这里直接return ErrorCode::OK。
// 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);
这些代码实现的就是赋予使能的操作，信号和前面的是对应的。
PS:{到这里为止，你可能会发现为什么前面有了关于Gear之类的设置，而这里还有，比如前面的Gear_report_503，这里的Gear_command_103，很显然这是两个不同的message，那么这时候就需要去查看apollo\modules\canbus\vehicle\devkit\ devkit_message_manager.cc，这个里面解释了这两个信号的区别。其实从名字上也可以看出来，503是报告报文，也就是反馈，或者说是接受的；而103是command，也就是请求报文，所以说是发送的。具体在devkit_message_manager.cc 中的代码我也贴出来：
DevkitMessageManager::DevkitMessageManager() {
// Control Messages
AddSendProtocolData<Brakecommand101, true>();
AddSendProtocolData<Gearcommand103, true>();
AddSendProtocolData<Parkcommand104, true>();
AddSendProtocolData<Steeringcommand102, true>();
AddSendProtocolData<Throttlecommand100, true>();
// Report Messages
AddRecvProtocolData<Brakereport501, true>();
AddRecvProtocolData<Gearreport503, true>();
AddRecvProtocolData<Parkreport504, true>();
AddRecvProtocolData<Steeringreport502, true>();
AddRecvProtocolData<Throttlereport500, true>();
AddRecvProtocolData<Ultrsensor1507, true>();
AddRecvProtocolData<Ultrsensor2508, true>();
AddRecvProtocolData<Ultrsensor3509, true>();
AddRecvProtocolData<Ultrsensor4510, true>();
AddRecvProtocolData<Ultrsensor5511, true>();
AddRecvProtocolData<Vcureport505, true>();
AddRecvProtocolData<Wheelspeedreport506, true>();
}
}
can_sender_->Update();
const int32_t flag =
CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
if (!CheckResponse(flag, true)) {
AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode.";
Emergency();
set_chassis_error_code(Chassis::CHASSIS_ERROR);
return ErrorCode::CANBUS_ERROR;
}
set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";
return ErrorCode::OK;
这段首先是更新了地址，然后判断是否可以收到转向和速度控制的使能标志位，如果是转向、驱动、制动都好使，那么就可以转换为自动驾驶模式，并且反馈return ErrorCode::OK，但是没搞懂这个const int32_t flag的定义是什么意思，要实现什么样的一种逻辑。
ErrorCode DevkitController::DisableAutoMode() {
ResetProtocol();
can_sender_->Update();
set_driving_mode(Chassis::COMPLETE_MANUAL);
set_chassis_error_code(Chassis::NO_ERROR);
AINFO << "Switch to COMPLETE_MANUAL ok.";
return ErrorCode::OK;
}
这个函数是切换到手动驾驶，但是虽然见过ResetProtocol()很多次了，仍然不知道他在reset什么。
ErrorCode DevkitController::EnableSteeringOnlyMode() {
if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
driving_mode() == Chassis::AUTO_STEER_ONLY) {
set_driving_mode(Chassis::AUTO_STEER_ONLY);
AINFO << "Already in AUTO_STEER_ONLY mode.";
return ErrorCode::OK;
}
AFATAL << "SpeedOnlyMode is not supported in devkit!";
return ErrorCode::CANBUS_ERROR;
}
这段就是字面意思，只是自动转向，但是不懂他的逻辑为什么要或者底盘的模式是完全自动驾驶呢，意思是转向系是独立的咯？只需要一个转向自动驾驶的模式就可以让转向自动驾驶，并不需要指定必须是ONLY的模式？
ErrorCode DevkitController::EnableSpeedOnlyMode() {
if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
driving_mode() == Chassis::AUTO_SPEED_ONLY) {
set_driving_mode(Chassis::AUTO_SPEED_ONLY);
AINFO << "Already in AUTO_SPEED_ONLY mode";
return ErrorCode::OK;
}
AFATAL << "SpeedOnlyMode is not supported in devkit!";
return ErrorCode::CANBUS_ERROR;
}
同上。
// NEUTRAL, REVERSE, DRIVE
void DevkitController::Gear(Chassis::GearPosition gear_position) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
AINFO << "This drive mode no need to set gear.";
return;
}
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;
}
}
}
这段代码通过第一个if来判断是否是完全自动驾驶的模式，只有当是完全自动驾驶并且速度也是自动驾驶的情况下，才可以按照gear_position的内容来设置gear_command_103_的报文内容。
// brake with pedal
// pedal:0.00~99.99, unit:%
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);
}
// drive with pedal
// pedal:0.0~99.9 unit:%
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);
}
这段很容易理解，如果speed是auto的，那么就按照目标的油门给制动或者驱动，如果speed是人工操纵的，就不设置制动和驱动了。
// confirm the car is driven by acceleration command instead of throttle/brake
// pedal drive with acceleration/deceleration acc:-7.0 ~ 5.0, unit:m/s^2
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
}
这里的注释说的很清楚，devkit小车的驱动与制动有两种控制方法，1）直接油门开度或制动踏板行程，2）车辆的加减速度。这段代码和前面还是一个意思，可是并没有给出如果满足了条件后，对于ACC这一项的set指令，莫非是devkit不支持加速度控制？
// devkit default, -30 ~ 00, left:+, right:-
// need to be compatible with control module, so reverse
// steering with default angle speed, 25-250 (default:250)
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
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);
}
这部分是关于转角的内容，和下面的函数的区别在于这个函数的输入只有目标转角，而转向角速度固定250。可以看到函数中的转向请求包括了两个参数，1）目标转角set_steer_angle_target(real_angle)，2）转向角速度set_steer_angle_spd(250)。这里面还包含了一个转角的弧度转角度/ M_PI * 180，并且应该是还有转向电机到轮端的传动比/ 100.0。
// steering with new angle speed
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
// angle_spd:25~250, unit:deg/s
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);
}
这个函数才是真正包含了转角和转向角速度请求的转向函数，可是为什么函数内关于转角速度的设定还是250，并没有用到double angle_spd呢？
void DevkitController::SetEpbBreak(const ControlCommand& command) {
if (command.parking_brake()) {
// None
} else {
// None
}
}
看来devkit没有EPB。
void DevkitController::SetBeam(const ControlCommand& command) {
if (command.signal().high_beam()) {
// None
} else if (command.signal().low_beam()) {
// None
} else {
// None
}
}
也没有蜂鸣器。
void DevkitController::SetHorn(const ControlCommand& command) {
if (command.signal().horn()) {
// None
} else {
// None
}
}
捞的嘛就淌口水，喇叭也没有？
void DevkitController::SetTurningSignal(const ControlCommand& command) {
// Set Turn Signal do not support on devkit
}
牛大了，转向灯也没有。
void DevkitController::ResetProtocol() {
message_manager_->ResetSendMessages();
}
总算找到这个reset是在干什么了，应该是清空上一帧发送报文重新填写。
bool DevkitController::CheckChassisError() {
ChassisDetail chassis_detail;
message_manager_->GetSensorData(&chassis_detail);
if (!chassis_detail.has_devkit()) {
AERROR_EVERY(100) << "ChassisDetail has no devkit vehicle info."
<< chassis_detail.DebugString();
return false;
}
Devkit devkit = chassis_detail.devkit();
// steer fault
if (devkit.has_steering_report_502()) {
if (Steering_report_502::STEER_FLT1_STEER_SYSTEM_HARDWARE_FAULT ==
devkit.steering_report_502().steer_flt1()) {
return true;
}
if (Steering_report_502::STEER_FLT2_STEER_SYSTEM_COMUNICATION_FAULT ==
devkit.steering_report_502().steer_flt2()) {
return true;
}
}
// drive fault
if (devkit.has_throttle_report_500()) {
if (Throttle_report_500::THROTTLE_FLT1_DRIVE_SYSTEM_HARDWARE_FAULT ==
devkit.throttle_report_500().throttle_flt1()) {
return true;
}
if (Throttle_report_500::THROTTLE_FLT2_DRIVE_SYSTEM_COMUNICATION_FAULT ==
devkit.throttle_report_500().throttle_flt2()) {
return true;
}
}
// brake fault
if (devkit.has_brake_report_501()) {
if (Brake_report_501::BRAKE_FLT1_BRAKE_SYSTEM_HARDWARE_FAULT ==
devkit.brake_report_501().brake_flt1()) {
return true;
}
if (Brake_report_501::BRAKE_FLT2_BRAKE_SYSTEM_COMUNICATION_FAULT ==
devkit.brake_report_501().brake_flt2()) {
return true;
}
}
// gear fault
if (devkit.has_gear_report_503()) {
if (Gear_report_503::GEAR_FLT_FAULT ==
devkit.gear_report_503().gear_flt()) {
return true;
}
}
// park fault
if (devkit.has_park_report_504()) {
if (Park_report_504::PARK_FLT_FAULT ==
devkit.park_report_504().park_flt()) {
return true;
}
}
return false;
}
这一大段代码是来判断底盘状况的，如果从各种report的反馈报文来看底盘状况是有问题的，那么就return true，只要是有一项有问题，底盘就是有问题的。大概率可以确定这个会影响进入自动驾驶模式。
void DevkitController::SecurityDogThreadFunc() {…}
这段很长的程序看名字是安全狗进程函数，但是从内部的注释来看，是检测横向（转向）驾驶模式与纵向（加减速）驾驶模式的。
PS：{里面关于状态空间std::this_thread的内容，可以参考这篇博客，这个状态空间讲的是线程调度https://blog.csdn.net/luoshabugui/article/details/86588578}
int32_t vertical_ctrl_fail = 0;
int32_t horizontal_ctrl_fail = 0;
先定义两个失败的计数变量。
if (can_sender_ == nullptr) {
AERROR << "Failed to run SecurityDogThreadFunc() because can_sender_ is "
"nullptr.";
return;
}
while (!can_sender_->IsRunning()) {
std::this_thread::yield();
}
只有当指针can_sender_不为空才进行线程调度。
std::chrono::duration<double, std::micro> default_period{50000};
int64_t start = 0;
int64_t end = 0;
while (can_sender_->IsRunning()) {
start = ::apollo::cyber::Time::Now().ToMicrosecond();
const Chassis::DrivingMode mode = driving_mode();
bool emergency_mode = false;
这个while循环将是这个函数的主要内容，开始就定义了一个进程周期（或者叫信号发送频率）default_period，进程开始时间int64_t start = 0，以及结束时间int64_t end = 0。start = ::apollo::cyber::Time::Now().ToMicrosecond()这个start是进程的开始时间，这里用的是cyber内的时间，精确到了微秒。获取了驾驶模式。把紧急模式的使能位emergency_mode设置为关闭。
// 1. horizontal control check
if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
mode == Chassis::AUTO_STEER_ONLY) &&
CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, false) == false) {
++horizontal_ctrl_fail;
if (horizontal_ctrl_fail >= kMaxFailAttempt) {
emergency_mode = true;
set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
}
} else {
horizontal_ctrl_fail = 0;
}
横向的控制检测，如果车辆是完全自动驾驶，或者是自动转向，并且反馈检测CheckResponse有问题，那么失败计数器就加一++horizontal_ctrl_fail；否则清零horizontal_ctrl_fail = 0。如果计数器超过了前面设置的最大尝试数kMaxFailAttempt，就把紧急模式的使能位设置为true，并且设置车辆进入人工接管MANUAL_INTERVENTION。
// 2. vertical control check
if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
mode == Chassis::AUTO_SPEED_ONLY) &&
!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, false)) {
++vertical_ctrl_fail;
if (vertical_ctrl_fail >= kMaxFailAttempt) {
emergency_mode = true;
set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
}
} else {
vertical_ctrl_fail = 0;
}
纵向控制的检测，主要逻辑和上面横向是一样的。
if (CheckChassisError()) {
set_chassis_error_code(Chassis::CHASSIS_ERROR);
emergency_mode = true;
}
这里调用了本文件中前面的函数CheckChassisError()，如果存在问题，那么紧急模式的使能位 emergency_mode为true。
if (emergency_mode && mode != Chassis::EMERGENCY_MODE) {
set_driving_mode(Chassis::EMERGENCY_MODE);
message_manager_->ResetSendMessages();
}
如果已经确定紧急模式使能位为true，并且车辆的模式并不是紧急模式，那么先进入紧急模式，再重置一下发送的报文内容。
end = ::apollo::cyber::Time::Now().ToMicrosecond();
进程的终止时间end，和start一样。
std::chrono::duration<double, std::micro> elapsed{end - start};
if (elapsed < default_period) {
std::this_thread::sleep_for(default_period - elapsed);
} else {
AERROR << "Too much time consumption in DevkitController looping process:"
<< elapsed.count();
}
while内这一段很有意思，说的是这个进程的持续时间= end - start，{}只是赋值方式而已。如果这个进程时间太短，那么要符合发送频率的要求，需要让这个进程的结束，即信号的发送延迟一段时间，这段时间的长度是default_period – elapsed，所以现在知道前面的default_period = 500000是什么意思了，指的是这个缺省的发送周期是500000微秒，也就是0.5秒。反之，如果超时了，那么就会报错。
bool DevkitController::CheckResponse(const int32_t flags, bool need_wait)
下面来介绍一下CheckResponse这个函数，这个函数前面被调用过，只知道从字面上来理解他是反馈检测函数，下面我们仔细看一下。先分析一下函数的声明，这个是devkit控制器状态空间内的一个函数，函数输入需要是否需要检测的请求标志const int32_t flags和是否允许重新检测的标志位bool need_wait。
int32_t retry_num = 20;
ChassisDetail chassis_detail;
bool is_eps_online = false;
bool is_vcu_online = false;
bool is_esp_online = false;
首先，定义一个尝试次数retry_num。继承定义底盘信息chassis_detail，如果没记错的话，他是指针。然后定义三个布尔类型的变量，分别是eps，vcu和esp的在线状态，初值都为false。
do {
if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) {
AERROR_EVERY(100) << "get chassis detail failed.";
return false;
}
bool check_ok = true;
if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
is_eps_online = chassis_detail.has_check_response() &&
chassis_detail.check_response().has_is_eps_online() &&
chassis_detail.check_response().is_eps_online();
check_ok = check_ok && is_eps_online;
}
if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {
is_vcu_online = chassis_detail.has_check_response() &&
chassis_detail.check_response().has_is_vcu_online() &&
chassis_detail.check_response().is_vcu_online();
is_esp_online = chassis_detail.has_check_response() &&
chassis_detail.check_response().has_is_esp_online() &&
chassis_detail.check_response().is_esp_online();
check_ok = check_ok && is_vcu_online && is_esp_online;
}
if (check_ok) {
return true;
} else {
AINFO << "Need to check response again.";
}
if (need_wait) {
--retry_num;
std::this_thread::sleep_for(
std::chrono::duration<double, std::milli>(20));
}
} while (need_wait && retry_num);
这个do{…}while()的循环是这个函数的主体。如果用message_manager_->GetSensorData(&chassis_detail)的方式获取data失败，那么直接就return false，因为这个函数的定义是布尔类型的，所以返回的也是布尔类型。随后定义一个检测成功与否的标志位，并初始化 bool check_ok = true。因为前面定义过CHECK_RESPONSE_STEER_UNIT_FLAG和CHECK_RESPONSE_SPEED_UNIT_FLAG，两者都是非0的，所以如果flags非0这个检测的if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG)是一定会执行的。检测的内容也很简单，就是相当于simulink中的一大堆与的关系，首先检测有没有反馈信号has_check_response()，再检测有没有eps在线这个信号check_response().has_is_eps_online()，最后再检测信号中eps是否在线check_response().is_eps_online()。最后更新检测成功与否的标志位check_ok = check_ok && is_eps_online。剩下的vcu和esp也是一样的逻辑。后面就是根据前面的check_ok来执行操作，如果ok就不用多说了，如果不ok，那么先判断这个函数的输入是否支持重新检测，如果支持，就把前面设定的重新尝试次数减1，然后把这个进程休眠20微秒，再次检测，直至retry_num为0，或者检测ok。因为如果retry_num为0，这个循环while (need_wait && retry_num)就不会继续。这个函数的最后打印出来eps，vcu，esp的状态。
void DevkitController::set_chassis_error_mask(const int32_t mask) {
std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
chassis_error_mask_ = mask;
}
int32_t DevkitController::chassis_error_mask() {
std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
return chassis_error_mask_;
}
Chassis::ErrorCode DevkitController::chassis_error_code() {
std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
return chassis_error_code_;
}
void DevkitController::set_chassis_error_code(
const Chassis::ErrorCode& error_code) {
std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
chassis_error_code_ = error_code;
}
后面的这4个函数，保证的都是整个devkit_controller.cc中的线程问题，这个是来保证线程安全性的。PS:{关于std::mutex和guard_lock的内容可以参考这两篇博客https://blog.csdn.net/guotianqing/article/details/104002449，https://www.cnblogs.com/ybqjymy/p/12357617.html}。
本篇完
2020/12/2 17:26 于吉林大学南岭校区图书馆5楼064桌
