31using ::apollo::common::ErrorCode;
32using ::apollo::common::VehicleSignal;
33using ::apollo::control::ControlCommand;
34using ::apollo::drivers::canbus::ProtocolData;
38const int32_t kMaxFailAttempt = 10;
39const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1;
40const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2;
46 CanSender<::apollo::canbus::Lexus>*
const can_sender,
47 MessageManager<::apollo::canbus::Lexus>*
const message_manager) {
49 AINFO <<
"LexusController has already been initiated.";
50 return ErrorCode::CANBUS_ERROR;
53 common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
55 if (!
params_.has_driving_mode()) {
56 AERROR <<
"Vehicle conf pb not set driving_mode.";
57 return ErrorCode::CANBUS_ERROR;
60 if (can_sender ==
nullptr) {
61 AERROR <<
"Canbus sender is null.";
62 return ErrorCode::CANBUS_ERROR;
66 if (message_manager ==
nullptr) {
67 AERROR <<
"Protocol manager is null.";
68 return ErrorCode::CANBUS_ERROR;
75 if (accel_cmd_100_ ==
nullptr) {
76 AERROR <<
"Accelcmd100 does not exist in the LexusMessalexusanager!";
77 return ErrorCode::CANBUS_ERROR;
82 if (brake_cmd_104_ ==
nullptr) {
83 AERROR <<
"Brakecmd104 does not exist in the LexusMessalexusanager!";
84 return ErrorCode::CANBUS_ERROR;
89 if (shift_cmd_128_ ==
nullptr) {
90 AERROR <<
"Shiftcmd128 does not exist in the LexusMessalexusanager!";
91 return ErrorCode::CANBUS_ERROR;
96 if (turn_cmd_130_ ==
nullptr) {
97 AERROR <<
"Turncmd130 does not exist in the LexusMessageManager!";
98 return ErrorCode::CANBUS_ERROR;
103 if (steering_cmd_12c_ ==
nullptr) {
104 AERROR <<
"Steeringcmd12c does not exist in the LexusMessageManager!";
105 return ErrorCode::CANBUS_ERROR;
115 AINFO <<
"LexusController is initialized.";
118 return ErrorCode::OK;
125 AERROR <<
"LexusController has NOT been initiated.";
128 const auto& update_func = [
this] { SecurityDogThreadFunc(); };
129 thread_.reset(
new std::thread(update_func));
136 AERROR <<
"LexusController stops or starts improperly!";
140 if (thread_ !=
nullptr && thread_->joinable()) {
143 AINFO <<
"LexusController stopped.";
150 Lexus chassis_detail;
159 chassis_.set_error_code(chassis_error_code());
162 chassis_.set_engine_started(
true);
165 if (chassis_detail.has_vehicle_speed_rpt_400() &&
167 chassis_.set_speed_mps(
static_cast<float>(
170 chassis_.set_speed_mps(0);
173 if (chassis_detail.has_wheel_speed_rpt_407()) {
175 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rr_valid(
true);
177 chassis_.mutable_wheel_speed()->set_wheel_spd_rr(
180 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rl_valid(
true);
185 chassis_.mutable_wheel_speed()->set_wheel_spd_rl(
188 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fr_valid(
true);
193 chassis_.mutable_wheel_speed()->set_wheel_spd_fr(
196 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fl_valid(
true);
201 chassis_.mutable_wheel_speed()->set_wheel_spd_fl(
206 chassis_.set_fuel_range_m(0);
208 if (chassis_detail.has_accel_rpt_200() &&
211 chassis_.set_throttle_percentage(
static_cast<float>(
214 chassis_.set_throttle_percentage(0);
217 if (chassis_detail.has_brake_rpt_204() &&
220 chassis_.set_brake_percentage(
static_cast<float>(
223 chassis_.set_brake_percentage(0);
227 if (chassis_detail.has_shift_rpt_228() &&
229 AINFO <<
"Start reading shift values";
250 chassis_.set_gear_location(gear_pos);
257 if (chassis_detail.has_steering_rpt_22c() &&
259 chassis_.set_steering_percentage(
263 chassis_.set_steering_percentage(0);
267 if (chassis_detail.has_turn_rpt_230() &&
273 chassis_.mutable_signal()->set_turn_signal(
277 chassis_.mutable_signal()->set_turn_signal(
280 chassis_.mutable_signal()->set_turn_signal(
284 chassis_.mutable_signal()->set_turn_signal(
290 if (chassis_error_mask_) {
291 chassis_.set_chassis_error_mask(chassis_error_mask_);
296 chassis_.mutable_engage_advice()->set_advice(
299 chassis_.mutable_engage_advice()->set_advice(
301 chassis_.mutable_engage_advice()->set_reason(
302 "CANBUS not ready, firmware error or emergency button pressed!");
308bool LexusController::VerifyID() {
return true; }
310void LexusController::Emergency() {
315ErrorCode LexusController::EnableAutoMode() {
317 AINFO <<
"Already in COMPLETE_AUTO_DRIVE mode";
318 return ErrorCode::OK;
332 CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
333 if (!CheckResponse(flag,
true)) {
334 AERROR <<
"Failed to switch to COMPLETE_AUTO_DRIVE mode.";
337 return ErrorCode::CANBUS_ERROR;
340 AINFO <<
"Switch to COMPLETE_AUTO_DRIVE mode ok.";
341 return ErrorCode::OK;
344ErrorCode LexusController::DisableAutoMode() {
349 AINFO <<
"Switch to COMPLETE_MANUAL ok.";
350 return ErrorCode::OK;
353ErrorCode LexusController::EnableSteeringOnlyMode() {
357 AINFO <<
"Already in AUTO_STEER_ONLY mode";
358 return ErrorCode::OK;
367 if (!CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG,
true)) {
368 AERROR <<
"Failed to switch to AUTO_STEER_ONLY mode.";
371 return ErrorCode::CANBUS_ERROR;
374 AINFO <<
"Switch to AUTO_STEER_ONLY mode ok.";
375 return ErrorCode::OK;
378ErrorCode LexusController::EnableSpeedOnlyMode() {
382 AINFO <<
"Already in AUTO_SPEED_ONLY mode.";
383 return ErrorCode::OK;
392 if (!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG,
true)) {
393 AERROR <<
"Failed to switch to AUTO_SPEED_ONLY mode.";
396 return ErrorCode::CANBUS_ERROR;
399 AINFO <<
"Switch to AUTO_SPEED_ONLY mode ok.";
400 return ErrorCode::OK;
407 AINFO <<
"This drive mode no need to set gear.";
411 switch (gear_position) {
437 AERROR <<
"Gear command is invalid!";
453void LexusController::Brake(
double pedal) {
457 AINFO <<
"The current drive mode does not need to set acceleration.";
465void LexusController::Throttle(
double pedal) {
468 AINFO <<
"The current drive mode does not need to set acceleration.";
476void LexusController::Acceleration(
double acc) {
479 AINFO <<
"The current drive mode does not need to set acceleration.";
489void LexusController::Steer(
double angle) {
492 AINFO <<
"The current driving mode does not need to set steer.";
508void LexusController::Steer(
double angle,
double angle_spd) {
511 AINFO <<
"The current driving mode does not need to set steer.";
516 const double real_angle_spd =
517 ProtocolData<::apollo::canbus::Lexus>::BoundedValue(
526void LexusController::SetEpbBreak(
const ControlCommand& command) {
527 if (command.parking_brake()) {
534ErrorCode LexusController::HandleCustomOperation(
535 const external_command::ChassisCommand& command) {
536 return ErrorCode::OK;
539void LexusController::SetBeam(
const VehicleSignal& vehicle_signal) {
540 if (vehicle_signal.has_high_beam() && vehicle_signal.high_beam()) {
542 }
else if (vehicle_signal.has_low_beam() && vehicle_signal.low_beam()) {
549void LexusController::SetHorn(
const VehicleSignal& vehicle_signal) {
550 if (vehicle_signal.horn()) {
557void LexusController::SetTurningSignal(
const VehicleSignal& vehicle_signal) {
558 auto signal = vehicle_signal.turn_signal();
568void LexusController::ResetProtocol() {
message_manager_->ResetSendMessages(); }
570bool LexusController::CheckChassisError() {
576void LexusController::SecurityDogThreadFunc() {
577 int32_t vertical_ctrl_fail = 0;
578 int32_t horizontal_ctrl_fail = 0;
581 AERROR <<
"Failed to run SecurityDogThreadFunc() because can_sender_ is "
586 std::this_thread::yield();
589 std::chrono::duration<double, std::micro> default_period{50000};
595 bool emergency_mode =
false;
600 !CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG,
false)) {
601 ++horizontal_ctrl_fail;
602 if (horizontal_ctrl_fail >= kMaxFailAttempt) {
603 emergency_mode =
true;
607 horizontal_ctrl_fail = 0;
613 !CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG,
false)) {
614 ++vertical_ctrl_fail;
615 if (vertical_ctrl_fail >= kMaxFailAttempt) {
616 emergency_mode =
true;
620 vertical_ctrl_fail = 0;
622 if (CheckChassisError()) {
624 emergency_mode =
true;
632 std::chrono::duration<double, std::micro> elapsed{end - start};
633 if (elapsed < default_period) {
634 std::this_thread::sleep_for(default_period - elapsed);
636 AERROR <<
"Too much time consumption in LexusController looping process:"
642bool LexusController::CheckResponse(
const int32_t flags,
bool need_wait) {
647 int32_t retry_num = 20;
648 Lexus chassis_detail;
649 bool is_accel_enabled =
false;
650 bool is_brake_enabled =
false;
651 bool is_steering_enabled =
false;
658 bool check_ok =
true;
659 if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
660 is_steering_enabled = chassis_detail.has_steering_rpt_22c() &&
661 chassis_detail.steering_rpt_22c().has_enabled() &&
662 chassis_detail.steering_rpt_22c().enabled();
663 check_ok = check_ok && is_steering_enabled;
666 if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {
667 is_brake_enabled = chassis_detail.has_brake_rpt_204() &&
668 chassis_detail.brake_rpt_204().has_enabled() &&
669 chassis_detail.brake_rpt_204().enabled();
670 is_accel_enabled = chassis_detail.has_accel_rpt_200() &&
671 chassis_detail.accel_rpt_200().has_enabled() &&
672 chassis_detail.accel_rpt_200().enabled();
673 check_ok = check_ok && is_brake_enabled && is_accel_enabled;
680 std::this_thread::sleep_for(
681 std::chrono::duration<double, std::milli>(20));
683 }
while (need_wait && retry_num);
686 AERROR <<
"check_response fail: is_steering_enabled:" << is_steering_enabled
687 <<
", is_brake_enabled:" << is_brake_enabled
688 <<
", is_accel_enabled:" << is_accel_enabled;
692void LexusController::set_chassis_error_mask(
const int32_t mask) {
693 std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
694 chassis_error_mask_ = mask;
697int32_t LexusController::chassis_error_mask() {
698 std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
699 return chassis_error_mask_;
703 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
704 return chassis_error_code_;
707void LexusController::set_chassis_error_code(
709 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
710 chassis_error_code_ = error_code;