Skip to content

Commit c13f25f

Browse files
lyklinglykling
authored
feat(canbus): update protocol of vehicle ch, devkit and neolix_edu (#66)
Signed-off-by: Pride Leong <[email protected]>
1 parent 8ce0c0d commit c13f25f

File tree

91 files changed

+4898
-820
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

91 files changed

+4898
-820
lines changed

‎modules/canbus/vehicle/ch/BUILD‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,13 @@ load("//tools:cpplint.bzl", "cpplint")
33

44
package(default_visibility = ["//visibility:public"])
55

6+
CANBUS_COPTS = ["-DMODULE_NAME=\\\"canbus\\\""]
7+
68
cc_library(
79
name = "ch_vehicle_factory",
810
srcs = ["ch_vehicle_factory.cc"],
911
hdrs = ["ch_vehicle_factory.h"],
12+
copts = CANBUS_COPTS,
1013
deps = [
1114
":ch_controller",
1215
":ch_message_manager",
@@ -18,6 +21,7 @@ cc_library(
1821
name = "ch_message_manager",
1922
srcs = ["ch_message_manager.cc"],
2023
hdrs = ["ch_message_manager.h"],
24+
copts = CANBUS_COPTS,
2125
deps = [
2226
"//modules/common_msgs/chassis_msgs:chassis_detail_cc_proto",
2327
"//modules/canbus/vehicle/ch/protocol:canbus_ch_protocol",
@@ -30,6 +34,7 @@ cc_library(
3034
name = "ch_controller",
3135
srcs = ["ch_controller.cc"],
3236
hdrs = ["ch_controller.h"],
37+
copts = CANBUS_COPTS,
3338
deps = [
3439
":ch_message_manager",
3540
"//modules/canbus/proto:canbus_conf_cc_proto",

‎modules/canbus/vehicle/ch/ch_controller.cc‎

Lines changed: 173 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@
1616

1717
#include "modules/canbus/vehicle/ch/ch_controller.h"
1818

19+
#include <string>
20+
1921
#include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h"
2022

2123
#include "cyber/common/log.h"
@@ -104,12 +106,21 @@ ErrorCode ChController::Init(
104106
return ErrorCode::CANBUS_ERROR;
105107
}
106108

109+
vehicle_mode_command_116_ = dynamic_cast<Vehiclemodecommand116*>(
110+
message_manager_->GetMutableProtocolDataById(Vehiclemodecommand116::ID));
111+
if (vehicle_mode_command_116_ == nullptr) {
112+
AERROR << "Vehiclemodecommand116 does not exist in the ChMessageManager!";
113+
return ErrorCode::CANBUS_ERROR;
114+
}
115+
107116
can_sender_->AddMessage(Brakecommand111::ID, brake_command_111_, false);
108117
can_sender_->AddMessage(Gearcommand114::ID, gear_command_114_, false);
109118
can_sender_->AddMessage(Steercommand112::ID, steer_command_112_, false);
110119
can_sender_->AddMessage(Throttlecommand110::ID, throttle_command_110_, false);
111120
can_sender_->AddMessage(Turnsignalcommand113::ID, turnsignal_command_113_,
112121
false);
122+
can_sender_->AddMessage(Vehiclemodecommand116::ID, vehicle_mode_command_116_,
123+
false);
113124

114125
// need sleep to ensure all messages received
115126
AINFO << "ChController is initialized.";
@@ -151,9 +162,9 @@ Chassis ChController::chassis() {
151162
message_manager_->GetSensorData(&chassis_detail);
152163

153164
// 21, 22, previously 1, 2
154-
if (driving_mode() == Chassis::EMERGENCY_MODE) {
155-
set_chassis_error_code(Chassis::NO_ERROR);
156-
}
165+
// if (driving_mode() == Chassis::EMERGENCY_MODE) {
166+
// set_chassis_error_code(Chassis::NO_ERROR);
167+
// }
157168

158169
chassis_.set_driving_mode(driving_mode());
159170
chassis_.set_error_code(chassis_error_code());
@@ -191,8 +202,7 @@ Chassis ChController::chassis() {
191202
} else {
192203
chassis_.set_brake_percentage(0);
193204
}
194-
195-
// 23, previously 10 gear
205+
// 10 gear
196206
if (chassis_detail.ch().has_gear_status_514() &&
197207
chassis_detail.ch().gear_status_514().has_gear_sts()) {
198208
Chassis::GearPosition gear_pos = Chassis::GEAR_INVALID;
@@ -227,32 +237,85 @@ Chassis ChController::chassis() {
227237
} else {
228238
chassis_.set_steering_percentage(0);
229239
}
230-
231-
// 26
232-
if (chassis_error_mask_) {
233-
chassis_.set_chassis_error_mask(chassis_error_mask_);
240+
// 12 battery soc
241+
if (chassis_detail.ch().has_ecu_status_2_516() &&
242+
chassis_detail.ch().ecu_status_2_516().has_battery_soc()) {
243+
chassis_.set_battery_soc_percentage(
244+
chassis_detail.ch().ecu_status_2_516().battery_soc());
234245
}
235-
246+
// 13
236247
if (chassis_detail.has_surround()) {
237248
chassis_.mutable_surround()->CopyFrom(chassis_detail.surround());
238249
}
239-
240-
// give engage_advice based on error_code and canbus feedback
241-
if (!chassis_error_mask_ && (chassis_.throttle_percentage() == 0.0)) {
250+
// 14 give engage_advice based on error_code and battery low soc warn
251+
if (!chassis_error_mask_ && chassis_.battery_soc_percentage() > 15.0) {
242252
chassis_.mutable_engage_advice()->set_advice(
243253
apollo::common::EngageAdvice::READY_TO_ENGAGE);
244254
} else {
245255
chassis_.mutable_engage_advice()->set_advice(
246256
apollo::common::EngageAdvice::DISALLOW_ENGAGE);
247-
chassis_.mutable_engage_advice()->set_reason(
248-
"CANBUS not ready, throttle percentage is not zero!");
257+
if (chassis_.battery_soc_percentage() > 0) {
258+
chassis_.mutable_engage_advice()->set_reason(
259+
"Battery soc percentage is lower than 15%, please charge it "
260+
"quickly!");
261+
}
249262
}
250-
251-
// 27 battery soc
252-
if (chassis_detail.ch().has_ecu_status_2_516() &&
253-
chassis_detail.ch().ecu_status_2_516().has_battery_soc()) {
254-
chassis_.set_battery_soc_percentage(
255-
chassis_detail.ch().ecu_status_2_516().battery_soc());
263+
// 15 set vin
264+
// vin set 17 bits, like LSBN1234567890123 is prased as
265+
// vin17(L),vin16(S),vin15(B),.....,vin03(1)vin02(2),vin01(3)
266+
std::string vin = "";
267+
if (chassis_detail.ch().has_vin_resp1_51b()) {
268+
Vin_resp1_51b vin_51b = chassis_detail.ch().vin_resp1_51b();
269+
vin += vin_51b.vin01();
270+
vin += vin_51b.vin02();
271+
vin += vin_51b.vin03();
272+
vin += vin_51b.vin04();
273+
vin += vin_51b.vin05();
274+
vin += vin_51b.vin06();
275+
vin += vin_51b.vin07();
276+
vin += vin_51b.vin08();
277+
}
278+
if (chassis_detail.ch().has_vin_resp2_51c()) {
279+
Vin_resp2_51c vin_51c = chassis_detail.ch().vin_resp2_51c();
280+
vin += vin_51c.vin09();
281+
vin += vin_51c.vin10();
282+
vin += vin_51c.vin11();
283+
vin += vin_51c.vin12();
284+
vin += vin_51c.vin13();
285+
vin += vin_51c.vin14();
286+
vin += vin_51c.vin15();
287+
vin += vin_51c.vin16();
288+
}
289+
if (chassis_detail.ch().has_vin_resp3_51d()) {
290+
Vin_resp3_51d vin_51d = chassis_detail.ch().vin_resp3_51d();
291+
vin += vin_51d.vin17();
292+
}
293+
std::reverse(vin.begin(), vin.end());
294+
chassis_.mutable_vehicle_id()->set_vin(vin);
295+
296+
// 16 front bumper event
297+
if (chassis_detail.ch().has_brake_status__511() &&
298+
chassis_detail.ch().brake_status__511().has_front_bump_env()) {
299+
if (chassis_detail.ch().brake_status__511().front_bump_env() ==
300+
Brake_status__511::FRONT_BUMP_ENV_FRONT_BUMPER_ENV) {
301+
chassis_.set_front_bumper_event(Chassis::BUMPER_PRESSED);
302+
} else {
303+
chassis_.set_front_bumper_event(Chassis::BUMPER_NORMAL);
304+
}
305+
} else {
306+
chassis_.set_front_bumper_event(Chassis::BUMPER_INVALID);
307+
}
308+
// 17 back bumper event
309+
if (chassis_detail.ch().has_brake_status__511() &&
310+
chassis_detail.ch().brake_status__511().has_back_bump_env()) {
311+
if (chassis_detail.ch().brake_status__511().back_bump_env() ==
312+
Brake_status__511::BACK_BUMP_ENV_BACK_BUMPER_ENV) {
313+
chassis_.set_back_bumper_event(Chassis::BUMPER_PRESSED);
314+
} else {
315+
chassis_.set_back_bumper_event(Chassis::BUMPER_NORMAL);
316+
}
317+
} else {
318+
chassis_.set_back_bumper_event(Chassis::BUMPER_INVALID);
256319
}
257320

258321
return chassis_;
@@ -275,12 +338,14 @@ ErrorCode ChController::EnableAutoMode() {
275338
Throttle_command_110::THROTTLE_PEDAL_EN_CTRL_ENABLE);
276339
steer_command_112_->set_steer_angle_en_ctrl(
277340
Steer_command_112::STEER_ANGLE_EN_CTRL_ENABLE);
278-
AINFO << "\n\n\n set enable \n\n\n";
341+
AINFO << "set enable";
342+
279343
can_sender_->Update();
280344
const int32_t flag =
281345
CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
282346
if (!CheckResponse(flag, true)) {
283-
AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode.";
347+
AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode. Please check the "
348+
"emergency button or chassis.";
284349
Emergency();
285350
set_chassis_error_code(Chassis::CHASSIS_ERROR);
286351
return ErrorCode::CANBUS_ERROR;
@@ -337,7 +402,7 @@ void ChController::Gear(Chassis::GearPosition gear_position) {
337402
break;
338403
}
339404
case Chassis::GEAR_INVALID: {
340-
AERROR << "Gear command is invalid!";
405+
// AERROR << "Gear command is invalid!";
341406
gear_command_114_->set_gear_cmd(Gear_command_114::GEAR_CMD_NEUTRAL);
342407
break;
343408
}
@@ -366,8 +431,8 @@ void ChController::Brake(double pedal) {
366431
// drive with old acceleration
367432
// gas:0.00~99.99 unit:
368433
void ChController::Throttle(double pedal) {
369-
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
370-
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
434+
if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
435+
driving_mode() == Chassis::AUTO_SPEED_ONLY)) {
371436
AINFO << "The current drive mode does not need to set acceleration.";
372437
return;
373438
}
@@ -377,10 +442,10 @@ void ChController::Throttle(double pedal) {
377442

378443
void ChController::Acceleration(double acc) {}
379444

380-
// ch default, -23 ~ 23, left:+, right:-
445+
// ch default, 23 ~ -23, left:+, right:-
381446
// need to be compatible with control module, so reverse
382447
// steering with old angle speed
383-
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
448+
// angle:99.99~0.00~-99.99, unit:, left:+, right:-
384449
void ChController::Steer(double angle) {
385450
if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
386451
driving_mode() == Chassis::AUTO_STEER_ONLY)) {
@@ -394,7 +459,7 @@ void ChController::Steer(double angle) {
394459
}
395460

396461
// steering with new angle speed
397-
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
462+
// angle:-99.99~0.00~99.99, unit:, left:+, right:-
398463
// angle_spd:0.00~99.99, unit:deg/s
399464
void ChController::Steer(double angle, double angle_spd) {
400465
if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
@@ -409,11 +474,73 @@ void ChController::Steer(double angle, double angle_spd) {
409474

410475
void ChController::SetEpbBreak(const ControlCommand& command) {}
411476

412-
void ChController::SetBeam(const ControlCommand& command) {}
477+
void ChController::SetBeam(const ControlCommand& command) {
478+
// Set low beam
479+
if (command.signal().low_beam()) {
480+
turnsignal_command_113_->set_low_beam_cmd(
481+
Turnsignal_command_113::LOW_BEAM_CMD_ON);
482+
} else {
483+
turnsignal_command_113_->set_low_beam_cmd(
484+
Turnsignal_command_113::LOW_BEAM_CMD_OFF);
485+
}
486+
}
413487

414488
void ChController::SetHorn(const ControlCommand& command) {}
415489

416-
void ChController::SetTurningSignal(const ControlCommand& command) {}
490+
void ChController::SetTurningSignal(const ControlCommand& command) {
491+
// Set Turn Signal
492+
auto signal = command.signal().turn_signal();
493+
if (signal == common::VehicleSignal::TURN_LEFT) {
494+
turnsignal_command_113_->set_turn_signal_cmd(
495+
Turnsignal_command_113::TURN_SIGNAL_CMD_LEFT);
496+
} else if (signal == common::VehicleSignal::TURN_RIGHT) {
497+
turnsignal_command_113_->set_turn_signal_cmd(
498+
Turnsignal_command_113::TURN_SIGNAL_CMD_RIGHT);
499+
} else if (signal == common::VehicleSignal::TURN_HAZARD_WARNING) {
500+
turnsignal_command_113_->set_turn_signal_cmd(
501+
Turnsignal_command_113::TURN_SIGNAL_CMD_HAZARD_WARNING_LAMPSTS);
502+
} else {
503+
turnsignal_command_113_->set_turn_signal_cmd(
504+
Turnsignal_command_113::TURN_SIGNAL_CMD_NONE);
505+
}
506+
}
507+
508+
bool ChController::VerifyID() {
509+
if (!CheckVin()) {
510+
AERROR << "Failed to get the vin. Get vin again.";
511+
GetVin();
512+
return false;
513+
} else {
514+
ResetVin();
515+
return true;
516+
}
517+
}
518+
519+
bool ChController::CheckVin() {
520+
if (chassis_.vehicle_id().vin().size() >= 7) {
521+
AINFO << "Vin check success! Vehicel vin is "
522+
<< chassis_.vehicle_id().vin();
523+
return true;
524+
} else {
525+
AINFO << "Vin check failed! Current vin size is "
526+
<< chassis_.vehicle_id().vin().size();
527+
return false;
528+
}
529+
}
530+
531+
void ChController::GetVin() {
532+
vehicle_mode_command_116_->set_vin_req_cmd(
533+
Vehicle_mode_command_116::VIN_REQ_CMD_VIN_REQ_ENABLE);
534+
AINFO << "Get vin";
535+
can_sender_->Update();
536+
}
537+
538+
void ChController::ResetVin() {
539+
vehicle_mode_command_116_->set_vin_req_cmd(
540+
Vehicle_mode_command_116::VIN_REQ_CMD_VIN_REQ_DISABLE);
541+
AINFO << "Reset vin";
542+
can_sender_->Update();
543+
}
417544

418545
void ChController::ResetProtocol() { message_manager_->ResetSendMessages(); }
419546

@@ -432,21 +559,24 @@ bool ChController::CheckChassisError() {
432559
ch.steer_status__512().steer_err()) {
433560
return true;
434561
}
435-
if (Steer_status__512::SENSOR_ERR_STEER_SENSOR_ERR ==
436-
ch.steer_status__512().sensor_err()) {
437-
return true;
438-
}
562+
// cancel the sensor err check because of discarding the steer sensor
563+
// if (Steer_status__512::SENSOR_ERR_STEER_SENSOR_ERR ==
564+
// ch.steer_status__512().sensor_err()) {
565+
// return false;
566+
// }
439567
}
440568
// drive error
441569
if (ch.has_throttle_status__510()) {
442570
if (Throttle_status__510::DRIVE_MOTOR_ERR_DRV_MOTOR_ERR ==
443571
ch.throttle_status__510().drive_motor_err()) {
444572
return true;
445573
}
446-
if (Throttle_status__510::BATTERY_BMS_ERR_BATTERY_ERR ==
447-
ch.throttle_status__510().battery_bms_err()) {
448-
return true;
449-
}
574+
// cancel the battery err check bacause of always causing this error block
575+
// the vehicle use
576+
// if (Throttle_status__510::BATTERY_BMS_ERR_BATTERY_ERR ==
577+
// ch.throttle_status__510().battery_bms_err()) {
578+
// return false;
579+
// }
450580
}
451581
// brake error
452582
if (ch.has_brake_status__511()) {
@@ -486,6 +616,7 @@ void ChController::SecurityDogThreadFunc() {
486616
++horizontal_ctrl_fail;
487617
if (horizontal_ctrl_fail >= kMaxFailAttempt) {
488618
emergency_mode = true;
619+
AINFO << "Driving_mode is into emergency by steer manual intervention";
489620
set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
490621
}
491622
} else {
@@ -499,6 +630,7 @@ void ChController::SecurityDogThreadFunc() {
499630
++vertical_ctrl_fail;
500631
if (vertical_ctrl_fail >= kMaxFailAttempt) {
501632
emergency_mode = true;
633+
AINFO << "Driving_mode is into emergency by speed manual intervention";
502634
set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
503635
}
504636
} else {
@@ -512,6 +644,7 @@ void ChController::SecurityDogThreadFunc() {
512644
if (emergency_mode && mode != Chassis::EMERGENCY_MODE) {
513645
set_driving_mode(Chassis::EMERGENCY_MODE);
514646
message_manager_->ResetSendMessages();
647+
can_sender_->Update();
515648
}
516649
end = ::apollo::cyber::Time::Now().ToMicrosecond();
517650
std::chrono::duration<double, std::micro> elapsed{end - start};
@@ -523,6 +656,7 @@ void ChController::SecurityDogThreadFunc() {
523656
}
524657
}
525658
}
659+
526660
bool ChController::CheckResponse(const int32_t flags, bool need_wait) {
527661
int32_t retry_num = 20;
528662
ChassisDetail chassis_detail;

0 commit comments

Comments
 (0)