Apollo 10.0
自动驾驶开放平台
wey_controller.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2019 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h"
20#include "cyber/time/time.h"
25
26namespace apollo {
27namespace canbus {
28namespace wey {
29
30using ::apollo::common::ErrorCode;
31using ::apollo::common::VehicleSignal;
32using ::apollo::control::ControlCommand;
33using ::apollo::drivers::canbus::ProtocolData;
34
35namespace {
36
37const int32_t kMaxFailAttempt = 10;
38const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1;
39const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2;
40double angle_init = 0;
41} // namespace
42
44 const VehicleParameter& params,
45 CanSender<::apollo::canbus::Wey>* const can_sender,
46 MessageManager<::apollo::canbus::Wey>* const message_manager) {
47 if (is_initialized_) {
48 AINFO << "WeyController has already been initialized.";
49 return ErrorCode::CANBUS_ERROR;
50 }
51 vehicle_params_.CopyFrom(
52 common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
53 params_.CopyFrom(params);
54 if (!params_.has_driving_mode()) {
55 AERROR << "Vehicle conf pb not set driving_mode.";
56 return ErrorCode::CANBUS_ERROR;
57 }
58
59 if (can_sender == nullptr) {
60 AERROR << "Canbus sender is null.";
61 return ErrorCode::CANBUS_ERROR;
62 }
63 can_sender_ = can_sender;
64
65 if (message_manager == nullptr) {
66 AERROR << "Protocol manager is null.";
67 return ErrorCode::CANBUS_ERROR;
68 }
69 message_manager_ = message_manager;
70
71 // sender part
72 ads1_111_ = dynamic_cast<Ads1111*>(
73 message_manager_->GetMutableProtocolDataById(Ads1111::ID));
74 if (ads1_111_ == nullptr) {
75 AERROR << "Ads1111 does not exist in the WeyMessageManager!";
76 return ErrorCode::CANBUS_ERROR;
77 }
78
79 ads3_38e_ = dynamic_cast<Ads338e*>(
80 message_manager_->GetMutableProtocolDataById(Ads338e::ID));
81 if (ads3_38e_ == nullptr) {
82 AERROR << "Ads338e does not exist in the WeyMessageManager!";
83 return ErrorCode::CANBUS_ERROR;
84 }
85
86 ads_eps_113_ = dynamic_cast<Adseps113*>(
87 message_manager_->GetMutableProtocolDataById(Adseps113::ID));
88 if (ads_eps_113_ == nullptr) {
89 AERROR << "Adseps113 does not exist in the WeyMessageManager!";
90 return ErrorCode::CANBUS_ERROR;
91 }
92
93 ads_req_vin_390_ = dynamic_cast<Adsreqvin390*>(
94 message_manager_->GetMutableProtocolDataById(Adsreqvin390::ID));
95 if (ads_req_vin_390_ == nullptr) {
96 AERROR << "Adsreqvin390 does not exist in the WeyMessageManager!";
97 return ErrorCode::CANBUS_ERROR;
98 }
99
100 ads_shifter_115_ = dynamic_cast<Adsshifter115*>(
101 message_manager_->GetMutableProtocolDataById(Adsshifter115::ID));
102 if (ads_shifter_115_ == nullptr) {
103 AERROR << "Adsshifter115 does not exist in the WeyMessageManager!";
104 return ErrorCode::CANBUS_ERROR;
105 }
106
107 can_sender_->AddMessage(Ads1111::ID, ads1_111_, false);
108 can_sender_->AddMessage(Ads338e::ID, ads3_38e_, false);
109 can_sender_->AddMessage(Adseps113::ID, ads_eps_113_, false);
110 can_sender_->AddMessage(Adsreqvin390::ID, ads_req_vin_390_, false);
111 can_sender_->AddMessage(Adsshifter115::ID, ads_shifter_115_, false);
112
113 // Need to sleep to ensure all messages received
114 AINFO << "WeyController is initialized.";
115
116 is_initialized_ = true;
117 return ErrorCode::OK;
118}
119
121
123 if (!is_initialized_) {
124 AERROR << "WeyController has NOT been initialized.";
125 return false;
126 }
127 const auto& update_func = [this] { SecurityDogThreadFunc(); };
128 thread_.reset(new std::thread(update_func));
129
130 return true;
131}
132
134 if (!is_initialized_) {
135 AERROR << "WeyController stops or starts improperly!";
136 return;
137 }
138
139 if (thread_ != nullptr && thread_->joinable()) {
140 thread_->join();
141 thread_.reset();
142 AINFO << "WeyController stopped.";
143 }
144}
145
147 chassis_.Clear();
148
149 Wey chassis_detail;
150 message_manager_->GetSensorData(&chassis_detail);
151
152 // 21, 22, previously 1, 2
153 // if (driving_mode() == Chassis::EMERGENCY_MODE) {
154 // set_chassis_error_code(Chassis::NO_ERROR);
155 // }
156
157 chassis_.set_driving_mode(driving_mode());
158 chassis_.set_error_code(chassis_error_code());
159
160 // 3
161 chassis_.set_engine_started(true);
162
163 // 4 engine_rpm
164 if (chassis_detail.has_fbs3_237() && chassis_detail.fbs3_237().has_engspd()) {
165 chassis_.set_engine_rpm(
166 static_cast<float>(chassis_detail.fbs3_237().engspd()));
167 } else {
168 chassis_.set_engine_rpm(0);
169 }
170
171 // 5 6
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()) {
175 Fbs2_240 fbs2_240 = chassis_detail.fbs2_240();
176 Fbs1_243 fbs1_243 = chassis_detail.fbs1_243();
177 Status_310 status_310 = chassis_detail.status_310();
178 // speed_mps
179 chassis_.set_speed_mps(static_cast<float>(fbs2_240.vehiclespd()));
180 // rr
181 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rr_valid(
182 static_cast<bool>(status_310.rrwheelspdvalid()));
184 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
186 } else if (fbs2_240.rrwheeldirection() ==
188 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
190 } else if (fbs2_240.rrwheeldirection() == Fbs2_240::RRWHEELDIRECTION_STOP) {
191 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
193 } else {
194 chassis_.mutable_wheel_speed()->set_wheel_direction_rr(
196 }
197 chassis_.mutable_wheel_speed()->set_wheel_spd_rr(fbs2_240.rrwheelspd());
198 // rl
199 chassis_.mutable_wheel_speed()->set_is_wheel_spd_rl_valid(
200 static_cast<bool>(status_310.rlwheelspdvalid()));
201 if (fbs2_240.rlwheeldrivedirection() ==
203 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
205 } else if (fbs2_240.rlwheeldrivedirection() ==
207 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
209 } else if (fbs2_240.rlwheeldrivedirection() ==
211 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
213 } else {
214 chassis_.mutable_wheel_speed()->set_wheel_direction_rl(
216 }
217 chassis_.mutable_wheel_speed()->set_wheel_spd_rl(fbs2_240.rlwheelspd());
218 // fr
219 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fr_valid(
220 static_cast<bool>(status_310.frwheelspdvalid()));
222 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
224 } else if (fbs1_243.frwheeldirection() ==
226 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
228 } else if (fbs1_243.frwheeldirection() == Fbs1_243::FRWHEELDIRECTION_STOP) {
229 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
231 } else {
232 chassis_.mutable_wheel_speed()->set_wheel_direction_fr(
234 }
235 chassis_.mutable_wheel_speed()->set_wheel_spd_fr(fbs2_240.frwheelspd());
236 // fl
237 chassis_.mutable_wheel_speed()->set_is_wheel_spd_fl_valid(
238 static_cast<bool>(status_310.flwheelspdvalid()));
240 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
242 } else if (fbs2_240.flwheeldirection() ==
244 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
246 } else if (fbs2_240.flwheeldirection() == Fbs2_240::FLWHEELDIRECTION_STOP) {
247 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
249 } else {
250 chassis_.mutable_wheel_speed()->set_wheel_direction_fl(
252 }
253 chassis_.mutable_wheel_speed()->set_wheel_spd_fl(fbs1_243.flwheelspd());
254 } else {
255 chassis_.set_speed_mps(0);
256 }
257 // 7
258 chassis_.set_fuel_range_m(0);
259 // 8
260 if (chassis_detail.has_fbs3_237() &&
261 chassis_detail.fbs3_237().has_accpedalpos()) {
262 chassis_.set_throttle_percentage(
263 static_cast<float>(chassis_detail.fbs3_237().accpedalpos()));
264 } else {
265 chassis_.set_throttle_percentage(0);
266 }
267 // 23, previously 10 gear position
268 if (chassis_detail.has_fbs3_237() &&
269 chassis_detail.fbs3_237().has_currentgear()) {
270 switch (chassis_detail.fbs3_237().currentgear()) {
272 chassis_.set_gear_location(Chassis::GEAR_DRIVE);
273 } break;
275 chassis_.set_gear_location(Chassis::GEAR_NEUTRAL);
276 } break;
278 chassis_.set_gear_location(Chassis::GEAR_REVERSE);
279 } break;
281 chassis_.set_gear_location(Chassis::GEAR_PARKING);
282 } break;
283 default:
284 chassis_.set_gear_location(Chassis::GEAR_INVALID);
285 break;
286 }
287 } else {
288 chassis_.set_gear_location(Chassis::GEAR_NONE);
289 }
290 // 11
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()) {
295 if (chassis_detail.status_310().steerwheelanglesign() ==
297 chassis_.set_steering_percentage(static_cast<float>(
298 chassis_detail.fbs4_235().steerwheelangle() * 100.0 /
299 vehicle_params_.max_steer_angle() * M_PI / 180));
300 angle_init = chassis_detail.fbs4_235().steerwheelangle();
301 } else if (chassis_detail.status_310().steerwheelanglesign() ==
303 chassis_.set_steering_percentage(static_cast<float>(
304 chassis_detail.fbs4_235().steerwheelangle() * (-1) * 100.0 /
305 vehicle_params_.max_steer_angle() * M_PI / 180));
306 angle_init = chassis_detail.fbs4_235().steerwheelangle() * (-1);
307 } else {
308 chassis_.set_steering_percentage(0);
309 }
310 } else {
311 chassis_.set_steering_percentage(0);
312 }
313
314 // 12
315 if (chassis_detail.has_fbs3_237() &&
316 chassis_detail.fbs3_237().has_epsdrvinputtrqvalue()) {
317 chassis_.set_steering_torque_nm(
318 static_cast<float>(chassis_detail.fbs3_237().epsdrvinputtrqvalue()));
319 } else {
320 chassis_.set_steering_torque_nm(0);
321 }
322 // 13
323 if (chassis_detail.has_status_310() &&
324 chassis_detail.status_310().has_epbsts()) {
325 chassis_.set_parking_brake(chassis_detail.status_310().epbsts() ==
327 } else {
328 chassis_.set_parking_brake(false);
329 }
330 // 14, 15
331 if (chassis_detail.has_status_310() &&
332 chassis_detail.status_310().has_lowbeamsts() &&
333 chassis_detail.status_310().lowbeamsts() == Status_310::LOWBEAMSTS_ON) {
334 chassis_.mutable_signal()->set_low_beam(true);
335 } else {
336 chassis_.mutable_signal()->set_low_beam(false);
337 }
338 // 16, 17
339 if (chassis_detail.has_status_310()) {
340 if (chassis_detail.status_310().has_leftturnlampsts() &&
341 chassis_detail.status_310().leftturnlampsts() ==
343 chassis_.mutable_signal()->set_turn_signal(
345 } else if (chassis_detail.status_310().has_rightturnlampsts() &&
346 chassis_detail.status_310().rightturnlampsts() ==
348 chassis_.mutable_signal()->set_turn_signal(
350 } else {
351 chassis_.mutable_signal()->set_turn_signal(
353 }
354 } else {
355 chassis_.mutable_signal()->set_turn_signal(
357 }
358 // 18
359 if (chassis_detail.has_vin_resp1_391() &&
360 chassis_detail.has_vin_resp2_392() &&
361 chassis_detail.has_vin_resp3_393()) {
362 Vin_resp1_391 vin_resp1_391 = chassis_detail.vin_resp1_391();
363 Vin_resp2_392 vin_resp2_392 = chassis_detail.vin_resp2_392();
364 Vin_resp3_393 vin_resp3_393 = chassis_detail.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()) {
374 int n[17];
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();
392 char ch[17];
393 memset(&ch, '\0', sizeof(ch));
394 for (int i = 0; i < 17; i++) {
395 ch[i] = static_cast<char>(n[i]);
396 }
397 chassis_.mutable_vehicle_id()->set_vin(ch);
398 }
399 }
400
401 if (chassis_error_mask_) {
402 chassis_.set_chassis_error_mask(chassis_error_mask_);
403 }
404 // Give engage_advice based on error_code and canbus feedback
405 if (!chassis_error_mask_ && !chassis_.parking_brake()) {
406 chassis_.mutable_engage_advice()->set_advice(
408 } else {
409 chassis_.mutable_engage_advice()->set_advice(
411 }
412
413 // 19 add checkresponse signal
414 if (chassis_detail.has_fbs3_237() &&
415 chassis_detail.fbs3_237().has_eps_streeingmode()) {
416 chassis_.mutable_check_response()->set_is_eps_online(
417 chassis_detail.fbs3_237().eps_streeingmode() == 1);
418 }
419 if (chassis_detail.has_status_310() &&
420 chassis_detail.status_310().has_longitudedrivingmode()) {
421 chassis_.mutable_check_response()->set_is_esp_online(
422 chassis_detail.status_310().longitudedrivingmode() == 1);
423 chassis_.mutable_check_response()->set_is_vcu_online(
424 chassis_detail.status_310().longitudedrivingmode() == 1);
425 }
426
427 return chassis_;
428}
429
430bool WeyController::VerifyID() { return true; }
431
432void WeyController::Emergency() {
434 ResetProtocol();
435}
436
437ErrorCode WeyController::EnableAutoMode() {
439 AINFO << "Already in COMPLETE_AUTO_DRIVE mode.";
440 return ErrorCode::OK;
441 }
444 // unlock the limited steering angle for first cmd within [-10,10] deg
445 ads_eps_113_->set_ads_reqepstargetangle(angle_init);
448 // BCM enable control for horn/ beam/ turnlight
449 // notice : if BCM enable, the beam manual control is invalid. If you use the
450 // car at night, please watch out or disable this function before auto_drive
455
456 can_sender_->Update();
457 const int32_t flag =
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.";
462 Emergency();
463 set_chassis_error_code(Chassis::CHASSIS_ERROR);
464 return ErrorCode::CANBUS_ERROR;
465 }
467 AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";
468 return ErrorCode::OK;
469}
470
471ErrorCode WeyController::DisableAutoMode() {
472 ResetProtocol();
473 can_sender_->Update();
475 set_chassis_error_code(Chassis::NO_ERROR);
476 AINFO << "Switch to COMPLETE_MANUAL ok.";
477 return ErrorCode::OK;
478}
479
480ErrorCode WeyController::EnableSteeringOnlyMode() {
484 AINFO << "Already in AUTO_STEER_ONLY mode";
485 return ErrorCode::OK;
486 }
487
490 ads_eps_113_->set_ads_reqepstargetangle(angle_init);
495
496 can_sender_->Update();
497 if (!CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, true)) {
498 AERROR << "Failed to switch to AUTO_STEER_ONLY mode.";
499 Emergency();
500 set_chassis_error_code(Chassis::CHASSIS_ERROR);
501 return ErrorCode::CANBUS_ERROR;
502 }
504 AINFO << "Switch to AUTO_STEER_ONLY mode ok.";
505 return ErrorCode::OK;
506}
507
508ErrorCode WeyController::EnableSpeedOnlyMode() {
512 AINFO << "Already in AUTO_SPEED_ONLY mode";
513 return ErrorCode::OK;
514 }
521
522 can_sender_->Update();
523 if (!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, true)) {
524 AERROR << "Failed to switch to AUTO_SPEED_ONLY mode.";
525 Emergency();
526 set_chassis_error_code(Chassis::CHASSIS_ERROR);
527 return ErrorCode::CANBUS_ERROR;
528 }
530 AINFO << "Switch to AUTO_SPEED_ONLY mode ok.";
531 return ErrorCode::OK;
532}
533
534// NEUTRAL, REVERSE, DRIVE, PARK
535void WeyController::Gear(Chassis::GearPosition gear_position) {
538 AINFO << "This drive mode no need to set gear.";
539 return;
540 }
541
542 switch (gear_position) {
545 break;
546 }
549 break;
550 }
551 case Chassis::GEAR_DRIVE: {
553 break;
554 }
557 break;
558 }
559 case Chassis::GEAR_LOW: {
561 break;
562 }
563 case Chassis::GEAR_NONE: {
565 break;
566 }
568 AERROR << "Gear command is invalid!";
570 break;
571 }
572 default: {
574 break;
575 }
576 }
577}
578
579// brake with pedal
580// acceleration:-7.0 ~ 5.0, unit:m/s^2
581void WeyController::Brake(double pedal) {
584 AINFO << "The current drive mode does not need to set acceleration.";
585 return;
586 }
587 // None
588}
589
590// drive with pedal
591// acceleration:-7.0 ~ 5.0, unit:m/s^2
592void WeyController::Throttle(double pedal) {
595 AINFO << "The current drive mode does not need to set acceleration.";
596 return;
597 }
598 // None
599}
600
601// wey use the acc to control the car acceleration and deceleration
602// drive with acceleration/deceleration
603// acc:-7.0 ~ 5.0, unit:m/s^2
604void WeyController::Acceleration(double acc) {
607 AINFO << "The current drive mode does not need to set acceleration.";
608 return;
609 }
610 if (acc >= 0) {
613 } else {
616 }
617 ads1_111_->set_ads_taracce(acc);
618}
619
620// wey default, -500 ~ 500, left:+, right:-
621// angle:-99.99~0.00~99.99, unit:, left:+, right:-
622void WeyController::Steer(double angle) {
625 AINFO << "The current driving mode does not need to set steer.";
626 return;
627 }
628 const double real_angle = 500 * angle / 100;
629 ads_eps_113_->set_ads_reqepstargetangle(real_angle);
630}
631
632// steering with new angle speed
633// angle:-99.99~0.00~99.99, unit:, left:-, right:+
634// wey has no angle_spd
635void WeyController::Steer(double angle, double angle_spd) {
638 AINFO << "The current driving mode does not need to set steer.";
639 return;
640 }
641 const double real_angle = 500 * angle / 100;
642 ads_eps_113_->set_ads_reqepstargetangle(real_angle);
643}
644
645void WeyController::SetEpbBreak(const ControlCommand& command) {
646 if (command.parking_brake()) {
647 // None
648 } else {
649 // None
650 }
651}
652
653ErrorCode WeyController::HandleCustomOperation(
654 const external_command::ChassisCommand& command) {
655 return ErrorCode::OK;
656}
657
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()) {
663 } else {
666 }
667}
668
669void WeyController::SetHorn(const VehicleSignal& vehicle_signal) {
670 if (vehicle_signal.horn()) {
672 } else {
674 }
675}
676
677void WeyController::SetTurningSignal(const VehicleSignal& vehicle_signal) {
678 // Set Turn Signal
679 auto signal = vehicle_signal.turn_signal();
680 if (signal == common::VehicleSignal::TURN_LEFT) {
682 } else if (signal == common::VehicleSignal::TURN_RIGHT) {
684 } else {
686 }
687}
688
689void WeyController::ResetProtocol() { message_manager_->ResetSendMessages(); }
690
691bool WeyController::CheckChassisError() {
692 Wey chassis_detail;
693 message_manager_->GetSensorData(&chassis_detail);
694 if (!chassis_.has_check_response()) {
695 AERROR_EVERY(100) << "ChassisDetail has NO wey vehicle info.";
696 return false;
697 }
698
699 // check steer error
700 if (chassis_detail.has_fail_241()) {
701 if (chassis_detail.fail_241().epsfail() == Fail_241::EPSFAIL_FAULT) {
702 return true;
703 }
704 }
705 // check ems error
706 if (chassis_detail.has_fail_241()) {
707 if (chassis_detail.fail_241().engfail() == Fail_241::ENGFAIL_FAIL) {
708 return true;
709 }
710 }
711 // check braking error
712 if (chassis_detail.has_fail_241()) {
713 if (chassis_detail.fail_241().espfail() == Fail_241::ESPFAIL_FAILURE) {
714 return true;
715 }
716 }
717 // check gear error question
718 if (chassis_detail.has_fail_241()) {
719 if (chassis_detail.fail_241().shiftfail() ==
721 return true;
722 }
723 }
724 // check parking error
725 if (chassis_detail.has_fail_241()) {
726 if (chassis_detail.fail_241().epbfail() == Fail_241::EPBFAIL_ERROR) {
727 return true;
728 }
729 }
730 return false;
731}
732
733void WeyController::SecurityDogThreadFunc() {
734 int32_t vertical_ctrl_fail = 0;
735 int32_t horizontal_ctrl_fail = 0;
736
737 if (can_sender_ == nullptr) {
738 AERROR << "Failed to run SecurityDogThreadFunc() because can_sender_ is "
739 "nullptr.";
740 return;
741 }
742 while (!can_sender_->IsRunning()) {
743 std::this_thread::yield();
744 }
745
746 std::chrono::duration<double, std::micro> default_period{50000};
747 int64_t start = 0;
748 int64_t end = 0;
749 while (can_sender_->IsRunning()) {
751 const Chassis::DrivingMode mode = driving_mode();
752 bool emergency_mode = false;
753
754 // 1. Horizontal control check
755 if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
756 mode == Chassis::AUTO_STEER_ONLY) &&
757 !CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, false)) {
758 ++horizontal_ctrl_fail;
759 if (horizontal_ctrl_fail >= kMaxFailAttempt) {
760 emergency_mode = true;
761 set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
762 }
763 } else {
764 horizontal_ctrl_fail = 0;
765 }
766
767 // 2. Vertical control check
768 if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
769 mode == Chassis::AUTO_SPEED_ONLY) &&
770 !CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, false)) {
771 ++vertical_ctrl_fail;
772 if (vertical_ctrl_fail >= kMaxFailAttempt) {
773 emergency_mode = true;
774 set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
775 }
776 } else {
777 vertical_ctrl_fail = 0;
778 }
779 if (CheckChassisError()) {
780 set_chassis_error_code(Chassis::CHASSIS_ERROR);
781 emergency_mode = true;
782 }
783
784 if (emergency_mode && mode != Chassis::EMERGENCY_MODE) {
786 message_manager_->ResetSendMessages();
787 }
789 std::chrono::duration<double, std::micro> elapsed{end - start};
790 if (elapsed < default_period) {
791 std::this_thread::sleep_for(default_period - elapsed);
792 } else {
793 AERROR << "Too much time consumption in WeyController looping process:"
794 << elapsed.count();
795 }
796 }
797}
798
799bool WeyController::CheckResponse(const int32_t flags, bool need_wait) {
800 Wey chassis_detail;
801 bool is_eps_online = false;
802 bool is_vcu_online = false;
803 bool is_esp_online = false;
804 int retry_num = 20;
805
806 do {
807 if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) {
808 AERROR_EVERY(100) << "Get chassis detail failed.";
809 return false;
810 }
811 bool check_ok = true;
812 if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
813 is_eps_online = chassis_.has_check_response() &&
814 chassis_.check_response().has_is_eps_online() &&
815 chassis_.check_response().is_eps_online();
816 check_ok = check_ok && is_eps_online;
817 }
818
819 if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {
820 is_vcu_online = chassis_.has_check_response() &&
821 chassis_.check_response().has_is_vcu_online() &&
822 chassis_.check_response().is_vcu_online();
823 is_esp_online = chassis_.has_check_response() &&
824 chassis_.check_response().has_is_esp_online() &&
825 chassis_.check_response().is_esp_online();
826 check_ok = check_ok && is_vcu_online && is_esp_online;
827 }
828 if (need_wait) {
829 --retry_num;
830 std::this_thread::sleep_for(
831 std::chrono::duration<double, std::milli>(20));
832 }
833 if (check_ok) {
834 return true;
835 }
836 AINFO << "Need to check response again.";
837 } while (need_wait && retry_num);
838
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;
842 return false;
843}
844
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;
848}
849
850int32_t WeyController::chassis_error_mask() {
851 std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
852 return chassis_error_mask_;
853}
854
855Chassis::ErrorCode WeyController::chassis_error_code() {
856 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
857 return chassis_error_code_;
858}
859
860void WeyController::set_chassis_error_code(
861 const Chassis::ErrorCode& error_code) {
862 std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
863 chassis_error_code_ = error_code;
864}
865
866} // namespace wey
867} // namespace canbus
868} // namespace apollo
Defines SenderMessage class and CanSender class.
virtual void set_driving_mode(const Chassis::DrivingMode &driving_mode)
MessageManager< ::apollo::canbus::Wey > * message_manager_
Ads1111 * set_ads_taracce(double ads_taracce)
Definition ads1_111.cc:93
Ads1111 * set_ads_dectostop(Ads1_111::Ads_dectostopType ads_dectostop)
Definition ads1_111.cc:57
Ads1111 * set_ads_driveoff_req(Ads1_111::Ads_driveoff_reqType ads_driveoff_req)
Definition ads1_111.cc:110
static const int32_t ID
Definition ads1_111.h:29
Ads1111 * set_ads_mode(Ads1_111::Ads_modeType ads_mode)
Definition ads1_111.cc:75
Ads338e * set_hornon(Ads3_38e::HornonType hornon)
Definition ads3_38e.cc:258
Ads338e * set_ads_reqcontrolbcm(Ads3_38e::Ads_reqcontrolbcmType ads_reqcontrolbcm)
Definition ads3_38e.cc:111
Ads338e * set_ads_bcm_worksts(Ads3_38e::Ads_bcm_workstsType ads_bcm_worksts)
Definition ads3_38e.cc:71
Ads338e * set_highbeamton(Ads3_38e::HighbeamtonType highbeamton)
Definition ads3_38e.cc:130
static const int32_t ID
Definition ads3_38e.h:29
Ads338e * set_turnllighton(Ads3_38e::TurnllightonType turnllighton)
Definition ads3_38e.cc:166
Ads338e * set_ads_bcmworkstsvalid(Ads3_38e::Ads_bcmworkstsvalidType ads_bcmworkstsvalid)
Definition ads3_38e.cc:92
Ads338e * set_dippedbeamon(Ads3_38e::DippedbeamonType dippedbeamon)
Definition ads3_38e.cc:148
Adseps113 * set_ads_epsmode(Ads_eps_113::Ads_epsmodeType ads_epsmode)
static const int32_t ID
Definition ads_eps_113.h:29
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 &params, CanSender<::apollo::canbus::Wey > *const can_sender, MessageManager<::apollo::canbus::Wey > *const message_manager) override
uint64_t ToMicrosecond() const
convert time to microsecond (us).
Definition time.cc:85
static Time Now()
get the current time.
Definition time.cc:57
#define AERROR
Definition log.h:44
#define AERROR_EVERY(freq)
Definition log.h:86
#define AINFO
Definition log.h:42
class register implement
Definition arena_queue.h:37
The class of ProtocolData
optional CheckResponse check_response
optional bool parking_brake
Definition chassis.proto:94
@ SHIFTFAIL_TRANSMISSION_P_ENGAGEMENT_FAULT
Definition wey.proto:429
optional FrwheeldirectionType frwheeldirection
Definition wey.proto:399
optional double flwheelspd
Definition wey.proto:397
optional double frwheelspd
Definition wey.proto:369
optional double rrwheelspd
Definition wey.proto:377
optional FlwheeldirectionType flwheeldirection
Definition wey.proto:367
optional RrwheeldirectionType rrwheeldirection
Definition wey.proto:375
optional double rlwheelspd
Definition wey.proto:373
optional double vehiclespd
Definition wey.proto:379
optional RlwheeldrivedirectionType rlwheeldrivedirection
Definition wey.proto:371
optional double accpedalpos
Definition wey.proto:486
optional double epsdrvinputtrqvalue
Definition wey.proto:494
optional Eps_streeingmodeType eps_streeingmode
Definition wey.proto:492
optional CurrentgearType currentgear
Definition wey.proto:490
optional double engspd
Definition wey.proto:484
optional double steerwheelangle
Definition wey.proto:405
optional FlwheelspdvalidType flwheelspdvalid
Definition wey.proto:192
optional RightturnlampstsType rightturnlampsts
Definition wey.proto:250
optional LowbeamstsType lowbeamsts
Definition wey.proto:244
optional RrwheelspdvalidType rrwheelspdvalid
Definition wey.proto:198
optional LongitudedrivingmodeType longitudedrivingmode
Definition wey.proto:203
optional EpbstsType epbsts
Definition wey.proto:216
optional LeftturnlampstsType leftturnlampsts
Definition wey.proto:248
optional FrwheelspdvalidType frwheelspdvalid
Definition wey.proto:194
optional SteerwheelanglesignType steerwheelanglesign
Definition wey.proto:228
optional RlwheelspdvalidType rlwheelspdvalid
Definition wey.proto:196
optional Fbs3_237 fbs3_237
Definition wey.proto:601
optional Status_310 status_310
Definition wey.proto:591
optional Fbs2_240 fbs2_240
Definition wey.proto:597
optional Vin_resp2_392 vin_resp2_392
Definition wey.proto:593
optional Fbs4_235 fbs4_235
Definition wey.proto:599
optional Fbs1_243 fbs1_243
Definition wey.proto:598
optional Vin_resp1_391 vin_resp1_391
Definition wey.proto:594
optional Vin_resp3_393 vin_resp3_393
Definition wey.proto:592
The class of VehicleController