19#include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h"
30using ::apollo::common::ErrorCode;
31using ::apollo::common::VehicleSignal;
32using ::apollo::control::ControlCommand;
33using ::apollo::drivers::canbus::ProtocolData;
37const int32_t kMaxFailAttempt = 10;
38const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1;
39const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2;
45 CanSender<::apollo::canbus::Wey>*
const can_sender,
46 MessageManager<::apollo::canbus::Wey>*
const message_manager) {
48 AINFO <<
"WeyController has already been initialized.";
49 return ErrorCode::CANBUS_ERROR;
52 common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
54 if (!
params_.has_driving_mode()) {
55 AERROR <<
"Vehicle conf pb not set driving_mode.";
56 return ErrorCode::CANBUS_ERROR;
59 if (can_sender ==
nullptr) {
60 AERROR <<
"Canbus sender is null.";
61 return ErrorCode::CANBUS_ERROR;
65 if (message_manager ==
nullptr) {
66 AERROR <<
"Protocol manager is null.";
67 return ErrorCode::CANBUS_ERROR;
72 ads1_111_ =
dynamic_cast<Ads1111*
>(
74 if (ads1_111_ ==
nullptr) {
75 AERROR <<
"Ads1111 does not exist in the WeyMessageManager!";
76 return ErrorCode::CANBUS_ERROR;
79 ads3_38e_ =
dynamic_cast<Ads338e*
>(
81 if (ads3_38e_ ==
nullptr) {
82 AERROR <<
"Ads338e does not exist in the WeyMessageManager!";
83 return ErrorCode::CANBUS_ERROR;
88 if (ads_eps_113_ ==
nullptr) {
89 AERROR <<
"Adseps113 does not exist in the WeyMessageManager!";
90 return ErrorCode::CANBUS_ERROR;
95 if (ads_req_vin_390_ ==
nullptr) {
96 AERROR <<
"Adsreqvin390 does not exist in the WeyMessageManager!";
97 return ErrorCode::CANBUS_ERROR;
102 if (ads_shifter_115_ ==
nullptr) {
103 AERROR <<
"Adsshifter115 does not exist in the WeyMessageManager!";
104 return ErrorCode::CANBUS_ERROR;
114 AINFO <<
"WeyController is initialized.";
117 return ErrorCode::OK;
124 AERROR <<
"WeyController has NOT been initialized.";
127 const auto& update_func = [
this] { SecurityDogThreadFunc(); };
128 thread_.reset(
new std::thread(update_func));
135 AERROR <<
"WeyController stops or starts improperly!";
139 if (thread_ !=
nullptr && thread_->joinable()) {
142 AINFO <<
"WeyController stopped.";
158 chassis_.set_error_code(chassis_error_code());
161 chassis_.set_engine_started(
true);
164 if (chassis_detail.has_fbs3_237() && chassis_detail.
fbs3_237().has_engspd()) {
165 chassis_.set_engine_rpm(
168 chassis_.set_engine_rpm(0);
172 if (chassis_detail.has_fbs2_240() &&
173 chassis_detail.
fbs2_240().has_vehiclespd() &&
174 chassis_detail.has_fbs1_243() && chassis_detail.has_status_310()) {
179 chassis_.set_speed_mps(
static_cast<float>(fbs2_240.
vehiclespd()));
181 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rr_valid(
184 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
188 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
191 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
194 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
197 chassis_.mutable_wheel_speed()->set_wheel_spd_rr(fbs2_240.
rrwheelspd());
199 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rl_valid(
203 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
207 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
211 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
214 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
217 chassis_.mutable_wheel_speed()->set_wheel_spd_rl(fbs2_240.
rlwheelspd());
219 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fr_valid(
222 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
226 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
229 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
232 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
235 chassis_.mutable_wheel_speed()->set_wheel_spd_fr(fbs2_240.
frwheelspd());
237 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fl_valid(
240 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
244 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
247 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
250 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
253 chassis_.mutable_wheel_speed()->set_wheel_spd_fl(fbs1_243.
flwheelspd());
255 chassis_.set_speed_mps(0);
258 chassis_.set_fuel_range_m(0);
260 if (chassis_detail.has_fbs3_237() &&
261 chassis_detail.
fbs3_237().has_accpedalpos()) {
262 chassis_.set_throttle_percentage(
265 chassis_.set_throttle_percentage(0);
268 if (chassis_detail.has_fbs3_237() &&
269 chassis_detail.
fbs3_237().has_currentgear()) {
291 if (chassis_detail.has_fbs4_235() &&
292 chassis_detail.
fbs4_235().has_steerwheelangle() &&
293 chassis_detail.has_status_310() &&
294 chassis_detail.
status_310().has_steerwheelanglesign()) {
297 chassis_.set_steering_percentage(
static_cast<float>(
303 chassis_.set_steering_percentage(
static_cast<float>(
308 chassis_.set_steering_percentage(0);
311 chassis_.set_steering_percentage(0);
315 if (chassis_detail.has_fbs3_237() &&
316 chassis_detail.
fbs3_237().has_epsdrvinputtrqvalue()) {
317 chassis_.set_steering_torque_nm(
320 chassis_.set_steering_torque_nm(0);
323 if (chassis_detail.has_status_310() &&
328 chassis_.set_parking_brake(
false);
331 if (chassis_detail.has_status_310() &&
332 chassis_detail.
status_310().has_lowbeamsts() &&
334 chassis_.mutable_signal()->set_low_beam(
true);
336 chassis_.mutable_signal()->set_low_beam(
false);
339 if (chassis_detail.has_status_310()) {
340 if (chassis_detail.
status_310().has_leftturnlampsts() &&
343 chassis_.mutable_signal()->set_turn_signal(
345 }
else if (chassis_detail.
status_310().has_rightturnlampsts() &&
348 chassis_.mutable_signal()->set_turn_signal(
351 chassis_.mutable_signal()->set_turn_signal(
355 chassis_.mutable_signal()->set_turn_signal(
359 if (chassis_detail.has_vin_resp1_391() &&
360 chassis_detail.has_vin_resp2_392() &&
361 chassis_detail.has_vin_resp3_393()) {
365 if (vin_resp1_391.has_vin00() && vin_resp1_391.has_vin01() &&
366 vin_resp1_391.has_vin02() && vin_resp1_391.has_vin03() &&
367 vin_resp1_391.has_vin04() && vin_resp1_391.has_vin05() &&
368 vin_resp1_391.has_vin06() && vin_resp1_391.has_vin07() &&
369 vin_resp2_392.has_vin08() && vin_resp2_392.has_vin09() &&
370 vin_resp2_392.has_vin10() && vin_resp2_392.has_vin11() &&
371 vin_resp2_392.has_vin12() && vin_resp2_392.has_vin13() &&
372 vin_resp2_392.has_vin14() && vin_resp2_392.has_vin15() &&
373 vin_resp3_393.has_vin16()) {
375 n[0] = vin_resp1_391.
vin07();
376 n[1] = vin_resp1_391.
vin06();
377 n[2] = vin_resp1_391.
vin05();
378 n[3] = vin_resp1_391.
vin04();
379 n[4] = vin_resp1_391.
vin03();
380 n[5] = vin_resp1_391.
vin02();
381 n[6] = vin_resp1_391.
vin01();
382 n[7] = vin_resp1_391.
vin00();
383 n[8] = vin_resp2_392.
vin15();
384 n[9] = vin_resp2_392.
vin14();
385 n[10] = vin_resp2_392.
vin13();
386 n[11] = vin_resp2_392.
vin12();
387 n[12] = vin_resp2_392.
vin11();
388 n[13] = vin_resp2_392.
vin10();
389 n[14] = vin_resp2_392.
vin09();
390 n[15] = vin_resp2_392.
vin08();
391 n[16] = vin_resp3_393.
vin16();
393 memset(&ch,
'\0',
sizeof(ch));
394 for (
int i = 0; i < 17; i++) {
395 ch[i] =
static_cast<char>(n[i]);
397 chassis_.mutable_vehicle_id()->set_vin(ch);
401 if (chassis_error_mask_) {
402 chassis_.set_chassis_error_mask(chassis_error_mask_);
406 chassis_.mutable_engage_advice()->set_advice(
409 chassis_.mutable_engage_advice()->set_advice(
414 if (chassis_detail.has_fbs3_237() &&
415 chassis_detail.
fbs3_237().has_eps_streeingmode()) {
416 chassis_.mutable_check_response()->set_is_eps_online(
419 if (chassis_detail.has_status_310() &&
420 chassis_detail.
status_310().has_longitudedrivingmode()) {
421 chassis_.mutable_check_response()->set_is_esp_online(
423 chassis_.mutable_check_response()->set_is_vcu_online(
430bool WeyController::VerifyID() {
return true; }
432void WeyController::Emergency() {
437ErrorCode WeyController::EnableAutoMode() {
439 AINFO <<
"Already in COMPLETE_AUTO_DRIVE mode.";
440 return ErrorCode::OK;
458 CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
459 if (!CheckResponse(flag,
true)) {
460 AERROR <<
"Failed to switch to COMPLETE_AUTO_DRIVE mode. Please check the "
461 "emergency button or chassis.";
464 return ErrorCode::CANBUS_ERROR;
467 AINFO <<
"Switch to COMPLETE_AUTO_DRIVE mode ok.";
468 return ErrorCode::OK;
471ErrorCode WeyController::DisableAutoMode() {
476 AINFO <<
"Switch to COMPLETE_MANUAL ok.";
477 return ErrorCode::OK;
480ErrorCode WeyController::EnableSteeringOnlyMode() {
484 AINFO <<
"Already in AUTO_STEER_ONLY mode";
485 return ErrorCode::OK;
497 if (!CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG,
true)) {
498 AERROR <<
"Failed to switch to AUTO_STEER_ONLY mode.";
501 return ErrorCode::CANBUS_ERROR;
504 AINFO <<
"Switch to AUTO_STEER_ONLY mode ok.";
505 return ErrorCode::OK;
508ErrorCode WeyController::EnableSpeedOnlyMode() {
512 AINFO <<
"Already in AUTO_SPEED_ONLY mode";
513 return ErrorCode::OK;
523 if (!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG,
true)) {
524 AERROR <<
"Failed to switch to AUTO_SPEED_ONLY mode.";
527 return ErrorCode::CANBUS_ERROR;
530 AINFO <<
"Switch to AUTO_SPEED_ONLY mode ok.";
531 return ErrorCode::OK;
538 AINFO <<
"This drive mode no need to set gear.";
542 switch (gear_position) {
568 AERROR <<
"Gear command is invalid!";
581void WeyController::Brake(
double pedal) {
584 AINFO <<
"The current drive mode does not need to set acceleration.";
592void WeyController::Throttle(
double pedal) {
595 AINFO <<
"The current drive mode does not need to set acceleration.";
604void WeyController::Acceleration(
double acc) {
607 AINFO <<
"The current drive mode does not need to set acceleration.";
622void WeyController::Steer(
double angle) {
625 AINFO <<
"The current driving mode does not need to set steer.";
628 const double real_angle = 500 * angle / 100;
635void WeyController::Steer(
double angle,
double angle_spd) {
638 AINFO <<
"The current driving mode does not need to set steer.";
641 const double real_angle = 500 * angle / 100;
645void WeyController::SetEpbBreak(
const ControlCommand& command) {
646 if (command.parking_brake()) {
653ErrorCode WeyController::HandleCustomOperation(
654 const external_command::ChassisCommand& command) {
655 return ErrorCode::OK;
658void WeyController::SetBeam(
const VehicleSignal& vehicle_signal) {
659 if (vehicle_signal.has_high_beam() && vehicle_signal.high_beam()) {
661 }
else if (vehicle_signal.has_low_beam() && vehicle_signal.low_beam()) {
669void WeyController::SetHorn(
const VehicleSignal& vehicle_signal) {
670 if (vehicle_signal.horn()) {
677void WeyController::SetTurningSignal(
const VehicleSignal& vehicle_signal) {
679 auto signal = vehicle_signal.turn_signal();
689void WeyController::ResetProtocol() {
message_manager_->ResetSendMessages(); }
691bool WeyController::CheckChassisError() {
694 if (!chassis_.has_check_response()) {
695 AERROR_EVERY(100) <<
"ChassisDetail has NO wey vehicle info.";
700 if (chassis_detail.has_fail_241()) {
706 if (chassis_detail.has_fail_241()) {
712 if (chassis_detail.has_fail_241()) {
718 if (chassis_detail.has_fail_241()) {
719 if (chassis_detail.fail_241().shiftfail() ==
725 if (chassis_detail.has_fail_241()) {
733void WeyController::SecurityDogThreadFunc() {
734 int32_t vertical_ctrl_fail = 0;
735 int32_t horizontal_ctrl_fail = 0;
738 AERROR <<
"Failed to run SecurityDogThreadFunc() because can_sender_ is "
743 std::this_thread::yield();
746 std::chrono::duration<double, std::micro> default_period{50000};
752 bool emergency_mode =
false;
757 !CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG,
false)) {
758 ++horizontal_ctrl_fail;
759 if (horizontal_ctrl_fail >= kMaxFailAttempt) {
760 emergency_mode =
true;
764 horizontal_ctrl_fail = 0;
770 !CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG,
false)) {
771 ++vertical_ctrl_fail;
772 if (vertical_ctrl_fail >= kMaxFailAttempt) {
773 emergency_mode =
true;
777 vertical_ctrl_fail = 0;
779 if (CheckChassisError()) {
781 emergency_mode =
true;
789 std::chrono::duration<double, std::micro> elapsed{end - start};
790 if (elapsed < default_period) {
791 std::this_thread::sleep_for(default_period - elapsed);
793 AERROR <<
"Too much time consumption in WeyController looping process:"
799bool WeyController::CheckResponse(
const int32_t flags,
bool need_wait) {
801 bool is_eps_online =
false;
802 bool is_vcu_online =
false;
803 bool is_esp_online =
false;
811 bool check_ok =
true;
812 if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
813 is_eps_online = chassis_.has_check_response() &&
816 check_ok = check_ok && is_eps_online;
819 if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {
820 is_vcu_online = chassis_.has_check_response() &&
823 is_esp_online = chassis_.has_check_response() &&
826 check_ok = check_ok && is_vcu_online && is_esp_online;
830 std::this_thread::sleep_for(
831 std::chrono::duration<double, std::milli>(20));
836 AINFO <<
"Need to check response again.";
837 }
while (need_wait && retry_num);
839 AINFO <<
"check_response fail: is_eps_online: " << is_eps_online
840 <<
", is_vcu_online: " << is_vcu_online
841 <<
", is_esp_online: " << is_esp_online;
845void WeyController::set_chassis_error_mask(
const int32_t mask) {
846 std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
847 chassis_error_mask_ = mask;
850int32_t WeyController::chassis_error_mask() {
851 std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
852 return chassis_error_mask_;
856 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
857 return chassis_error_code_;
860void WeyController::set_chassis_error_code(
862 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
863 chassis_error_code_ = error_code;
Defines SenderMessage class and CanSender class.
common::VehicleParam vehicle_params_
canbus::VehicleParameter params_
CanSender< ::apollo::canbus::Wey > * can_sender_
virtual void set_driving_mode(const Chassis::DrivingMode &driving_mode)
MessageManager< ::apollo::canbus::Wey > * message_manager_
virtual Chassis::DrivingMode driving_mode()
Ads1111 * set_ads_taracce(double ads_taracce)
Ads1111 * set_ads_dectostop(Ads1_111::Ads_dectostopType ads_dectostop)
Ads1111 * set_ads_driveoff_req(Ads1_111::Ads_driveoff_reqType ads_driveoff_req)
Ads1111 * set_ads_mode(Ads1_111::Ads_modeType ads_mode)
Ads338e * set_hornon(Ads3_38e::HornonType hornon)
Ads338e * set_ads_reqcontrolbcm(Ads3_38e::Ads_reqcontrolbcmType ads_reqcontrolbcm)
Ads338e * set_ads_bcm_worksts(Ads3_38e::Ads_bcm_workstsType ads_bcm_worksts)
Ads338e * set_highbeamton(Ads3_38e::HighbeamtonType highbeamton)
Ads338e * set_turnllighton(Ads3_38e::TurnllightonType turnllighton)
Ads338e * set_ads_bcmworkstsvalid(Ads3_38e::Ads_bcmworkstsvalidType ads_bcmworkstsvalid)
Ads338e * set_dippedbeamon(Ads3_38e::DippedbeamonType dippedbeamon)
Adseps113 * set_ads_epsmode(Ads_eps_113::Ads_epsmodeType ads_epsmode)
Adseps113 * set_ads_reqepstargetangle(double ads_reqepstargetangle)
Adsreqvin390 * set_req_vin_signal(Ads_req_vin_390::Req_vin_signalType req_vin_signal)
Adsshifter115 * set_ads_shiftmode(Ads_shifter_115::Ads_shiftmodeType ads_shiftmode)
Adsshifter115 * set_ads_targetgear(Ads_shifter_115::Ads_targetgearType ads_targetgear)
bool Start() override
start the vehicle controller.
void Stop() override
stop the vehicle controller.
Chassis chassis() override
calculate and return the chassis.
::apollo::common::ErrorCode Init(const VehicleParameter ¶ms, CanSender<::apollo::canbus::Wey > *const can_sender, MessageManager<::apollo::canbus::Wey > *const message_manager) override
uint64_t ToMicrosecond() const
convert time to microsecond (us).
static Time Now()
get the current time.
#define AERROR_EVERY(freq)
The class of ProtocolData
@ ADS_DRIVEOFF_REQ_DEMAND
@ ADS_DRIVEOFF_REQ_NO_DEMAND
@ ADS_DECTOSTOP_NO_DEMAND
@ ADS_BCM_WORKSTS_DISABLE
@ ADS_BCMWORKSTSVALID_VALID
@ ADS_BCMWORKSTSVALID_INVALID
@ ADS_REQCONTROLBCM_REQUEST
@ ADS_REQCONTROLBCM_NO_REQUEST
@ TURNLLIGHTON_TURN_LEFT_ON
@ TURNLLIGHTON_TURN_RIGHT_ON
optional CheckResponse check_response
optional bool parking_brake
optional bool is_esp_online
optional bool is_vcu_online
optional bool is_eps_online
@ SHIFTFAIL_TRANSMISSION_P_ENGAGEMENT_FAULT
@ FRWHEELDIRECTION_FORWARD
@ FRWHEELDIRECTION_BACKWARD
optional FrwheeldirectionType frwheeldirection
optional double flwheelspd
optional double frwheelspd
optional double rrwheelspd
optional FlwheeldirectionType flwheeldirection
optional RrwheeldirectionType rrwheeldirection
optional double rlwheelspd
@ FLWHEELDIRECTION_BACKWARD
@ FLWHEELDIRECTION_FORWARD
optional double vehiclespd
@ RRWHEELDIRECTION_FORWARD
@ RRWHEELDIRECTION_BACKWARD
optional RlwheeldrivedirectionType rlwheeldrivedirection
@ RLWHEELDRIVEDIRECTION_STOP
@ RLWHEELDRIVEDIRECTION_FORWARD
@ RLWHEELDRIVEDIRECTION_BACKWARD
optional double accpedalpos
optional double epsdrvinputtrqvalue
optional Eps_streeingmodeType eps_streeingmode
optional CurrentgearType currentgear
optional double steerwheelangle
optional FlwheelspdvalidType flwheelspdvalid
optional RightturnlampstsType rightturnlampsts
optional LowbeamstsType lowbeamsts
optional RrwheelspdvalidType rrwheelspdvalid
optional LongitudedrivingmodeType longitudedrivingmode
optional EpbstsType epbsts
@ STEERWHEELANGLESIGN_LEFT_POSITIVE
@ STEERWHEELANGLESIGN_RIGHT_NEGATIVE
optional LeftturnlampstsType leftturnlampsts
optional FrwheelspdvalidType frwheelspdvalid
optional SteerwheelanglesignType steerwheelanglesign
optional RlwheelspdvalidType rlwheelspdvalid
optional Fbs3_237 fbs3_237
optional Status_310 status_310
optional Fbs2_240 fbs2_240
optional Vin_resp2_392 vin_resp2_392
optional Fbs4_235 fbs4_235
optional Fbs1_243 fbs1_243
optional Vin_resp1_391 vin_resp1_391
optional Vin_resp3_393 vin_resp3_393
optional double max_steer_angle
The class of VehicleController