From bbad8a7f17a85a2402f4ff628ec39997ee115dba Mon Sep 17 00:00:00 2001 From: Dkit-lite Date: Thu, 15 Aug 2024 16:01:07 +0800 Subject: [PATCH] canbus: update recent canbus changes --- modules/canbus/canbus_component.cc | 219 ++++- modules/canbus/canbus_component.h | 14 +- modules/canbus/common/canbus_gflags.cc | 22 +- modules/canbus/common/canbus_gflags.h | 14 + modules/canbus/conf/canbus.conf | 7 +- modules/canbus/conf/canbus_conf.pb.txt | 1 - .../vehicle/abstract_vehicle_factory.cc | 12 + .../canbus/vehicle/abstract_vehicle_factory.h | 26 +- modules/canbus/vehicle/vehicle_controller.h | 191 ++++- modules/canbus_vehicle/ch/ch_controller.cc | 90 +- modules/canbus_vehicle/ch/ch_controller.h | 9 - .../canbus_vehicle/ch/ch_controller_test.cc | 10 +- .../canbus_vehicle/ch/ch_vehicle_factory.cc | 22 +- .../canbus_vehicle/ch/ch_vehicle_factory.h | 17 +- modules/canbus_vehicle/demo/BUILD | 92 +++ modules/canbus_vehicle/demo/cyberfile.xml | 24 + .../canbus_vehicle/demo/demo_controller.cc | 782 ++++++++++++++++++ modules/canbus_vehicle/demo/demo_controller.h | 154 ++++ .../demo/demo_message_manager.cc | 78 ++ .../demo/demo_message_manager.h | 42 + .../demo/demo_vehicle_factory.cc | 208 +++++ .../demo/demo_vehicle_factory.h | 164 ++++ modules/canbus_vehicle/demo/proto/BUILD | 12 + modules/canbus_vehicle/demo/proto/demo.proto | 496 +++++++++++ .../demo/protocol/bms_report_512.cc | 139 ++++ .../demo/protocol/bms_report_512.h | 80 ++ .../demo/protocol/brake_command_101.cc | 267 ++++++ .../demo/protocol/brake_command_101.h | 144 ++++ .../demo/protocol/brake_report_501.cc | 111 +++ .../demo/protocol/brake_report_501.h | 73 ++ .../demo/protocol/gear_command_103.cc | 184 +++++ .../demo/protocol/gear_command_103.h | 119 +++ .../demo/protocol/gear_report_503.cc | 71 ++ .../demo/protocol/gear_report_503.h | 56 ++ .../demo/protocol/park_command_104.cc | 182 ++++ .../demo/protocol/park_command_104.h | 117 +++ .../demo/protocol/park_report_504.cc | 70 ++ .../demo/protocol/park_report_504.h | 55 ++ .../demo/protocol/steering_command_102.cc | 228 +++++ .../demo/protocol/steering_command_102.h | 128 +++ .../demo/protocol/steering_report_502.cc | 146 ++++ .../demo/protocol/steering_report_502.h | 86 ++ .../demo/protocol/throttle_command_100.cc | 279 +++++++ .../demo/protocol/throttle_command_100.h | 140 ++++ .../demo/protocol/throttle_report_500.cc | 112 +++ .../demo/protocol/throttle_report_500.h | 73 ++ .../demo/protocol/ultr_sensor_1_507.cc | 114 +++ .../demo/protocol/ultr_sensor_1_507.h | 63 ++ .../demo/protocol/ultr_sensor_2_508.cc | 114 +++ .../demo/protocol/ultr_sensor_2_508.h | 67 ++ .../demo/protocol/ultr_sensor_3_509.cc | 114 +++ .../demo/protocol/ultr_sensor_3_509.h | 63 ++ .../demo/protocol/ultr_sensor_4_510.cc | 114 +++ .../demo/protocol/ultr_sensor_4_510.h | 67 ++ .../demo/protocol/ultr_sensor_5_511.cc | 114 +++ .../demo/protocol/ultr_sensor_5_511.h | 63 ++ .../demo/protocol/vcu_report_505.cc | 248 ++++++ .../demo/protocol/vcu_report_505.h | 133 +++ .../demo/protocol/vehicle_mode_command_105.cc | 260 ++++++ .../demo/protocol/vehicle_mode_command_105.h | 162 ++++ .../demo/protocol/vin_resp1_514.cc | 134 +++ .../demo/protocol/vin_resp1_514.h | 79 ++ .../demo/protocol/vin_resp2_515.cc | 134 +++ .../demo/protocol/vin_resp2_515.h | 79 ++ .../demo/protocol/vin_resp3_516.cc | 50 ++ .../demo/protocol/vin_resp3_516.h | 44 + .../demo/protocol/wheelspeed_report_506.cc | 110 +++ .../demo/protocol/wheelspeed_report_506.h | 59 ++ .../devkit/devkit_controller.cc | 120 +-- .../canbus_vehicle/devkit/devkit_controller.h | 8 +- .../devkit/devkit_controller_test.cc | 10 +- .../devkit/devkit_vehicle_factory.cc | 40 +- .../devkit/devkit_vehicle_factory.h | 43 +- modules/canbus_vehicle/ge3/ge3_controller.cc | 7 - modules/canbus_vehicle/ge3/ge3_controller.h | 1 - .../canbus_vehicle/ge3/ge3_controller_test.cc | 12 +- .../canbus_vehicle/ge3/ge3_vehicle_factory.cc | 8 +- .../canbus_vehicle/ge3/ge3_vehicle_factory.h | 8 +- modules/canbus_vehicle/gem/gem_controller.cc | 7 - modules/canbus_vehicle/gem/gem_controller.h | 1 - .../canbus_vehicle/gem/gem_controller_test.cc | 10 +- .../canbus_vehicle/gem/gem_vehicle_factory.cc | 9 +- .../canbus_vehicle/gem/gem_vehicle_factory.h | 8 +- .../canbus_vehicle/lexus/lexus_controller.cc | 7 - .../canbus_vehicle/lexus/lexus_controller.h | 1 - .../lexus/lexus_controller_test.cc | 13 +- .../lexus/lexus_vehicle_factory.cc | 8 +- .../lexus/lexus_vehicle_factory.h | 9 +- .../lincoln/lincoln_controller.cc | 7 - .../lincoln/lincoln_controller.h | 10 +- .../lincoln/lincoln_controller_test.cc | 13 +- .../lincoln/lincoln_vehicle_factory.cc | 7 +- .../lincoln/lincoln_vehicle_factory.h | 9 +- .../neolix_edu/neolix_edu_controller.cc | 98 +-- .../neolix_edu/neolix_edu_controller.h | 15 +- .../neolix_edu/neolix_edu_controller_test.cc | 10 +- .../neolix_edu/neolix_edu_vehicle_factory.cc | 45 +- .../neolix_edu/neolix_edu_vehicle_factory.h | 43 +- .../protocol/ads_brake_command_46.cc | 28 +- .../protocol/ads_brake_command_46.h | 5 + .../protocol/ads_drive_command_50.cc | 35 +- .../protocol/ads_drive_command_50.h | 6 + .../neolix_edu/protocol/ads_eps_command_56.cc | 22 +- .../neolix_edu/protocol/ads_eps_command_56.h | 4 + .../transit/transit_controller.cc | 7 - .../transit/transit_controller.h | 1 - .../transit/transit_controller_test.cc | 13 +- .../transit/transit_vehicle_factory.cc | 7 +- .../transit/transit_vehicle_factory.h | 9 +- modules/canbus_vehicle/wey/wey_controller.cc | 7 - modules/canbus_vehicle/wey/wey_controller.h | 1 - .../canbus_vehicle/wey/wey_controller_test.cc | 13 +- .../canbus_vehicle/wey/wey_vehicle_factory.cc | 8 +- .../canbus_vehicle/wey/wey_vehicle_factory.h | 8 +- .../zhongyun/zhongyun_controller.cc | 7 - .../zhongyun/zhongyun_controller.h | 1 - .../zhongyun/zhongyun_controller_test.cc | 14 +- .../zhongyun/zhongyun_vehicle_factory.cc | 7 +- .../zhongyun/zhongyun_vehicle_factory.h | 9 +- modules/common/adapters/adapter_gflags.cc | 3 + modules/common/adapters/adapter_gflags.h | 1 + modules/drivers/canbus/BUILD | 9 +- .../drivers/canbus/can_comm/can_receiver.h | 3 +- modules/drivers/canbus/can_comm/can_sender.h | 20 +- .../drivers/canbus/can_comm/message_manager.h | 147 +++- .../gen_vehicle_protocol/apollo_demo.dbc | 438 ++++++++++ .../template/canbus_aem.conf.tpl | 7 +- .../template/canbus_conf.pb.tpl | 2 +- .../template/canbus_source.conf.tpl | 7 +- .../template/control_protocol.cc.tpl | 4 +- .../template/controller.cc.tpl | 89 +- .../template/controller.h.tpl | 10 +- .../template/controllerdemo.cc.tpl | 114 +-- .../template/vehicle_factory.cc.tpl | 39 +- .../template/vehicle_factory.h.tpl | 40 +- 135 files changed, 8913 insertions(+), 770 deletions(-) create mode 100644 modules/canbus_vehicle/demo/BUILD create mode 100644 modules/canbus_vehicle/demo/cyberfile.xml create mode 100644 modules/canbus_vehicle/demo/demo_controller.cc create mode 100644 modules/canbus_vehicle/demo/demo_controller.h create mode 100644 modules/canbus_vehicle/demo/demo_message_manager.cc create mode 100644 modules/canbus_vehicle/demo/demo_message_manager.h create mode 100644 modules/canbus_vehicle/demo/demo_vehicle_factory.cc create mode 100644 modules/canbus_vehicle/demo/demo_vehicle_factory.h create mode 100644 modules/canbus_vehicle/demo/proto/BUILD create mode 100644 modules/canbus_vehicle/demo/proto/demo.proto create mode 100644 modules/canbus_vehicle/demo/protocol/bms_report_512.cc create mode 100644 modules/canbus_vehicle/demo/protocol/bms_report_512.h create mode 100644 modules/canbus_vehicle/demo/protocol/brake_command_101.cc create mode 100644 modules/canbus_vehicle/demo/protocol/brake_command_101.h create mode 100644 modules/canbus_vehicle/demo/protocol/brake_report_501.cc create mode 100644 modules/canbus_vehicle/demo/protocol/brake_report_501.h create mode 100644 modules/canbus_vehicle/demo/protocol/gear_command_103.cc create mode 100644 modules/canbus_vehicle/demo/protocol/gear_command_103.h create mode 100644 modules/canbus_vehicle/demo/protocol/gear_report_503.cc create mode 100644 modules/canbus_vehicle/demo/protocol/gear_report_503.h create mode 100644 modules/canbus_vehicle/demo/protocol/park_command_104.cc create mode 100644 modules/canbus_vehicle/demo/protocol/park_command_104.h create mode 100644 modules/canbus_vehicle/demo/protocol/park_report_504.cc create mode 100644 modules/canbus_vehicle/demo/protocol/park_report_504.h create mode 100644 modules/canbus_vehicle/demo/protocol/steering_command_102.cc create mode 100644 modules/canbus_vehicle/demo/protocol/steering_command_102.h create mode 100644 modules/canbus_vehicle/demo/protocol/steering_report_502.cc create mode 100644 modules/canbus_vehicle/demo/protocol/steering_report_502.h create mode 100644 modules/canbus_vehicle/demo/protocol/throttle_command_100.cc create mode 100644 modules/canbus_vehicle/demo/protocol/throttle_command_100.h create mode 100644 modules/canbus_vehicle/demo/protocol/throttle_report_500.cc create mode 100644 modules/canbus_vehicle/demo/protocol/throttle_report_500.h create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.cc create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.cc create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.cc create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.cc create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.cc create mode 100644 modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h create mode 100644 modules/canbus_vehicle/demo/protocol/vcu_report_505.cc create mode 100644 modules/canbus_vehicle/demo/protocol/vcu_report_505.h create mode 100644 modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.cc create mode 100644 modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp1_514.cc create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp1_514.h create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp2_515.cc create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp2_515.h create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp3_516.cc create mode 100644 modules/canbus_vehicle/demo/protocol/vin_resp3_516.h create mode 100644 modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.cc create mode 100644 modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h create mode 100644 modules/tools/gen_vehicle_protocol/apollo_demo.dbc diff --git a/modules/canbus/canbus_component.cc b/modules/canbus/canbus_component.cc index a6665c14872..4871f851564 100644 --- a/modules/canbus/canbus_component.cc +++ b/modules/canbus/canbus_component.cc @@ -88,30 +88,47 @@ bool CanbusComponent::Init() { chassis_cmd_reader_config.pending_queue_size = FLAGS_control_cmd_pending_queue_size; + // init cmd reader if (FLAGS_receive_guardian) { guardian_cmd_reader_ = node_->CreateReader( guardian_cmd_reader_config, [this](const std::shared_ptr &cmd) { ADEBUG << "Received guardian data: run canbus callback."; + const auto start_time = Time::Now().ToMicrosecond(); OnGuardianCommand(*cmd); + const auto end_time = Time::Now().ToMicrosecond(); + if ((end_time - start_time) * 1e-6 > FLAGS_guardian_period) { + AWARN << "Guardian callback time: " + << (end_time - start_time) * 1e-3 << " ms."; + } }); } else { control_command_reader_ = node_->CreateReader( control_cmd_reader_config, [this](const std::shared_ptr &cmd) { ADEBUG << "Received control data: run canbus callback."; + const auto start_time = Time::Now().ToMicrosecond(); OnControlCommand(*cmd); - }); - chassis_command_reader_ = node_->CreateReader( - chassis_cmd_reader_config, - [this](const std::shared_ptr &cmd) { - ADEBUG << "Received control data: run canbus callback."; - OnChassisCommand(*cmd); + const auto end_time = Time::Now().ToMicrosecond(); + if ((end_time - start_time) * 1e-6 > FLAGS_control_period) { + AWARN << "Control callback time: " << (end_time - start_time) * 1e-3 + << " ms."; + } }); } + // init chassis cmd reader + chassis_command_reader_ = node_->CreateReader( + chassis_cmd_reader_config, + [this](const std::shared_ptr &cmd) { + ADEBUG << "Received control data: run canbus callback."; + OnChassisCommand(*cmd); + }); + + // init chassis writer chassis_writer_ = node_->CreateWriter(FLAGS_chassis_topic); + // start canbus vehicle if (!vehicle_object_->Start()) { AERROR << "Fail to start canclient, cansender, canreceiver, canclient, " "vehicle controller."; @@ -138,54 +155,155 @@ void CanbusComponent::PublishChassis() { ADEBUG << chassis.ShortDebugString(); } -void CanbusComponent::CheckChassisCommunication() { +bool CanbusComponent::Proc() { + const auto start_time = Time::Now().ToMicrosecond(); + + if (FLAGS_receive_guardian) { + guardian_cmd_reader_->Observe(); + const auto &guardian_cmd_msg = guardian_cmd_reader_->GetLatestObserved(); + if (guardian_cmd_msg == nullptr) { + AERROR << "guardian cmd msg is not ready!"; + } else { + OnGuardianCommandCheck(*guardian_cmd_msg); + } + } else { + control_command_reader_->Observe(); + const auto &control_cmd_msg = control_command_reader_->GetLatestObserved(); + if (control_cmd_msg == nullptr) { + AERROR << "control cmd msg is not ready!"; + } else { + OnControlCommandCheck(*control_cmd_msg); + } + } + + // check can receiver msg lost if (vehicle_object_->CheckChassisCommunicationFault()) { AERROR << "Can not get the chassis info, please check the chassis " "communication!"; - is_chassis_communicate_lost_ = true; - } else { - is_chassis_communicate_lost_ = false; } -} -bool CanbusComponent::Proc() { - CheckChassisCommunication(); + // publish "/apollo/canbus/chassis" PublishChassis(); - if (!is_chassis_communicate_lost_) { - if (FLAGS_enable_chassis_detail_pub) { - vehicle_object_->PublishChassisDetail(); - } + + // publish "/apollo/canbus/chassis_detail" + if (FLAGS_enable_chassis_detail_pub) { + vehicle_object_->PublishChassisDetail(); } + + // publish "/apollo/canbus/chassis_detail_sender" + if (FLAGS_enable_chassis_detail_sender_pub) { + vehicle_object_->PublishChassisDetailSender(); + } + + // update heartbeat in can sender vehicle_object_->UpdateHeartbeat(); + + const auto end_time = Time::Now().ToMicrosecond(); + const double time_diff_ms = (end_time - start_time) * 1e-3; + if (time_diff_ms > (1 / FLAGS_chassis_freq * 1e3)) { + AWARN << "CanbusComponent::Proc() takes too much time: " << time_diff_ms + << " ms"; + } + return true; } void CanbusComponent::OnControlCommand(const ControlCommand &control_command) { - int64_t current_timestamp = Time::Now().ToMicrosecond(); + // us : microsecord = 1e-3 millisecond = 1e-6 second + double current_timestamp = Time::Now().ToMicrosecond(); // if command coming too soon, just ignore it. + // us < 5 ms(millisecond) *1000 (=5000us microsecord) if (current_timestamp - last_timestamp_controlcmd_ < FLAGS_min_cmd_interval * 1000) { - ADEBUG << "Control command comes too soon. Ignore.\n Required " + ADEBUG << "Control command comes too soon. Ignore. Required " "FLAGS_min_cmd_interval[" - << FLAGS_min_cmd_interval << "], actual time interval[" - << current_timestamp - last_timestamp_controlcmd_ << "]."; + << FLAGS_min_cmd_interval << "] ms, actual time interval[" + << (current_timestamp - last_timestamp_controlcmd_) * 1e-3 + << "] ms."; return; } - last_timestamp_controlcmd_ = current_timestamp; - ADEBUG << "Control_sequence_number:" - << control_command.header().sequence_num() << ", Time_of_delay:" - << current_timestamp - - static_cast(control_command.header().timestamp_sec() * - 1e6) - << " micro seconds"; - vehicle_object_->UpdateCommand(&control_command); + if (!is_control_cmd_time_delay_) { + vehicle_object_->UpdateCommand(&control_command); + } +} + +void CanbusComponent::OnControlCommandCheck( + const ControlCommand &control_command) { + // us : microsecord = 1e-3 millisecond = 1e-6 second + double current_timestamp = Time::Now().ToMicrosecond(); + // cmd_time_diff: s + double cmd_time_diff = + current_timestamp * 1e-6 - control_command.header().timestamp_sec(); + if (FLAGS_use_control_cmd_check && + (cmd_time_diff > (FLAGS_max_control_miss_num * FLAGS_control_period))) { + AERROR << "Control cmd timeout, sequence_number:" + << control_command.header().sequence_num() + << ", Time_of_delay:" << cmd_time_diff << " s" + << ", time delay threshold: " + << (FLAGS_max_control_miss_num * FLAGS_control_period) << " s"; + + if (vehicle_object_->Driving_Mode() == Chassis::COMPLETE_AUTO_DRIVE || + vehicle_object_->Driving_Mode() == Chassis::AUTO_STEER_ONLY || + vehicle_object_->Driving_Mode() == Chassis::AUTO_SPEED_ONLY) { + is_control_cmd_time_delay_ = true; + GuardianCommand new_guardian_command; + new_guardian_command.mutable_control_command()->CopyFrom(control_command); + ProcessGuardianCmdTimeout(&new_guardian_command); + ADEBUG << "new_guardian_command is " + << new_guardian_command.ShortDebugString(); + vehicle_object_->UpdateCommand(&new_guardian_command.control_command()); + } + } else { + is_control_cmd_time_delay_ = false; + } +} + +void CanbusComponent::OnGuardianCommand( + const GuardianCommand &guardian_command) { + if (!is_control_cmd_time_delay_) { + OnControlCommand(guardian_command.control_command()); + } +} + +void CanbusComponent::OnGuardianCommandCheck( + const GuardianCommand &guardian_command) { + // us : microsecord = 1e-3 millisecond = 1e-6 second + double current_timestamp = Time::Now().ToMicrosecond(); + // cmd_time_diff: s + double guardian_cmd_time_diff = + current_timestamp * 1e-6 - guardian_command.header().timestamp_sec(); + if (FLAGS_use_guardian_cmd_check && + (guardian_cmd_time_diff > + (FLAGS_max_guardian_miss_num * FLAGS_guardian_period))) { + AERROR << "Guardain cmd timeout, sequence_number:" + << guardian_command.header().sequence_num() + << ", Time_of_delay:" << guardian_cmd_time_diff << " s" + << ", time delay threshold: " + << (FLAGS_max_guardian_miss_num * FLAGS_guardian_period) << " s"; + + if (vehicle_object_->Driving_Mode() == Chassis::COMPLETE_AUTO_DRIVE || + vehicle_object_->Driving_Mode() == Chassis::AUTO_STEER_ONLY || + vehicle_object_->Driving_Mode() == Chassis::AUTO_SPEED_ONLY) { + is_control_cmd_time_delay_ = true; + GuardianCommand new_guardian_command; + new_guardian_command.CopyFrom(guardian_command); + ProcessGuardianCmdTimeout(&new_guardian_command); + ADEBUG << "new_guardian_command is " + << new_guardian_command.ShortDebugString(); + vehicle_object_->UpdateCommand(&new_guardian_command.control_command()); + } + } else { + is_control_cmd_time_delay_ = false; + } } void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) { + // us : microsecord = 1e-3 millisecond = 1e-6 second int64_t current_timestamp = Time::Now().ToMicrosecond(); // if command coming too soon, just ignore it. + // us < 5 ms(millisecond) *1000 (=5000us microsecord) if (current_timestamp - last_timestamp_chassiscmd_ < FLAGS_min_cmd_interval * 1000) { ADEBUG << "Control command comes too soon. Ignore.\n Required " @@ -194,8 +312,8 @@ void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) { << current_timestamp - last_timestamp_chassiscmd_ << "]."; return; } - last_timestamp_chassiscmd_ = current_timestamp; + ADEBUG << "Control_sequence_number:" << chassis_command.header().sequence_num() << ", Time_of_delay:" << current_timestamp - @@ -206,15 +324,46 @@ void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) { vehicle_object_->UpdateCommand(&chassis_command); } -void CanbusComponent::OnGuardianCommand( - const GuardianCommand &guardian_command) { - OnControlCommand(guardian_command.control_command()); -} - common::Status CanbusComponent::OnError(const std::string &error_msg) { monitor_logger_buffer_.ERROR(error_msg); return ::apollo::common::Status(ErrorCode::CANBUS_ERROR, error_msg); } +void CanbusComponent::ProcessTimeoutByClearCanSender() { + if (vehicle_object_->Driving_Mode() != Chassis::COMPLETE_AUTO_DRIVE && + vehicle_object_->Driving_Mode() != Chassis::AUTO_STEER_ONLY && + vehicle_object_->Driving_Mode() != Chassis::AUTO_SPEED_ONLY && + !FLAGS_chassis_debug_mode) { + ADEBUG << "The current driving mode does not need to check cmd timeout."; + if (vehicle_object_->IsSendProtocolClear()) { + AINFO << "send protocol is clear, ignore driving mode, need to recover " + "send protol."; + vehicle_object_->AddSendProtocol(); + } + return; + } + + if (!is_control_cmd_time_delay_previous_ && is_control_cmd_time_delay_) { + AINFO << "control cmd time latency delay, clear send protocol."; + vehicle_object_->ClearSendProtocol(); + } else if (is_control_cmd_time_delay_previous_ && + !is_control_cmd_time_delay_) { + AINFO << "control cmd time latency reover, add send protocol."; + if (vehicle_object_->IsSendProtocolClear()) { + vehicle_object_->AddSendProtocol(); + } + } + is_control_cmd_time_delay_previous_ = is_control_cmd_time_delay_; +} + +void CanbusComponent::ProcessGuardianCmdTimeout( + GuardianCommand *guardian_command) { + AINFO << "Into cmd timeout process, set estop."; + guardian_command->mutable_control_command()->set_throttle(0.0); + guardian_command->mutable_control_command()->set_steering_target(0.0); + guardian_command->mutable_control_command()->set_steering_rate(25.0); + guardian_command->mutable_control_command()->set_brake(FLAGS_estop_brake); +} + } // namespace canbus } // namespace apollo diff --git a/modules/canbus/canbus_component.h b/modules/canbus/canbus_component.h index b8956a3cf85..b0de32e3cfd 100644 --- a/modules/canbus/canbus_component.h +++ b/modules/canbus/canbus_component.h @@ -82,13 +82,18 @@ class CanbusComponent final : public apollo::cyber::TimerComponent { void PublishChassis(); void OnControlCommand(const apollo::control::ControlCommand &control_command); + void OnControlCommandCheck( + const apollo::control::ControlCommand &control_command); void OnChassisCommand( const apollo::external_command::ChassisCommand &chassis_command); void OnGuardianCommand( const apollo::guardian::GuardianCommand &guardian_command); + void OnGuardianCommandCheck( + const apollo::guardian::GuardianCommand &guardian_command); apollo::common::Status OnError(const std::string &error_msg); - void RegisterCanClients(); - void CheckChassisCommunication(); + void ProcessTimeoutByClearCanSender(); + void ProcessGuardianCmdTimeout( + apollo::guardian::GuardianCommand *guardian_command); CanbusConf canbus_conf_; std::shared_ptr<::apollo::canbus::AbstractVehicleFactory> vehicle_object_ = @@ -99,11 +104,12 @@ class CanbusComponent final : public apollo::cyber::TimerComponent { control_command_reader_; std::shared_ptr> chassis_command_reader_; - int64_t last_timestamp_controlcmd_ = 0; + double last_timestamp_controlcmd_ = 0.0; int64_t last_timestamp_chassiscmd_ = 0; ::apollo::common::monitor::MonitorLogBuffer monitor_logger_buffer_; std::shared_ptr> chassis_writer_; - bool is_chassis_communicate_lost_ = false; + bool is_control_cmd_time_delay_ = false; + bool is_control_cmd_time_delay_previous_ = false; }; CYBER_REGISTER_COMPONENT(CanbusComponent) diff --git a/modules/canbus/common/canbus_gflags.cc b/modules/canbus/common/canbus_gflags.cc index 7e74fb9b911..6fb3dab4d9e 100644 --- a/modules/canbus/common/canbus_gflags.cc +++ b/modules/canbus/common/canbus_gflags.cc @@ -27,10 +27,24 @@ DEFINE_string(canbus_conf_file, // Canbus gflags DEFINE_double(chassis_freq, 100, "Chassis feedback timer frequency."); + +// cmd input check DEFINE_int64(min_cmd_interval, 5, "Minimum control command interval in ms."); +DEFINE_int64(pad_msg_delay_interval, 3, + "Minimum pad msg command delay interval in s."); +DEFINE_int32(max_control_miss_num, 5, "max control miss num."); +DEFINE_double(control_period, 0.01, "control period in s."); +DEFINE_int32(max_guardian_miss_num, 5, "max guardian miss num."); +DEFINE_double(guardian_period, 0.01, "control period in s."); +DEFINE_bool(use_control_cmd_check, false, "enable control cmd input check."); +DEFINE_bool(use_guardian_cmd_check, false, "nable guardian cmd input check."); +DEFINE_double(estop_brake, 30.0, "brake action when cmd input check error."); // chassis_detail message publish -DEFINE_bool(enable_chassis_detail_pub, false, "Chassis Detail message publish"); +DEFINE_bool(enable_chassis_detail_pub, true, + "Chassis Detail receive message publish"); +DEFINE_bool(enable_chassis_detail_sender_pub, true, + "Chassis Detail sender message publish"); // canbus test files DEFINE_string(canbus_test_file, @@ -48,8 +62,12 @@ DEFINE_int32(control_cmd_pending_queue_size, 10, "Max control cmd pending queue size"); DEFINE_int32(chassis_cmd_pending_queue_size, 10, "Max control cmd pending queue size"); + // enable forward Ultrasonic AEB -DEFINE_bool(enable_aeb, true, "Enable forward Ultrasonic AEB"); +DEFINE_bool(enable_aeb, false, "Enable forward Ultrasonic AEB"); + +// enabel chassis debug mode for such as ignore pad msg timestamp check +DEFINE_bool(chassis_debug_mode, false, "Enable chassis in debug mode"); // vehicle factory dynamic library path and class name DEFINE_string(load_vehicle_library, diff --git a/modules/canbus/common/canbus_gflags.h b/modules/canbus/common/canbus_gflags.h index 573b25ea919..40e1324d80f 100644 --- a/modules/canbus/common/canbus_gflags.h +++ b/modules/canbus/common/canbus_gflags.h @@ -27,10 +27,21 @@ DECLARE_string(canbus_conf_file); // Canbus gflags DECLARE_double(chassis_freq); + +// cmd input check DECLARE_int64(min_cmd_interval); +DECLARE_int64(pad_msg_delay_interval); +DECLARE_int32(max_control_miss_num); +DECLARE_double(control_period); +DECLARE_int32(max_guardian_miss_num); +DECLARE_double(guardian_period); +DECLARE_bool(use_control_cmd_check); +DECLARE_bool(use_guardian_cmd_check); +DECLARE_double(estop_brake); // chassis_detail message publish DECLARE_bool(enable_chassis_detail_pub); +DECLARE_bool(enable_chassis_detail_sender_pub); // canbus test files DECLARE_string(canbus_test_file); @@ -45,6 +56,9 @@ DECLARE_int32(chassis_cmd_pending_queue_size); // enable forward Ultrasonic AEB DECLARE_bool(enable_aeb); +// enabel chassis debug mode for such as ignore pad msg timestamp check +DECLARE_bool(chassis_debug_mode); + // vehicle factory dynamic library path and class name DECLARE_string(load_vehicle_library); DECLARE_string(load_vehicle_class_name); diff --git a/modules/canbus/conf/canbus.conf b/modules/canbus/conf/canbus.conf index d250d068d2c..5803b206b44 100644 --- a/modules/canbus/conf/canbus.conf +++ b/modules/canbus/conf/canbus.conf @@ -2,5 +2,10 @@ --canbus_conf_file=/apollo/modules/canbus/conf/canbus_conf.pb.txt --load_vehicle_library=/opt/apollo/neo/lib/modules/canbus_vehicle/lincoln/liblincoln_vehicle_factory_lib.so --load_vehicle_class_name=LincolnVehicleFactory ---noenable_chassis_detail_pub +--enable_chassis_detail_pub +--enable_chassis_detail_sender_pub +--chassis_debug_mode=false +--use_control_cmd_check=false +--use_guardian_cmd_check=false --noreceive_guardian +--estop_brake=30.0 \ No newline at end of file diff --git a/modules/canbus/conf/canbus_conf.pb.txt b/modules/canbus/conf/canbus_conf.pb.txt index 671e45fe264..70236f935a8 100644 --- a/modules/canbus/conf/canbus_conf.pb.txt +++ b/modules/canbus/conf/canbus_conf.pb.txt @@ -1,5 +1,4 @@ vehicle_parameter { - brand: LINCOLN_MKZ max_enable_fail_attempt: 5 driving_mode: COMPLETE_AUTO_DRIVE } diff --git a/modules/canbus/vehicle/abstract_vehicle_factory.cc b/modules/canbus/vehicle/abstract_vehicle_factory.cc index b5df304d77b..9a3c4ff1963 100644 --- a/modules/canbus/vehicle/abstract_vehicle_factory.cc +++ b/modules/canbus/vehicle/abstract_vehicle_factory.cc @@ -21,8 +21,20 @@ namespace canbus { void AbstractVehicleFactory::UpdateHeartbeat() {} +void AbstractVehicleFactory::PublishChassisDetailSender() {} + bool AbstractVehicleFactory::CheckChassisCommunicationFault() { return false; } +void AbstractVehicleFactory::AddSendProtocol() {} + +void AbstractVehicleFactory::ClearSendProtocol() {} + +bool AbstractVehicleFactory::IsSendProtocolClear() { return false; } + +Chassis::DrivingMode AbstractVehicleFactory::Driving_Mode() { + return Chassis::COMPLETE_MANUAL; +} + void AbstractVehicleFactory::SetVehicleParameter( const VehicleParameter &vehicle_parameter) { vehicle_parameter_ = vehicle_parameter; diff --git a/modules/canbus/vehicle/abstract_vehicle_factory.h b/modules/canbus/vehicle/abstract_vehicle_factory.h index de9d6388afe..6326923e6b0 100644 --- a/modules/canbus/vehicle/abstract_vehicle_factory.h +++ b/modules/canbus/vehicle/abstract_vehicle_factory.h @@ -24,7 +24,6 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/class_loader/class_loader_register_macro.h" #include "modules/canbus/vehicle/vehicle_controller.h" #include "modules/drivers/canbus/can_comm/message_manager.h" @@ -94,6 +93,11 @@ class AbstractVehicleFactory { */ virtual void PublishChassisDetail() = 0; + /** + * @brief publish chassis for vehicle messages + */ + virtual void PublishChassisDetailSender(); + /** * @brief create cansender heartbeat */ @@ -104,6 +108,26 @@ class AbstractVehicleFactory { */ virtual bool CheckChassisCommunicationFault(); + /** + * @brief add send protocol message + */ + virtual void AddSendProtocol(); + + /** + * @brief clear send protocol message, make a sender can error + */ + virtual void ClearSendProtocol(); + + /** + * @brief check send protocol message whether is clear or not + */ + virtual bool IsSendProtocolClear(); + + /** + * @brief get chassis driving mode + */ + virtual Chassis::DrivingMode Driving_Mode(); + private: VehicleParameter vehicle_parameter_; }; diff --git a/modules/canbus/vehicle/vehicle_controller.h b/modules/canbus/vehicle/vehicle_controller.h index 42631664c19..3f1f31c48f8 100644 --- a/modules/canbus/vehicle/vehicle_controller.h +++ b/modules/canbus/vehicle/vehicle_controller.h @@ -30,6 +30,8 @@ #include "modules/common_msgs/external_command_msgs/chassis_command.pb.h" #include "cyber/common/log.h" +#include "cyber/time/time.h" +#include "modules/canbus/common/canbus_gflags.h" #include "modules/common/configs/vehicle_config_helper.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -43,6 +45,7 @@ namespace apollo { namespace canbus { +using apollo::cyber::Time; using ::apollo::drivers::canbus::CanReceiver; using ::apollo::drivers::canbus::CanSender; using ::apollo::drivers::canbus::MessageManager; @@ -66,7 +69,6 @@ class VehicleController { */ virtual common::ErrorCode Init( const VehicleParameter ¶ms, CanSender *const can_sender, - CanReceiver *const can_receiver, MessageManager *const message_manager) = 0; /** @@ -94,6 +96,7 @@ class VehicleController { virtual common::ErrorCode Update(const control::ControlCommand &command); virtual common::ErrorCode Update( const external_command::ChassisCommand &command); + /** * @brief set vehicle to appointed driving mode. * @param driving mode to be appointed. @@ -102,6 +105,16 @@ class VehicleController { virtual common::ErrorCode SetDrivingMode( const Chassis::DrivingMode &driving_mode); + virtual bool CheckChassisCommunicationError(); + + virtual void AddSendMessage(); + + virtual SensorType GetNewRecvChassisDetail(); + + virtual SensorType GetNewSenderChassisDetail(); + + virtual Chassis::DrivingMode driving_mode(); + private: /* * @brief main logical function for operation the car enter or exit the auto @@ -192,7 +205,6 @@ class VehicleController { virtual bool VerifyID() = 0; protected: - virtual Chassis::DrivingMode driving_mode(); virtual void set_driving_mode(const Chassis::DrivingMode &driving_mode); protected: @@ -207,12 +219,119 @@ class VehicleController { Chassis::DrivingMode driving_mode_ = Chassis::COMPLETE_MANUAL; bool is_reset_ = false; // reset command from control command std::mutex mode_mutex_; // only use in this base class + uint32_t lost_chassis_reveive_detail_count_ = 0; // check chassis detail lost + bool is_need_count_ = true; // check chassis detail lost + size_t sender_data_size_previous_ = 0.0; // check apollo sender preiod + int64_t start_time_ = 0; // check apollo sender preiod + bool is_chassis_communication_error_ = false; // check chassis communication }; using common::ErrorCode; using control::ControlCommand; using external_command::ChassisCommand; +template +bool VehicleController::CheckChassisCommunicationError() { + SensorType chassis_detail_sender; + if (message_manager_->GetSensorCheckSenderData(&chassis_detail_sender) != + ErrorCode::OK) { + AERROR_EVERY(100) << "Get " << typeid(SensorType).name() + << " chassis receive detail failed."; + } + size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); + ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; + int64_t end_time = 0; + if ((sender_data_size_previous_ < 2) && (sender_data_size > 2)) { + end_time = ::apollo::cyber::Time::Now().ToMicrosecond(); + ADEBUG << "end_time is " << end_time; + if (start_time_ > 0) { + const double sender_diff = (end_time - start_time_) * 1e-3; + ADEBUG << "sender protocol preiod is " << sender_diff; + } + } else if ((sender_data_size_previous_ > 2) && (sender_data_size < 2)) { + start_time_ = ::apollo::cyber::Time::Now().ToMicrosecond(); + ADEBUG << "start_time_ is " << start_time_; + } + sender_data_size_previous_ = sender_data_size; + message_manager_->ClearSensorCheckSenderData(); + + SensorType chassis_detail_receiver; + if (message_manager_->GetSensorCheckRecvData(&chassis_detail_receiver) != + ErrorCode::OK) { + AERROR_EVERY(100) << "Get " << typeid(SensorType).name() + << " chassis receive detail failed."; + } + ADEBUG << "chassis_detail_receiver is " + << chassis_detail_receiver.ShortDebugString(); + size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); + ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; + // check receiver data is null + if (receiver_data_size < 2) { + if (is_need_count_) { + lost_chassis_reveive_detail_count_++; + } + } else { + lost_chassis_reveive_detail_count_ = 0; + is_need_count_ = true; + } + ADEBUG << "lost_chassis_reveive_detail_count_ is " + << lost_chassis_reveive_detail_count_; + // check receive data lost threshold is (100 * 10)ms + if (lost_chassis_reveive_detail_count_ > 100) { + is_need_count_ = false; + is_chassis_communication_error_ = true; + AERROR << typeid(SensorType).name() + << " chassis detail is lost, please check the " + "communication error."; + message_manager_->ClearSensorCheckRecvData(); + message_manager_->ResetSendMessages(); + return true; + } else { + is_chassis_communication_error_ = false; + } + message_manager_->ClearSensorCheckRecvData(); + + return false; +} + +template +void VehicleController::AddSendMessage() {} + +template +SensorType VehicleController::GetNewRecvChassisDetail() { + SensorType receiver_chassis_detail; + if (message_manager_->GetSensorRecvData(&receiver_chassis_detail) != + ErrorCode::OK) { + AERROR_EVERY(100) << "Get " << typeid(SensorType).name() + << " chassis detail receiver failed."; + return receiver_chassis_detail; + } + ADEBUG << "receiver_chassis_detail is " + << receiver_chassis_detail.ShortDebugString(); + if (is_chassis_communication_error_) { + message_manager_->ClearSensorRecvData(); + } + return receiver_chassis_detail; +} + +template +SensorType VehicleController::GetNewSenderChassisDetail() { + SensorType sender_chassis_detail; + if (message_manager_->GetSensorSenderData(&sender_chassis_detail) != + ErrorCode::OK) { + AERROR_EVERY(100) << "Get " << typeid(SensorType).name() + << " chassis detail receiver failed."; + return sender_chassis_detail; + } + message_manager_->GetSensorSenderData(&sender_chassis_detail); + ADEBUG << "sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + if (can_sender_->IsMessageClear()) { + message_manager_->ClearSensorSenderData(); + } + return sender_chassis_detail; +} + template Chassis::DrivingMode VehicleController::driving_mode() { std::lock_guard lock(mode_mutex_); @@ -291,38 +410,52 @@ ErrorCode VehicleController::Update( return ErrorCode::CANBUS_ERROR; } - // Execute action to transform driving mode if (control_command.has_pad_msg() && control_command.pad_msg().has_action()) { - AINFO << "Canbus received pad msg: " - << control_command.pad_msg().ShortDebugString(); - if (control_command.pad_msg().action() == control::DrivingAction::VIN_REQ) { - if (!VerifyID()) { - AINFO << "Response vid failed, please request again."; - } else { - AINFO << "Response vid success!"; - } - } else { - Chassis::DrivingMode mode = Chassis::COMPLETE_MANUAL; - switch (control_command.pad_msg().action()) { - case control::DrivingAction::START: { - mode = Chassis::COMPLETE_AUTO_DRIVE; - break; + ADEBUG << "Canbus received pad msg: " + << control_command.pad_msg().ShortDebugString(); + const double current_timestamp = Time::Now().ToSecond(); + // pad_msg_time_diff: s + const double pad_msg_time_diff = + current_timestamp - control_command.pad_msg().header().timestamp_sec(); + // Execute action to transform driving mode + if ((FLAGS_chassis_debug_mode || + (pad_msg_time_diff < FLAGS_pad_msg_delay_interval)) && + !is_chassis_communication_error_) { + if (control_command.pad_msg().action() == + control::DrivingAction::VIN_REQ) { + if (!VerifyID()) { + AINFO << "Response vid failed, please request again."; + } else { + AINFO << "Response vid success!"; } - case control::DrivingAction::RESET: { - // In COMPLETE_MANUAL mode - break; + } else { + Chassis::DrivingMode mode = Chassis::COMPLETE_MANUAL; + switch (control_command.pad_msg().action()) { + case control::DrivingAction::START: { + mode = Chassis::COMPLETE_AUTO_DRIVE; + break; + } + case control::DrivingAction::RESET: { + // In COMPLETE_MANUAL mode + AINFO << "Into the Reset action."; + break; + } + default: { + AERROR << "No response for this action."; + break; + } } - default: { - AERROR << "No response for this action."; - break; + auto error_code = SetDrivingMode(mode); + if (error_code != ErrorCode::OK) { + AERROR << "Failed to set driving mode."; + } else { + AINFO << "Set driving mode success."; } } - auto error_code = SetDrivingMode(mode); - if (error_code != ErrorCode::OK) { - AERROR << "Failed to set driving mode."; - } else { - AINFO << "Set driving mode success."; - } + } else { + ADEBUG << "pad msg time out, current time interval is " + << pad_msg_time_diff << " s, threshold is " + << FLAGS_pad_msg_delay_interval << " s"; } } diff --git a/modules/canbus_vehicle/ch/ch_controller.cc b/modules/canbus_vehicle/ch/ch_controller.cc index a8ed33233f7..3413fd2dc80 100644 --- a/modules/canbus_vehicle/ch/ch_controller.cc +++ b/modules/canbus_vehicle/ch/ch_controller.cc @@ -45,7 +45,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode ChController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Ch>* const can_sender, - CanReceiver<::apollo::canbus::Ch>* const can_receiver, MessageManager<::apollo::canbus::Ch>* const message_manager) { if (is_initialized_) { AINFO << "ChController has already been initiated."; @@ -66,12 +65,6 @@ ErrorCode ChController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; @@ -166,10 +159,9 @@ void ChController::Stop() { Chassis ChController::chassis() { chassis_.Clear(); - Ch chassis_detail; - message_manager_->GetSensorData(&chassis_detail); + Ch chassis_detail = GetNewRecvChassisDetail(); - // 21, 22, previously 1, 2 + // 1, 2 // if (driving_mode() == Chassis::EMERGENCY_MODE) { // set_chassis_error_code(Chassis::NO_ERROR); // } @@ -344,11 +336,12 @@ Chassis ChController::chassis() { "Chassis has some fault, please check the chassis_detail."); } + // check the chassis detail lost if (is_chassis_communication_error_) { chassis_.mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::DISALLOW_ENGAGE); chassis_.mutable_engage_advice()->set_reason( - "devkit chassis detail is lost! Please check the communication error."); + "ch chassis detail is lost! Please check the communication error."); set_chassis_error_code(Chassis::CHASSIS_CAN_LOST); set_driving_mode(Chassis::EMERGENCY_MODE); } @@ -584,67 +577,12 @@ void ChController::ResetVin() { void ChController::ResetProtocol() { message_manager_->ResetSendMessages(); } -bool ChController::CheckChassisCommunicationError() { - Ch chassis_detail_receiver; - ADEBUG << "Can receiver finished recv once: " - << can_receiver_->IsFinishRecvOnce(); - if (message_manager_->GetSensorRecvData(&chassis_detail_receiver) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_receiver is " - << chassis_detail_receiver.ShortDebugString(); - size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); - ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; - // check receiver data is null - if (receiver_data_size < 2) { - if (is_need_count_) { - lost_chassis_reveive_detail_count_++; - } - } else { - lost_chassis_reveive_detail_count_ = 0; - is_need_count_ = true; - } - ADEBUG << "lost_chassis_reveive_detail_count_ is " - << lost_chassis_reveive_detail_count_; - // check receive data lost threshold is (100 * 10)ms - if (lost_chassis_reveive_detail_count_ > 100) { - is_need_count_ = false; - is_chassis_communication_error_ = true; - AERROR << "neolix chassis detail is lost, please check the communication " - "error."; - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorData(); - return true; - } else { - is_chassis_communication_error_ = false; - } - - Ch chassis_detail_sender; - if (message_manager_->GetSensorSenderData(&chassis_detail_sender) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_sender is " - << chassis_detail_sender.ShortDebugString(); - size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); - ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; - - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorSenderData(); - return false; -} - bool ChController::CheckChassisError() { if (is_chassis_communication_error_) { - AERROR_EVERY(100) << "ChassisDetail has no ch vehicle info."; + AERROR_EVERY(100) << "ChassisDetail has no devkit vehicle info."; return false; } - Ch chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis detail failed."; - } + Ch chassis_detail = GetNewRecvChassisDetail(); // steer motor fault if (chassis_detail.has_steer_status__512()) { if (Steer_status__512::STEER_ERR_STEER_MOTOR_ERR == @@ -736,6 +674,7 @@ void ChController::SecurityDogThreadFunc() { can_sender_->Update(); } + // recove error code if (!emergency_mode && !is_chassis_communication_error_ && mode == Chassis::EMERGENCY_MODE) { set_chassis_error_code(Chassis::NO_ERROR); @@ -760,10 +699,6 @@ bool ChController::CheckResponse(const int32_t flags, bool need_wait) { bool is_esp_online = 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_.has_check_response() && @@ -793,9 +728,14 @@ bool ChController::CheckResponse(const int32_t flags, bool need_wait) { } } while (need_wait && retry_num); - AINFO << "check_response fail: is_eps_online:" << is_eps_online - << ", is_vcu_online:" << is_vcu_online - << ", is_esp_online:" << is_esp_online; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } return false; } diff --git a/modules/canbus_vehicle/ch/ch_controller.h b/modules/canbus_vehicle/ch/ch_controller.h index 16b8f50f951..708ed459ee2 100644 --- a/modules/canbus_vehicle/ch/ch_controller.h +++ b/modules/canbus_vehicle/ch/ch_controller.h @@ -46,7 +46,6 @@ class ChController final : public VehicleController<::apollo::canbus::Ch> { ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Ch>* const can_sender, - CanReceiver<::apollo::canbus::Ch>* const can_receiver, MessageManager<::apollo::canbus::Ch>* const message_manager) override; bool Start() override; @@ -62,11 +61,6 @@ class ChController final : public VehicleController<::apollo::canbus::Ch> { */ Chassis chassis() override; - /** - * @brief ccheck the chassis detail received or lost. - */ - bool CheckChassisCommunicationError(); - FRIEND_TEST(ChControllerTest, SetDrivingMode); FRIEND_TEST(ChControllerTest, Status); FRIEND_TEST(ChControllerTest, UpdateDrivingMode); @@ -146,9 +140,6 @@ class ChController final : public VehicleController<::apollo::canbus::Ch> { std::mutex chassis_mask_mutex_; int32_t chassis_error_mask_ = 0; - uint32_t lost_chassis_reveive_detail_count_ = 0; - bool is_need_count_ = true; - bool is_chassis_communication_error_ = false; }; } // namespace ch diff --git a/modules/canbus_vehicle/ch/ch_controller_test.cc b/modules/canbus_vehicle/ch/ch_controller_test.cc index 48f2f468e1a..3b711165b02 100644 --- a/modules/canbus_vehicle/ch/ch_controller_test.cc +++ b/modules/canbus_vehicle/ch/ch_controller_test.cc @@ -53,15 +53,13 @@ class ChControllerTest : public ::testing::Test { ControlCommand control_cmd_; VehicleSignal vehicle_signal_; CanSender<::apollo::canbus::Ch> sender_; - CanReceiver<::apollo::canbus::Ch> receiver_; ChMessageManager msg_manager_; CanbusConf canbus_conf_; VehicleParameter params_; }; TEST_F(ChControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -69,14 +67,14 @@ TEST_F(ChControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); EXPECT_EQ(controller_.SetDrivingMode(chassis.driving_mode()), ErrorCode::OK); } TEST_F(ChControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); controller_.SetHorn(control_cmd_.signal()); @@ -86,7 +84,7 @@ TEST_F(ChControllerTest, Status) { } TEST_F(ChControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), ErrorCode::OK); diff --git a/modules/canbus_vehicle/ch/ch_vehicle_factory.cc b/modules/canbus_vehicle/ch/ch_vehicle_factory.cc index 4c64bed66cc..7aeccb02e1f 100644 --- a/modules/canbus_vehicle/ch/ch_vehicle_factory.cc +++ b/modules/canbus_vehicle/ch/ch_vehicle_factory.cc @@ -69,7 +69,6 @@ bool ChVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -84,6 +83,9 @@ bool ChVehicleFactory::Init(const CanbusConf *canbus_conf) { chassis_detail_writer_ = node_->CreateWriter<::apollo::canbus::Ch>(FLAGS_chassis_detail_topic); + chassis_detail_sender_writer_ = node_->CreateWriter<::apollo::canbus::Ch>( + FLAGS_chassis_detail_sender_topic); + return true; } @@ -152,12 +154,18 @@ Chassis ChVehicleFactory::publish_chassis() { } void ChVehicleFactory::PublishChassisDetail() { - Ch chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - ADEBUG << chassis_detail.ShortDebugString(); + Ch chassis_detail = vehicle_controller_->GetNewRecvChassisDetail(); + ADEBUG << "latest chassis_detail is " << chassis_detail.ShortDebugString(); chassis_detail_writer_->Write(chassis_detail); } +void ChVehicleFactory::PublishChassisDetailSender() { + Ch sender_chassis_detail = vehicle_controller_->GetNewSenderChassisDetail(); + ADEBUG << "latest sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + chassis_detail_sender_writer_->Write(sender_chassis_detail); +} + bool ChVehicleFactory::CheckChassisCommunicationFault() { if (vehicle_controller_->CheckChassisCommunicationError()) { return true; @@ -165,8 +173,10 @@ bool ChVehicleFactory::CheckChassisCommunicationFault() { return false; } -std::unique_ptr ChVehicleFactory::CreateVehicleController() { - return std::unique_ptr(new ch::ChController()); +std::unique_ptr> +ChVehicleFactory::CreateVehicleController() { + return std::unique_ptr>( + new ch::ChController()); } std::unique_ptr> diff --git a/modules/canbus_vehicle/ch/ch_vehicle_factory.h b/modules/canbus_vehicle/ch/ch_vehicle_factory.h index b3bdd99c795..99c788405cc 100644 --- a/modules/canbus_vehicle/ch/ch_vehicle_factory.h +++ b/modules/canbus_vehicle/ch/ch_vehicle_factory.h @@ -94,14 +94,23 @@ class ChVehicleFactory : public AbstractVehicleFactory { */ void PublishChassisDetail() override; - bool CheckChassisCommunicationFault(); + /** + * @brief publish chassis for apollo sender messages + */ + void PublishChassisDetailSender() override; + + /** + * @brief check chassis can receiver lost + */ + bool CheckChassisCommunicationFault() override; private: /** * @brief create ch vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create ch message manager @@ -114,10 +123,12 @@ class ChVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Ch> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Ch> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Ch>> chassis_detail_writer_; + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Ch>> + chassis_detail_sender_writer_; }; CYBER_REGISTER_VEHICLEFACTORY(ChVehicleFactory) diff --git a/modules/canbus_vehicle/demo/BUILD b/modules/canbus_vehicle/demo/BUILD new file mode 100644 index 00000000000..600a725051e --- /dev/null +++ b/modules/canbus_vehicle/demo/BUILD @@ -0,0 +1,92 @@ +load("//tools:apollo_package.bzl", "apollo_cc_binary", "apollo_cc_library", "apollo_cc_test", "apollo_component", "apollo_package") +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +CANBUS_COPTS = ["-DMODULE_NAME=\\\"canbus\\\""] + +apollo_cc_library( + name = "apollo_canbus_vehicle_demo", + srcs = [ + "demo_controller.cc", + "demo_message_manager.cc", + "demo_vehicle_factory.cc", + "protocol/brake_command_101.cc", + "protocol/gear_command_103.cc", + "protocol/park_command_104.cc", + "protocol/steering_command_102.cc", + "protocol/throttle_command_100.cc", + "protocol/vehicle_mode_command_105.cc", + "protocol/bms_report_512.cc", + "protocol/brake_report_501.cc", + "protocol/gear_report_503.cc", + "protocol/park_report_504.cc", + "protocol/steering_report_502.cc", + "protocol/throttle_report_500.cc", + "protocol/ultr_sensor_1_507.cc", + "protocol/ultr_sensor_2_508.cc", + "protocol/ultr_sensor_3_509.cc", + "protocol/ultr_sensor_4_510.cc", + "protocol/ultr_sensor_5_511.cc", + "protocol/vcu_report_505.cc", + "protocol/vin_resp1_514.cc", + "protocol/vin_resp2_515.cc", + "protocol/vin_resp3_516.cc", + "protocol/wheelspeed_report_506.cc", + ], + hdrs = [ + "demo_controller.h", + "demo_message_manager.h", + "demo_vehicle_factory.h", + "protocol/brake_command_101.h", + "protocol/gear_command_103.h", + "protocol/park_command_104.h", + "protocol/steering_command_102.h", + "protocol/throttle_command_100.h", + "protocol/vehicle_mode_command_105.h", + "protocol/bms_report_512.h", + "protocol/brake_report_501.h", + "protocol/gear_report_503.h", + "protocol/park_report_504.h", + "protocol/steering_report_502.h", + "protocol/throttle_report_500.h", + "protocol/ultr_sensor_1_507.h", + "protocol/ultr_sensor_2_508.h", + "protocol/ultr_sensor_3_509.h", + "protocol/ultr_sensor_4_510.h", + "protocol/ultr_sensor_5_511.h", + "protocol/vcu_report_505.h", + "protocol/vin_resp1_514.h", + "protocol/vin_resp2_515.h", + "protocol/vin_resp3_516.h", + "protocol/wheelspeed_report_506.h", + ], + copts = CANBUS_COPTS, + deps = [ + "//modules/canbus:apollo_canbus", + "//modules/canbus/proto:canbus_conf_cc_proto", + "//modules/canbus/proto:vehicle_parameter_cc_proto", + "//modules/canbus_vehicle/demo/proto:demo_proto", + "//modules/common/adapters:adapter_gflags", + "//modules/common/status", + "//modules/common_msgs/basic_msgs:error_code_cc_proto", + "//modules/common_msgs/chassis_msgs:chassis_cc_proto", + "//modules/common_msgs/control_msgs:control_cmd_cc_proto", + "//modules/drivers/canbus:apollo_drivers_canbus", + ], +) + +apollo_component( + name = "libdemo_vehicle_factory_lib.so", + deps = [":apollo_canbus_vehicle_demo"], +) + +filegroup( + name = "runtime_data", + srcs = glob([ + "testdata/**", + ]), +) + +apollo_package() +cpplint() diff --git a/modules/canbus_vehicle/demo/cyberfile.xml b/modules/canbus_vehicle/demo/cyberfile.xml new file mode 100644 index 00000000000..8be8c74e071 --- /dev/null +++ b/modules/canbus_vehicle/demo/cyberfile.xml @@ -0,0 +1,24 @@ + + canbus-vehicle-demo + local + + Dynamic loading for canbus demo vehicle module + + + Apollo + Apache License 2.0 + https://www.apollo.auto/ + https://github.com/ApolloAuto/apollo + https://github.com/ApolloAuto/apollo/issues + + module + //modules/canbus_vehicle/demo + + cyber + common + canbus + common-msgs + drivers-canbus + 3rd-gtest + + \ No newline at end of file diff --git a/modules/canbus_vehicle/demo/demo_controller.cc b/modules/canbus_vehicle/demo/demo_controller.cc new file mode 100644 index 00000000000..4224d8dac27 --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_controller.cc @@ -0,0 +1,782 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/demo_controller.h" + +#include + +#include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h" + +#include "cyber/common/log.h" +#include "cyber/time/time.h" +#include "modules/canbus/vehicle/vehicle_controller.h" +#include "modules/canbus_vehicle/demo/demo_message_manager.h" +#include "modules/drivers/canbus/can_comm/can_sender.h" +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { +using ::apollo::common::ErrorCode; +using ::apollo::common::VehicleSignal; +using ::apollo::control::ControlCommand; +using ::apollo::drivers::canbus::ProtocolData; + +namespace { +const int32_t kMaxFailAttempt = 10; +const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1; +const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; + +} // namespace + +void DemoController::AddSendMessage() { + can_sender_->AddMessage(Brakecommand101::ID, brake_command_101_, false); + can_sender_->AddMessage(Gearcommand103::ID, gear_command_103_, false); + can_sender_->AddMessage(Parkcommand104::ID, park_command_104_, false); + can_sender_->AddMessage(Steeringcommand102::ID, steering_command_102_, false); + can_sender_->AddMessage(Throttlecommand100::ID, throttle_command_100_, false); + can_sender_->AddMessage(Vehiclemodecommand105::ID, vehicle_mode_command_105_, + false); +} + +ErrorCode DemoController::Init( + const VehicleParameter& params, + CanSender<::apollo::canbus::Demo>* const can_sender, + MessageManager<::apollo::canbus::Demo>* const message_manager) { + if (is_initialized_) { + AINFO << "DemoController has already been initiated."; + return ErrorCode::CANBUS_ERROR; + } + + vehicle_params_.CopyFrom( + common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param()); + params_.CopyFrom(params); + if (!params_.has_driving_mode()) { + AERROR << "Vehicle conf pb not set driving_mode."; + return ErrorCode::CANBUS_ERROR; + } + + if (can_sender == nullptr) { + AERROR << "Canbus sender is null."; + return ErrorCode::CANBUS_ERROR; + } + can_sender_ = can_sender; + + if (message_manager == nullptr) { + AERROR << "protocol manager is null."; + return ErrorCode::CANBUS_ERROR; + } + message_manager_ = message_manager; + + // sender part + brake_command_101_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Brakecommand101::ID)); + if (brake_command_101_ == nullptr) { + AERROR << "Brakecommand101 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + gear_command_103_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Gearcommand103::ID)); + if (gear_command_103_ == nullptr) { + AERROR << "Gearcommand103 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + park_command_104_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Parkcommand104::ID)); + if (park_command_104_ == nullptr) { + AERROR << "Parkcommand104 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + steering_command_102_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Steeringcommand102::ID)); + if (steering_command_102_ == nullptr) { + AERROR << "Steeringcommand102 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + throttle_command_100_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Throttlecommand100::ID)); + if (throttle_command_100_ == nullptr) { + AERROR << "Throttlecommand100 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + vehicle_mode_command_105_ = dynamic_cast( + message_manager_->GetMutableProtocolDataById(Vehiclemodecommand105::ID)); + if (vehicle_mode_command_105_ == nullptr) { + AERROR << "Vehiclemodecommand105 does not exist in the DemoMessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + + AddSendMessage(); + + AINFO << "DemoController is initialized."; + + is_initialized_ = true; + return ErrorCode::OK; +} + +DemoController::~DemoController() {} + +bool DemoController::Start() { + if (!is_initialized_) { + AERROR << "DemoController has NOT been initiated."; + return false; + } + const auto& update_func = [this] { SecurityDogThreadFunc(); }; + thread_.reset(new std::thread(update_func)); + + return true; +} + +void DemoController::Stop() { + if (!is_initialized_) { + AERROR << "DemoController stops or starts improperly!"; + return; + } + + if (thread_ != nullptr && thread_->joinable()) { + thread_->join(); + thread_.reset(); + AINFO << "DemoController stopped."; + } +} + +Chassis DemoController::chassis() { + chassis_.Clear(); + Demo chassis_detail = GetNewRecvChassisDetail(); + + // 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); + + // 4 chassis spd + if (chassis_detail.has_vcu_report_505() && + chassis_detail.vcu_report_505().has_speed()) { + chassis_.set_speed_mps( + static_cast(chassis_detail.vcu_report_505().speed())); + } else { + chassis_.set_speed_mps(0); + } + + // 5 throttle + if (chassis_detail.has_throttle_report_500() && + chassis_detail.throttle_report_500().has_throttle_pedal_actual()) { + chassis_.set_throttle_percentage(static_cast( + chassis_detail.throttle_report_500().throttle_pedal_actual())); + } else { + chassis_.set_throttle_percentage(0); + } + + // 6 brake + if (chassis_detail.has_brake_report_501() && + chassis_detail.brake_report_501().has_brake_pedal_actual()) { + chassis_.set_brake_percentage(static_cast( + chassis_detail.brake_report_501().brake_pedal_actual())); + } else { + chassis_.set_brake_percentage(0); + } + + // 7 gear + if (chassis_detail.has_gear_report_503() && + chassis_detail.gear_report_503().has_gear_actual()) { + Chassis::GearPosition gear_pos = Chassis::GEAR_INVALID; + + if (chassis_detail.gear_report_503().gear_actual() == + Gear_report_503::GEAR_ACTUAL_NEUTRAL) { + gear_pos = Chassis::GEAR_NEUTRAL; + } + if (chassis_detail.gear_report_503().gear_actual() == + Gear_report_503::GEAR_ACTUAL_REVERSE) { + gear_pos = Chassis::GEAR_REVERSE; + } + if (chassis_detail.gear_report_503().gear_actual() == + Gear_report_503::GEAR_ACTUAL_DRIVE) { + gear_pos = Chassis::GEAR_DRIVE; + } + if (chassis_detail.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); + } + + // 8 steer + if (chassis_detail.has_steering_report_502() && + chassis_detail.steering_report_502().has_steer_angle_actual()) { + chassis_.set_steering_percentage(static_cast( + chassis_detail.steering_report_502().steer_angle_actual() * 100.0 / + vehicle_params_.max_steer_angle())); + } else { + chassis_.set_steering_percentage(0); + } + + // 9 checkresponse signal + // 9 checkresponse signal + // 9 checkresponse signal + if (chassis_detail.has_brake_report_501() && + chassis_detail.brake_report_501().has_brake_en_state()) { + chassis_.mutable_check_response()->set_is_esp_online( + chassis_detail.brake_report_501().brake_en_state() == 1); + } + + if (chassis_detail.has_steering_report_502() && + chassis_detail.steering_report_502().has_steer_en_state()) { + chassis_.mutable_check_response()->set_is_eps_online( + chassis_detail.steering_report_502().steer_en_state() == 1); + } + + if (chassis_detail.has_throttle_report_500() && + chassis_detail.throttle_report_500().has_throttle_en_state()) { + chassis_.mutable_check_response()->set_is_vcu_online( + chassis_detail.throttle_report_500().throttle_en_state() == 1); + } + + // check chassis error + if (CheckChassisError()) { + chassis_.mutable_engage_advice()->set_advice( + apollo::common::EngageAdvice::DISALLOW_ENGAGE); + chassis_.mutable_engage_advice()->set_reason( + "Chassis has some fault, please check the chassis_detail."); + } + + // check the chassis detail lost + if (is_chassis_communication_error_) { + chassis_.mutable_engage_advice()->set_advice( + apollo::common::EngageAdvice::DISALLOW_ENGAGE); + chassis_.mutable_engage_advice()->set_reason( + "demo chassis detail is lost! Please check the communication error."); + set_chassis_error_code(Chassis::CHASSIS_CAN_LOST); + set_driving_mode(Chassis::EMERGENCY_MODE); + } + + /* ADD YOUR OWN CAR CHASSIS OPERATION + // 14 battery soc + // 16 sonor list + // 17 set vin + // 18,19 bumper event + // 20 add checkresponse signal + */ + + return chassis_; +} + +void DemoController::Emergency() { + set_driving_mode(Chassis::EMERGENCY_MODE); + ResetProtocol(); +} + +ErrorCode DemoController::EnableAutoMode() { + if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) { + AINFO << "already in COMPLETE_AUTO_DRIVE mode"; + return ErrorCode::OK; + } + // set enable + brake_command_101_->set_brake_en_ctrl( + Brake_command_101::BRAKE_EN_CTRL_ENABLE); + + gear_command_103_->set_gear_en_ctrl(Gear_command_103::GEAR_EN_CTRL_ENABLE); + + steering_command_102_->set_steer_en_ctrl( + Steering_command_102::STEER_EN_CTRL_ENABLE); + + throttle_command_100_->set_throttle_en_ctrl( + Throttle_command_100::THROTTLE_EN_CTRL_ENABLE); + + 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. Please check the " + "emergency button or chassis."; + 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; +} + +ErrorCode DemoController::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; +} + +ErrorCode DemoController::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; + } + /* ADD YOUR OWN CAR CHASSIS OPERATION + // TODO(ALL): CHECK YOUR VEHICLE WHETHER SUPPORT THIS MODE OR NOT + // set enable + brake_command_101_->set_brake_en_ctrl( + Brake_command_101::BRAKE_EN_CTRL_DISABLE); + + gear_command_103_->set_gear_en_ctrl( + Gear_command_103::GEAR_EN_CTRL_DISABLE); + + steering_command_102_->set_steer_en_ctrl( + Steering_command_102::STEER_EN_CTRL_ENABLE); + + throttle_command_100_->set_throttle_en_ctrl( + Throttle_command_100::THROTTLE_EN_CTRL_DISABLE); + + + can_sender_->Update(); + if (!CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, true)) { + AERROR << "Failed to switch to AUTO_STEER_ONLY mode."; + Emergency(); + set_chassis_error_code(Chassis::CHASSIS_ERROR); + return ErrorCode::CANBUS_ERROR; + } + set_driving_mode(Chassis::AUTO_STEER_ONLY); + AINFO << "Switch to AUTO_STEER_ONLY mode ok."; + return ErrorCode::OK; + */ + return ErrorCode::OK; +} + +ErrorCode DemoController::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; + } + /* ADD YOUR OWN CAR CHASSIS OPERATION + // TODO(ALL): CHECK YOUR VEHICLE WHETHER SUPPORT THIS MODE OR NOT + // set enable + brake_command_101_->set_brake_en_ctrl( + Brake_command_101::BRAKE_EN_CTRL_ENABLE); + + gear_command_103_->set_gear_en_ctrl( + Gear_command_103::GEAR_EN_CTRL_ENABLE); + + steering_command_102_->set_steer_en_ctrl( + Steering_command_102::STEER_EN_CTRL_DISABLE); + + throttle_command_100_->set_throttle_en_ctrl( + Throttle_command_100::THROTTLE_EN_CTRL_ENABLE); + + + can_sender_->Update(); + if (!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, true)) { + AERROR << "Failed to switch to AUTO_SPEED_ONLY mode."; + Emergency(); + set_chassis_error_code(Chassis::CHASSIS_ERROR); + return ErrorCode::CANBUS_ERROR; + } + set_driving_mode(Chassis::AUTO_SPEED_ONLY); + AINFO << "Switch to AUTO_SPEED_ONLY mode ok."; + return ErrorCode::OK; + */ + return ErrorCode::OK; +} + +// NEUTRAL, REVERSE, DRIVE +void DemoController::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; + } + } +} + +// brake with pedal +// pedal:0.00~99.99, unit:percentage +void DemoController::Brake(double pedal) { + // double real_value = vehicle_params_.max_acceleration() * 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:percentage +void DemoController::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); +} + +// 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 DemoController::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; + } + /* ADD YOUR OWN CAR CHASSIS OPERATION + // TODO(ALL): CHECK YOUR VEHICLE WHETHER SUPPORT THIS DRIVE MODE + */ +} + +// confirm the car is driven by speed command +// speed:-xx.0~xx.0, unit:m/s +void DemoController::Speed(double speed) { + if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE && + driving_mode() != Chassis::AUTO_SPEED_ONLY) { + AINFO << "The current drive mode does not need to set speed."; + return; + } + /* ADD YOUR OWN CAR CHASSIS OPERATION + // TODO(ALL): CHECK YOUR VEHICLE WHETHER SUPPORT THIS DRIVE MODE + */ +} + +// demo default, +470 ~ -470 or other, left:+, right:- +// need to be compatible with control module, so reverse +// steering with steering angle +// angle:99.99~0.00~-99.99, unit:deg, left:+, right:- +void DemoController::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); +} + +// demo default, steering with new angle and angle speed +// angle:99.99~0.00~-99.99, unit:deg, left:+, right:- +// angle_spd:0.00~99.99, unit:deg/s +void DemoController::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); +} + +void DemoController::SetEpbBreak(const ControlCommand& command) { + if (command.parking_brake()) { + // None + } else { + // None + } +} + +void DemoController::SetBeam(const VehicleSignal& vehicle_signal) { + if (vehicle_signal.high_beam()) { + // None + } else if (vehicle_signal.low_beam()) { + // None + } else { + // None + } +} + +void DemoController::SetHorn(const VehicleSignal& vehicle_signal) { + if (vehicle_signal.horn()) { + // None + } else { + // None + } +} + +void DemoController::SetTurningSignal(const VehicleSignal& vehicle_signal) { + // Set Turn Signal + /* ADD YOUR OWN CAR CHASSIS OPERATION + auto signal = vehicle_signal.turn_signal(); + if (signal == common::VehicleSignal::TURN_LEFT) { + + } else if (signal == common::VehicleSignal::TURN_RIGHT) { + + } else { + + } + */ +} + +ErrorCode DemoController::HandleCustomOperation( + const external_command::ChassisCommand& command) { + return ErrorCode::OK; +} + +bool DemoController::VerifyID() { + if (!CheckVin()) { + AERROR << "Failed to get the vin. Get vin again."; + GetVin(); + return false; + } else { + ResetVin(); + return true; + } +} + +bool DemoController::CheckVin() { + /* ADD YOUR OWN CAR CHASSIS OPERATION + if (chassis_.vehicle_id().vin().size() >= 7) { + AINFO << "Vin check success! Vehicel vin is " + << chassis_.vehicle_id().vin(); + return true; + } else { + AINFO << "Vin check failed! Current vin size is " + << chassis_.vehicle_id().vin().size(); + return false; + } + */ + return false; +} + +void DemoController::GetVin() { + // Get vin from vehicle if exist + /* ADD YOUR OWN CAR CHASSIS OPERATION + vehicle_mode_command_116_->set_vin_req_cmd( + Vehicle_mode_command_116::VIN_REQ_CMD_VIN_REQ_ENABLE); + AINFO << "Get vin"; + can_sender_->Update(); + */ +} + +void DemoController::ResetVin() { + // Reset vin from vehicle if exist + /* ADD YOUR OWN CAR CHASSIS OPERATION + vehicle_mode_command_116_->set_vin_req_cmd( + Vehicle_mode_command_116::VIN_REQ_CMD_VIN_REQ_DISABLE); + AINFO << "Reset vin"; + can_sender_->Update(); + */ +} + +void DemoController::ResetProtocol() { message_manager_->ResetSendMessages(); } + +bool DemoController::CheckChassisError() { + if (is_chassis_communication_error_) { + AERROR_EVERY(100) << "ChassisDetail has no demo vehicle info."; + return false; + } + + /* ADD YOUR OWN CAR CHASSIS OPERATION + // steer fault + // drive fault + // brake fault + */ + return false; +} + +void DemoController::SecurityDogThreadFunc() { + 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(); + } + + std::chrono::duration 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; + + // 1. horizontal control check + if ((mode == Chassis::COMPLETE_AUTO_DRIVE || + mode == Chassis::AUTO_STEER_ONLY) && + !CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, false)) { + ++horizontal_ctrl_fail; + if (horizontal_ctrl_fail >= kMaxFailAttempt) { + emergency_mode = true; + AERROR << "Driving_mode is into emergency by steer manual intervention"; + set_chassis_error_code(Chassis::MANUAL_INTERVENTION); + } + } else { + horizontal_ctrl_fail = 0; + } + + // 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; + AERROR << "Driving_mode is into emergency by speed manual intervention"; + set_chassis_error_code(Chassis::MANUAL_INTERVENTION); + } + } else { + vertical_ctrl_fail = 0; + } + + // 3. chassis fault check + if (CheckChassisError()) { + set_chassis_error_code(Chassis::CHASSIS_ERROR); + emergency_mode = true; + } + + // process emergency_mode + if (emergency_mode && mode != Chassis::EMERGENCY_MODE) { + set_driving_mode(Chassis::EMERGENCY_MODE); + message_manager_->ResetSendMessages(); + can_sender_->Update(); + } + + // recove error code + if (!emergency_mode && !is_chassis_communication_error_ && + mode == Chassis::EMERGENCY_MODE) { + set_chassis_error_code(Chassis::NO_ERROR); + } + + end = ::apollo::cyber::Time::Now().ToMicrosecond(); + std::chrono::duration elapsed{end - start}; + if (elapsed < default_period) { + std::this_thread::sleep_for(default_period - elapsed); + } else { + AERROR << "Too much time consumption in DemoController looping process:" + << elapsed.count(); + } + } +} + +bool DemoController::CheckResponse(const int32_t flags, bool need_wait) { + int32_t retry_num = 20; + bool is_eps_online = false; + bool is_vcu_online = false; + bool is_esp_online = false; + + do { + bool check_ok = true; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + is_eps_online = chassis_.has_check_response() && + chassis_.check_response().has_is_eps_online() && + chassis_.check_response().is_eps_online(); + check_ok = check_ok && is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + is_vcu_online = chassis_.has_check_response() && + chassis_.check_response().has_is_vcu_online() && + chassis_.check_response().is_vcu_online(); + is_esp_online = chassis_.has_check_response() && + chassis_.check_response().has_is_esp_online() && + chassis_.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(20)); + } + } while (need_wait && retry_num); + + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } + + return false; +} + +void DemoController::set_chassis_error_mask(const int32_t mask) { + std::lock_guard lock(chassis_mask_mutex_); + chassis_error_mask_ = mask; +} + +int32_t DemoController::chassis_error_mask() { + std::lock_guard lock(chassis_mask_mutex_); + return chassis_error_mask_; +} + +Chassis::ErrorCode DemoController::chassis_error_code() { + std::lock_guard lock(chassis_error_code_mutex_); + return chassis_error_code_; +} + +void DemoController::set_chassis_error_code( + const Chassis::ErrorCode& error_code) { + std::lock_guard lock(chassis_error_code_mutex_); + chassis_error_code_ = error_code; +} + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/demo_controller.h b/modules/canbus_vehicle/demo/demo_controller.h new file mode 100644 index 00000000000..7abcd354b84 --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_controller.h @@ -0,0 +1,154 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include + +#include "modules/canbus/proto/canbus_conf.pb.h" +#include "modules/canbus/proto/vehicle_parameter.pb.h" +#include "modules/common_msgs/basic_msgs/error_code.pb.h" +#include "modules/common_msgs/chassis_msgs/chassis.pb.h" +#include "modules/common_msgs/control_msgs/control_cmd.pb.h" + +#include "modules/canbus/vehicle/vehicle_controller.h" +#include "modules/canbus_vehicle/demo/protocol/brake_command_101.h" +#include "modules/canbus_vehicle/demo/protocol/gear_command_103.h" +#include "modules/canbus_vehicle/demo/protocol/park_command_104.h" +#include "modules/canbus_vehicle/demo/protocol/steering_command_102.h" +#include "modules/canbus_vehicle/demo/protocol/throttle_command_100.h" +#include "modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class DemoController final : public VehicleController<::apollo::canbus::Demo> { + public: + DemoController() {} + + virtual ~DemoController(); + + ::apollo::common::ErrorCode Init( + const VehicleParameter& params, + CanSender<::apollo::canbus::Demo>* const can_sender, + MessageManager<::apollo::canbus::Demo>* const message_manager) override; + + bool Start() override; + + /** + * @brief stop the vehicle controller. + */ + void Stop() override; + + /** + * @brief calculate and return the chassis. + * @returns a copy of chassis. Use copy here to avoid multi-thread issues. + */ + Chassis chassis() override; + + /** + * @brief add the sender message. + */ + void AddSendMessage() override; + + private: + // main logical function for operation the car enter or exit the auto driving + void Emergency() override; + ::apollo::common::ErrorCode EnableAutoMode() override; + ::apollo::common::ErrorCode DisableAutoMode() override; + ::apollo::common::ErrorCode EnableSteeringOnlyMode() override; + ::apollo::common::ErrorCode EnableSpeedOnlyMode() override; + + // NEUTRAL, REVERSE, DRIVE + void Gear(Chassis::GearPosition state) override; + + // brake with new acceleration + // acceleration:0.00~99.99, unit: + // acceleration_spd: 60 ~ 100, suggest: 90 + void Brake(double acceleration) override; + + // drive with old acceleration + // gas:0.00~99.99 unit: + void Throttle(double throttle) override; + + // drive with acceleration/deceleration + // acc:-7.0~5.0 unit:m/s^2 + void Acceleration(double acc) override; + + // drive with speed + // speed:-xx.0~xx.0 unit:m/s + void Speed(double speed); + + // steering with old angle speed + // angle:-99.99~0.00~99.99, unit:, left:+, right:- + void Steer(double angle) override; + + // steering with new angle speed + // angle:-99.99~0.00~99.99, unit:, left:+, right:- + // angle_spd:0.00~99.99, unit:deg/s + void Steer(double angle, double angle_spd) override; + + // set Electrical Park Brake + void SetEpbBreak(const control::ControlCommand& command) override; + void SetBeam(const common::VehicleSignal& vehicle_signal) override; + void SetHorn(const common::VehicleSignal& vehicle_signal) override; + void SetTurningSignal(const common::VehicleSignal& vehicle_signal) override; + + // set Chassis Command + common::ErrorCode HandleCustomOperation( + const external_command::ChassisCommand& command) override; + + // response vid + bool VerifyID() override; + bool CheckVin(); + void GetVin(); + void ResetVin(); + void ResetProtocol(); + bool CheckChassisError(); + + private: + void SecurityDogThreadFunc(); + virtual bool CheckResponse(const int32_t flags, bool need_wait); + void set_chassis_error_mask(const int32_t mask); + int32_t chassis_error_mask(); + Chassis::ErrorCode chassis_error_code(); + void set_chassis_error_code(const Chassis::ErrorCode& error_code); + + private: + // control protocol + Brakecommand101* brake_command_101_ = nullptr; + Gearcommand103* gear_command_103_ = nullptr; + Parkcommand104* park_command_104_ = nullptr; + Steeringcommand102* steering_command_102_ = nullptr; + Throttlecommand100* throttle_command_100_ = nullptr; + Vehiclemodecommand105* vehicle_mode_command_105_ = nullptr; + + Chassis chassis_; + std::unique_ptr thread_; + bool is_chassis_error_ = false; + + std::mutex chassis_error_code_mutex_; + Chassis::ErrorCode chassis_error_code_ = Chassis::NO_ERROR; + + std::mutex chassis_mask_mutex_; + int32_t chassis_error_mask_ = 0; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/demo_message_manager.cc b/modules/canbus_vehicle/demo/demo_message_manager.cc new file mode 100644 index 00000000000..bae4e30fe05 --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_message_manager.cc @@ -0,0 +1,78 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/demo_message_manager.h" + +#include "modules/canbus_vehicle/demo/protocol/brake_command_101.h" +#include "modules/canbus_vehicle/demo/protocol/gear_command_103.h" +#include "modules/canbus_vehicle/demo/protocol/park_command_104.h" +#include "modules/canbus_vehicle/demo/protocol/steering_command_102.h" +#include "modules/canbus_vehicle/demo/protocol/throttle_command_100.h" +#include "modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h" +#include "modules/canbus_vehicle/demo/protocol/bms_report_512.h" +#include "modules/canbus_vehicle/demo/protocol/brake_report_501.h" +#include "modules/canbus_vehicle/demo/protocol/gear_report_503.h" +#include "modules/canbus_vehicle/demo/protocol/park_report_504.h" +#include "modules/canbus_vehicle/demo/protocol/steering_report_502.h" +#include "modules/canbus_vehicle/demo/protocol/throttle_report_500.h" +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h" +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h" +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h" +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h" +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h" +#include "modules/canbus_vehicle/demo/protocol/vcu_report_505.h" +#include "modules/canbus_vehicle/demo/protocol/vin_resp1_514.h" +#include "modules/canbus_vehicle/demo/protocol/vin_resp2_515.h" +#include "modules/canbus_vehicle/demo/protocol/vin_resp3_516.h" +#include "modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h" + +namespace apollo { +namespace canbus { +namespace demo { + +DemoMessageManager::DemoMessageManager() { + // Control Messages + AddSendProtocolData(); + AddSendProtocolData(); + AddSendProtocolData(); + AddSendProtocolData(); + AddSendProtocolData(); + AddSendProtocolData(); + + // Report Messages + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); + AddRecvProtocolData(); +} + +DemoMessageManager::~DemoMessageManager() {} + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/demo_message_manager.h b/modules/canbus_vehicle/demo/demo_message_manager.h new file mode 100644 index 00000000000..c377f041531 --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_message_manager.h @@ -0,0 +1,42 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * @file demo_message_manager.h + * @brief the class of DemoMessageManager + */ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/message_manager.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::MessageManager; + +class DemoMessageManager : public MessageManager<::apollo::canbus::Demo> { + public: + DemoMessageManager(); + virtual ~DemoMessageManager(); +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/demo_vehicle_factory.cc b/modules/canbus_vehicle/demo/demo_vehicle_factory.cc new file mode 100644 index 00000000000..be93952b3fa --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_vehicle_factory.cc @@ -0,0 +1,208 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/demo_vehicle_factory.h" + +#include "cyber/common/log.h" +#include "modules/canbus/common/canbus_gflags.h" +#include "modules/common/adapters/adapter_gflags.h" +#include "modules/common/util/util.h" +#include "modules/drivers/canbus/can_client/can_client_factory.h" + +using apollo::common::ErrorCode; +using apollo::control::ControlCommand; +using apollo::drivers::canbus::CanClientFactory; + +namespace apollo { +namespace canbus { + +bool DemoVehicleFactory::Init(const CanbusConf *canbus_conf) { + // Init can client + auto can_factory = CanClientFactory::Instance(); + can_factory->RegisterCanClients(); + can_client_ = can_factory->CreateCANClient(canbus_conf->can_card_parameter()); + if (!can_client_) { + AERROR << "Failed to create can client."; + return false; + } + AINFO << "Can client is successfully created."; + + message_manager_ = this->CreateMessageManager(); + if (message_manager_ == nullptr) { + AERROR << "Failed to create message manager."; + return false; + } + AINFO << "Message manager is successfully created."; + + if (can_receiver_.Init(can_client_.get(), message_manager_.get(), + canbus_conf->enable_receiver_log()) != ErrorCode::OK) { + AERROR << "Failed to init can receiver."; + return false; + } + AINFO << "The can receiver is successfully initialized."; + + if (can_sender_.Init(can_client_.get(), message_manager_.get(), + canbus_conf->enable_sender_log()) != ErrorCode::OK) { + AERROR << "Failed to init can sender."; + return false; + } + AINFO << "The can sender is successfully initialized."; + + vehicle_controller_ = CreateVehicleController(); + if (vehicle_controller_ == nullptr) { + AERROR << "Failed to create vehicle controller."; + return false; + } + AINFO << "The vehicle controller is successfully created."; + + if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, + message_manager_.get()) != ErrorCode::OK) { + AERROR << "Failed to init vehicle controller."; + return false; + } + + AINFO << "The vehicle controller is successfully" + << " initialized with canbus conf as : " + << canbus_conf->vehicle_parameter().ShortDebugString(); + + node_ = ::apollo::cyber::CreateNode("chassis_detail"); + + chassis_detail_writer_ = + node_->CreateWriter<::apollo::canbus::Demo>(FLAGS_chassis_detail_topic); + + chassis_detail_sender_writer_ = node_->CreateWriter<::apollo::canbus::Demo>( + FLAGS_chassis_detail_sender_topic); + + return true; +} + +bool DemoVehicleFactory::Start() { + // 1. init and start the can card hardware + if (can_client_->Start() != ErrorCode::OK) { + AERROR << "Failed to start can client"; + return false; + } + AINFO << "Can client is started."; + + // 2. start receive first then send + if (can_receiver_.Start() != ErrorCode::OK) { + AERROR << "Failed to start can receiver."; + return false; + } + AINFO << "Can receiver is started."; + + // 3. start send + if (can_sender_.Start() != ErrorCode::OK) { + AERROR << "Failed to start can sender."; + return false; + } + + // 4. start controller + if (!vehicle_controller_->Start()) { + AERROR << "Failed to start vehicle controller."; + return false; + } + + return true; +} + +void DemoVehicleFactory::Stop() { + can_sender_.Stop(); + can_receiver_.Stop(); + can_client_->Stop(); + vehicle_controller_->Stop(); + AINFO << "Cleanup cansender, canreceiver, canclient, vehicle controller."; +} + +void DemoVehicleFactory::UpdateCommand( + const apollo::control::ControlCommand *control_command) { + if (vehicle_controller_->Update(*control_command) != ErrorCode::OK) { + AERROR << "Failed to process callback function OnControlCommand because " + "vehicle_controller_->Update error."; + return; + } + can_sender_.Update(); +} + +void DemoVehicleFactory::UpdateCommand( + const apollo::external_command::ChassisCommand *chassis_command) { + if (vehicle_controller_->Update(*chassis_command) != ErrorCode::OK) { + AERROR << "Failed to process callback function OnControlCommand because " + "vehicle_controller_->Update error."; + return; + } + can_sender_.Update(); +} + +Chassis DemoVehicleFactory::publish_chassis() { + Chassis chassis = vehicle_controller_->chassis(); + ADEBUG << chassis.ShortDebugString(); + return chassis; +} + +void DemoVehicleFactory::PublishChassisDetail() { + Demo chassis_detail = vehicle_controller_->GetNewRecvChassisDetail(); + ADEBUG << "latest chassis_detail is " << chassis_detail.ShortDebugString(); + chassis_detail_writer_->Write(chassis_detail); +} + +void DemoVehicleFactory::PublishChassisDetailSender() { + Demo sender_chassis_detail = vehicle_controller_->GetNewSenderChassisDetail(); + ADEBUG << "latest sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + chassis_detail_sender_writer_->Write(sender_chassis_detail); +} + +void DemoVehicleFactory::UpdateHeartbeat() { can_sender_.Update_Heartbeat(); } + +bool DemoVehicleFactory::CheckChassisCommunicationFault() { + if (vehicle_controller_->CheckChassisCommunicationError()) { + return true; + } + return false; +} + +void DemoVehicleFactory::AddSendProtocol() { + vehicle_controller_->AddSendMessage(); +} + +void DemoVehicleFactory::ClearSendProtocol() { can_sender_.ClearMessage(); } + +bool DemoVehicleFactory::IsSendProtocolClear() { + if (can_sender_.IsMessageClear()) { + return true; + } + return false; +} + +Chassis::DrivingMode DemoVehicleFactory::Driving_Mode() { + return vehicle_controller_->driving_mode(); +} + +std::unique_ptr> +DemoVehicleFactory::CreateVehicleController() { + return std::unique_ptr>( + new demo::DemoController()); +} + +std::unique_ptr> +DemoVehicleFactory::CreateMessageManager() { + return std::unique_ptr>( + new demo::DemoMessageManager()); +} + +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/demo_vehicle_factory.h b/modules/canbus_vehicle/demo/demo_vehicle_factory.h new file mode 100644 index 00000000000..24eadb7558c --- /dev/null +++ b/modules/canbus_vehicle/demo/demo_vehicle_factory.h @@ -0,0 +1,164 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * @file demo_vehicle_factory.h + */ + +#pragma once + +#include + +#include "modules/canbus/proto/canbus_conf.pb.h" +#include "modules/canbus/proto/vehicle_parameter.pb.h" +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" +#include "modules/common_msgs/control_msgs/control_cmd.pb.h" + +#include "cyber/cyber.h" +#include "modules/canbus/vehicle/abstract_vehicle_factory.h" +#include "modules/canbus/vehicle/vehicle_controller.h" +#include "modules/canbus_vehicle/demo/demo_controller.h" +#include "modules/canbus_vehicle/demo/demo_message_manager.h" +#include "modules/common/status/status.h" +#include "modules/drivers/canbus/can_client/can_client.h" +#include "modules/drivers/canbus/can_comm/can_receiver.h" +#include "modules/drivers/canbus/can_comm/can_sender.h" +#include "modules/drivers/canbus/can_comm/message_manager.h" + +/** + * @namespace apollo::canbus + * @brief apollo::canbus + */ +namespace apollo { +namespace canbus { + +/** + * @class DemoVehicleFactory + * + * @brief this class is inherited from AbstractVehicleFactory. It can be used to + * create controller and message manager for demo vehicle. + */ +class DemoVehicleFactory : public AbstractVehicleFactory { + public: + /** + * @brief destructor + */ + virtual ~DemoVehicleFactory() = default; + + /** + * @brief init vehicle factory + * @returns true if successfully initialized + */ + bool Init(const CanbusConf *canbus_conf) override; + + /** + * @brief start canclient, cansender, canreceiver, vehicle controller + * @returns true if successfully started + */ + bool Start() override; + + /** + * @brief stop canclient, cansender, canreceiver, vehicle controller + */ + void Stop() override; + + /** + * @brief update control command + */ + void UpdateCommand( + const apollo::control::ControlCommand *control_command) override; + + void UpdateCommand( + const apollo::external_command::ChassisCommand *chassis_command) override; + + /** + * @brief publish chassis messages + */ + Chassis publish_chassis() override; + + /** + * @brief create cansender heartbeat + */ + void UpdateHeartbeat() override; + + /** + * @brief publish chassis for vehicle messages + */ + void PublishChassisDetail() override; + + /** + * @brief publish chassis for apollo sender messages + */ + void PublishChassisDetailSender() override; + + /** + * @brief check chassis can receiver lost + */ + bool CheckChassisCommunicationFault() override; + + /** + * @brief add the can sender messages + */ + void AddSendProtocol() override; + + /** + * @brief clear the can sender messages + */ + void ClearSendProtocol() override; + + /** + * @brief check the sender message clear or not + */ + bool IsSendProtocolClear() override; + + /** + * @brief get the latest chassis driving mode + */ + Chassis::DrivingMode Driving_Mode() override; + + private: + /** + * @brief create demo vehicle controller + * @returns a unique_ptr that points to the created controller + */ + std::unique_ptr> + CreateVehicleController(); + + /** + * @brief create demo message manager + * @returns a unique_ptr that points to the created message manager + */ + std::unique_ptr> + CreateMessageManager(); + + std::unique_ptr<::apollo::cyber::Node> node_ = nullptr; + std::unique_ptr can_client_; + CanSender<::apollo::canbus::Demo> can_sender_; + apollo::drivers::canbus::CanReceiver<::apollo::canbus::Demo> can_receiver_; + std::unique_ptr> message_manager_; + std::unique_ptr> + vehicle_controller_; + + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Demo>> + chassis_detail_writer_; + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Demo>> + chassis_detail_sender_writer_; +}; + +CYBER_REGISTER_VEHICLEFACTORY(DemoVehicleFactory) + +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/proto/BUILD b/modules/canbus_vehicle/demo/proto/BUILD new file mode 100644 index 00000000000..c5b1f7f98ba --- /dev/null +++ b/modules/canbus_vehicle/demo/proto/BUILD @@ -0,0 +1,12 @@ +## Auto generated by `proto_build_generator.py` +load("//tools/proto:proto.bzl", "proto_library") +load("//tools:apollo_package.bzl", "apollo_package") + +package(default_visibility = ["//visibility:public"]) + +proto_library( + name = "demo_proto", + srcs = ["demo.proto"], +) + +apollo_package() diff --git a/modules/canbus_vehicle/demo/proto/demo.proto b/modules/canbus_vehicle/demo/proto/demo.proto new file mode 100644 index 00000000000..f7c3fe3a513 --- /dev/null +++ b/modules/canbus_vehicle/demo/proto/demo.proto @@ -0,0 +1,496 @@ +syntax = "proto2"; + +package apollo.canbus; + +message Throttle_command_100 { +// Control Message + enum Throttle_en_ctrlType { + THROTTLE_EN_CTRL_DISABLE = 0; + THROTTLE_EN_CTRL_ENABLE = 1; + } + // [] [0|15] + optional int32 heartbeat_100 = 1; + // [m/s] [0|40.95] + optional double speed_target = 2; + // [m/s^2] [0|10] + optional double throttle_acc = 3; + // [] [0|255] + optional int32 checksum_100 = 4; + // command [%] [0|100] + optional double throttle_pedal_target = 5; + // enable [] [0|1] + optional Throttle_en_ctrlType throttle_en_ctrl = 6; +} + +message Brake_command_101 { +// Control Message + enum Aeb_en_ctrlType { + AEB_EN_CTRL_DISABLE_AEB = 0; + AEB_EN_CTRL_ENABLE_AEB = 1; + } + enum Brake_en_ctrlType { + BRAKE_EN_CTRL_DISABLE = 0; + BRAKE_EN_CTRL_ENABLE = 1; + } + // [] [0|15] + optional int32 heartbeat_101 = 1; + // [] [0|0] + optional Aeb_en_ctrlType aeb_en_ctrl = 2; + // [m/s^2] [0|10] + optional double brake_dec = 3; + // [] [0|255] + optional int32 checksum_101 = 4; + // command [%] [0|100] + optional double brake_pedal_target = 5; + // enable [] [0|1] + optional Brake_en_ctrlType brake_en_ctrl = 6; +} + +message Steering_command_102 { +// Control Message + enum Steer_en_ctrlType { + STEER_EN_CTRL_DISABLE = 0; + STEER_EN_CTRL_ENABLE = 1; + } + // [] [0|15] + optional int32 heartbeat_102 = 1; + // enable [] [0|1] + optional Steer_en_ctrlType steer_en_ctrl = 2; + // command [deg] [-500|500] + optional int32 steer_angle_target = 3; + // [deg/s] [0|250] + optional int32 steer_angle_spd_target = 4; + // [] [0|255] + optional int32 checksum_102 = 5; +} + +message Gear_command_103 { +// Control Message + enum Gear_targetType { + GEAR_TARGET_INVALID = 0; + GEAR_TARGET_PARK = 1; + GEAR_TARGET_REVERSE = 2; + GEAR_TARGET_NEUTRAL = 3; + GEAR_TARGET_DRIVE = 4; + } + enum Gear_en_ctrlType { + GEAR_EN_CTRL_DISABLE = 0; + GEAR_EN_CTRL_ENABLE = 1; + } + // [] [0|15] + optional int32 heartbeat_103 = 1; + // command [] [0|4] + optional Gear_targetType gear_target = 2; + // enable [] [0|1] + optional Gear_en_ctrlType gear_en_ctrl = 3; + // [] [0|255] + optional int32 checksum_103 = 4; +} + +message Park_command_104 { +// Control Message + enum Park_targetType { + PARK_TARGET_RELEASE = 0; + PARK_TARGET_PARKING_TRIGGER = 1; + } + enum Park_en_ctrlType { + PARK_EN_CTRL_DISABLE = 0; + PARK_EN_CTRL_ENABLE = 1; + } + // [] [0|15] + optional int32 heartbeat_104 = 1; + // [] [0|255] + optional int32 checksum_104 = 2; + // command [] [0|1] + optional Park_targetType park_target = 3; + // enable [] [0|1] + optional Park_en_ctrlType park_en_ctrl = 4; +} + +message Vehicle_mode_command_105 { +// Control Message + enum Turn_light_ctrlType { + TURN_LIGHT_CTRL_TURNLAMP_OFF = 0; + TURN_LIGHT_CTRL_LEFT_TURNLAMP_ON = 1; + TURN_LIGHT_CTRL_RIGHT_TURNLAMP_ON = 2; + TURN_LIGHT_CTRL_HAZARD_WARNING_LAMPSTS_ON = 3; + } + enum Vin_reqType { + VIN_REQ_VIN_REQ_DISABLE = 0; + VIN_REQ_VIN_REQ_ENABLE = 1; + } + enum Drive_mode_ctrlType { + DRIVE_MODE_CTRL_THROTTLE_PADDLE_DRIVE = 0; + DRIVE_MODE_CTRL_SPEED_DRIVE = 1; + } + enum Steer_mode_ctrlType { + STEER_MODE_CTRL_STANDARD_STEER = 0; + STEER_MODE_CTRL_NON_DIRECTION_STEER = 1; + STEER_MODE_CTRL_SYNC_DIRECTION_STEER = 2; + } + // [] [0|15] + optional int32 heartbeat_105 = 1; + // [] [0|255] + optional int32 checksum_105 = 2; + // [] [0|7] + optional Turn_light_ctrlType turn_light_ctrl = 3; + // [] [0|1] + optional Vin_reqType vin_req = 4; + // [] [0|7] + optional Drive_mode_ctrlType drive_mode_ctrl = 5; + // [] [0|7] + optional Steer_mode_ctrlType steer_mode_ctrl = 6; +} + +message Throttle_report_500 { +// Report Message + enum Throttle_flt2Type { + THROTTLE_FLT2_NO_FAULT = 0; + THROTTLE_FLT2_DRIVE_SYSTEM_COMUNICATION_FAULT = 1; + } + enum Throttle_flt1Type { + THROTTLE_FLT1_NO_FAULT = 0; + THROTTLE_FLT1_DRIVE_SYSTEM_HARDWARE_FAULT = 1; + } + enum Throttle_en_stateType { + THROTTLE_EN_STATE_MANUAL = 0; + THROTTLE_EN_STATE_AUTO = 1; + THROTTLE_EN_STATE_TAKEOVER = 2; + THROTTLE_EN_STATE_STANDBY = 3; + } + // command [%] [0|100] + optional double throttle_pedal_actual = 1; + // Drive system communication fault [] [0|1] + optional Throttle_flt2Type throttle_flt2 = 2; + // Drive system hardware fault [] [0|1] + optional Throttle_flt1Type throttle_flt1 = 3; + // enable [] [0|2] + optional Throttle_en_stateType throttle_en_state = 4; +} + +message Brake_report_501 { +// Report Message + enum Brake_flt2Type { + BRAKE_FLT2_NO_FAULT = 0; + BRAKE_FLT2_BRAKE_SYSTEM_COMUNICATION_FAULT = 1; + } + enum Brake_flt1Type { + BRAKE_FLT1_NO_FAULT = 0; + BRAKE_FLT1_BRAKE_SYSTEM_HARDWARE_FAULT = 1; + } + enum Brake_en_stateType { + BRAKE_EN_STATE_MANUAL = 0; + BRAKE_EN_STATE_AUTO = 1; + BRAKE_EN_STATE_TAKEOVER = 2; + BRAKE_EN_STATE_STANDBY = 3; + } + // command [%] [0|100] + optional double brake_pedal_actual = 1; + // Brake system communication fault [] [0|1] + optional Brake_flt2Type brake_flt2 = 2; + // Brake system hardware fault [] [0|1] + optional Brake_flt1Type brake_flt1 = 3; + // enable [] [0|2] + optional Brake_en_stateType brake_en_state = 4; +} + +message Steering_report_502 { +// Report Message + enum Steer_flt2Type { + STEER_FLT2_NO_FAULT = 0; + STEER_FLT2_STEER_SYSTEM_COMUNICATION_FAULT = 1; + } + enum Steer_flt1Type { + STEER_FLT1_NO_FAULT = 0; + STEER_FLT1_STEER_SYSTEM_HARDWARE_FAULT = 1; + } + enum Steer_en_stateType { + STEER_EN_STATE_MANUAL = 0; + STEER_EN_STATE_AUTO = 1; + STEER_EN_STATE_TAKEOVER = 2; + STEER_EN_STATE_STANDBY = 3; + } + // [deg] [-500|500] + optional int32 steer_angle_rear_actual = 1; + // [deg/s] [0|0] + optional int32 steer_angle_spd_actual = 2; + // Steer system communication fault [] [0|255] + optional Steer_flt2Type steer_flt2 = 3; + // Steer system hardware fault [] [0|255] + optional Steer_flt1Type steer_flt1 = 4; + // enable [] [0|2] + optional Steer_en_stateType steer_en_state = 5; + // command [deg] [-500|500] + optional int32 steer_angle_actual = 6; +} + +message Gear_report_503 { +// Report Message + enum Gear_fltType { + GEAR_FLT_NO_FAULT = 0; + GEAR_FLT_FAULT = 1; + } + enum Gear_actualType { + GEAR_ACTUAL_INVALID = 0; + GEAR_ACTUAL_PARK = 1; + GEAR_ACTUAL_REVERSE = 2; + GEAR_ACTUAL_NEUTRAL = 3; + GEAR_ACTUAL_DRIVE = 4; + } + // fault [] [0|1] + optional Gear_fltType gear_flt = 1; + // command [] [0|4] + optional Gear_actualType gear_actual = 2; +} + +message Park_report_504 { +// Report Message + enum Parking_actualType { + PARKING_ACTUAL_RELEASE = 0; + PARKING_ACTUAL_PARKING_TRIGGER = 1; + } + enum Park_fltType { + PARK_FLT_NO_FAULT = 0; + PARK_FLT_FAULT = 1; + } + // command [] [0|1] + optional Parking_actualType parking_actual = 1; + // fault [] [0|1] + optional Park_fltType park_flt = 2; +} + +message Vcu_report_505 { +// Report Message + enum Vehicle_mode_stateType { + VEHICLE_MODE_STATE_MANUAL_REMOTE_MODE = 0; + VEHICLE_MODE_STATE_AUTO_MODE = 1; + VEHICLE_MODE_STATE_EMERGENCY_MODE = 2; + VEHICLE_MODE_STATE_STANDBY_MODE = 3; + } + enum Aeb_modeType { + AEB_MODE_DISABLE = 0; + AEB_MODE_ENABLE = 1; + } + enum Brake_light_actualType { + BRAKE_LIGHT_ACTUAL_BRAKELIGHT_OFF = 0; + BRAKE_LIGHT_ACTUAL_BRAKELIGHT_ON = 1; + } + enum Turn_light_actualType { + TURN_LIGHT_ACTUAL_TURNLAMPSTS_OFF = 0; + TURN_LIGHT_ACTUAL_LEFT_TURNLAMPSTS_ON = 1; + TURN_LIGHT_ACTUAL_RIGHT_TURNLAMPSTS_ON = 2; + TURN_LIGHT_ACTUAL_HAZARD_WARNING_LAMPSTS_ON = 3; + } + enum Drive_mode_stsType { + DRIVE_MODE_STS_THROTTLE_PADDLE_DRIVE_MODE = 0; + DRIVE_MODE_STS_SPEED_DRIVE_MODE = 1; + } + enum Steer_mode_stsType { + STEER_MODE_STS_STANDARD_STEER_MODE = 0; + STEER_MODE_STS_NON_DIRECTION_STEER_MODE = 1; + STEER_MODE_STS_SYNC_DIRECTION_STEER_MODE = 2; + } + enum Frontcrash_stateType { + FRONTCRASH_STATE_NO_EVENT = 0; + FRONTCRASH_STATE_CRASH_EVENT = 1; + } + enum Backcrash_stateType { + BACKCRASH_STATE_NO_EVENT = 0; + BACKCRASH_STATE_CRASH_EVENT = 1; + } + enum Aeb_brake_stateType { + AEB_BRAKE_STATE_INACTIVE = 0; + AEB_BRAKE_STATE_ACTIVE = 1; + } + // [] [0|0] + optional Vehicle_mode_stateType vehicle_mode_state = 1; + // describle the vehicle AEB mode whether was set [] [0|1] + optional Aeb_modeType aeb_mode = 2; + // [] [0|1] + optional Brake_light_actualType brake_light_actual = 3; + // [] [0|0] + optional Turn_light_actualType turn_light_actual = 4; + // [] [0|255] + optional int32 chassis_errcode = 5; + // [] [0|7] + optional Drive_mode_stsType drive_mode_sts = 6; + // [] [0|7] + optional Steer_mode_stsType steer_mode_sts = 7; + // [] [0|0] + optional Frontcrash_stateType frontcrash_state = 8; + // [] [0|0] + optional Backcrash_stateType backcrash_state = 9; + // describe the vehicle e-brake command whether was triggered by AEB [] [0|0] + optional Aeb_brake_stateType aeb_brake_state = 10; + // [m/s^2] [-10|10] + optional double acc = 11; + // speed [m/s] [-32.768|32.767] + optional double speed = 12; +} + +message Wheelspeed_report_506 { +// Report Message + // [m/s] [0|65.535] + optional double rr = 1; + // [m/s] [0|65.535] + optional double rl = 2; + // [m/s] [0|65.535] + optional double fr = 3; + // [m/s] [0|65.535] + optional double fl = 4; +} + +message Ultr_sensor_1_507 { +// Report Message + // [cm] [0|65535] + optional double uiuss9_tof_direct = 1; + // [cm] [0|65535] + optional double uiuss8_tof_direct = 2; + // [cm] [0|65535] + optional double uiuss11_tof_direct = 3; + // [cm] [0|65535] + optional double uiuss10_tof_direct = 4; +} + +message Ultr_sensor_2_508 { +// Report Message + // [cm] [0|65535] + optional double uiuss9_tof_indirect = 1; + // [cm] [0|65535] + optional double uiuss8_tof_indirect = 2; + // [cm] [0|65535] + optional double uiuss11_tof_indirect = 3; + // [cm] [0|65535] + optional double uiuss10_tof_indirect = 4; +} + +message Ultr_sensor_3_509 { +// Report Message + // [cm] [0|65535] + optional double uiuss5_tof_direct = 1; + // [cm] [0|65535] + optional double uiuss4_tof_direct = 2; + // [cm] [0|65535] + optional double uiuss3_tof_direct = 3; + // [cm] [0|65535] + optional double uiuss2_tof_direct = 4; +} + +message Ultr_sensor_4_510 { +// Report Message + // [cm] [0|65535] + optional double uiuss5_tof_indirect = 1; + // [cm] [0|65535] + optional double uiuss4_tof_indirect = 2; + // [cm] [0|65535] + optional double uiuss3_tof_indirect = 3; + // [cm] [0|65535] + optional double uiuss2_tof_indirect = 4; +} + +message Ultr_sensor_5_511 { +// Report Message + // [cm] [0|65535] + optional double uiuss7_tof_direct = 1; + // [cm] [0|65535] + optional double uiuss6_tof_direct = 2; + // [cm] [0|65535] + optional double uiuss1_tof_direct = 3; + // [cm] [0|65535] + optional double uiuss0_tof_direct = 4; +} + +message Bms_report_512 { +// Report Message + enum Battery_flt_overtempType { + BATTERY_FLT_OVERTEMP_NO = 0; + BATTERY_FLT_OVERTEMP_FAULT = 1; + } + enum Battery_flt_lowtempType { + BATTERY_FLT_LOWTEMP_NO = 0; + BATTERY_FLT_LOWTEMP_FAULT = 1; + } + // [] [0|15] + optional Battery_flt_overtempType battery_flt_overtemp = 1; + // [] [0|15] + optional Battery_flt_lowtempType battery_flt_lowtemp = 2; + // Battery Total Voltage [] [-40|215] + optional int32 battery_inside_temperature = 3; + // Battery Total Current [A] [-3200|3353.5] + optional double battery_current = 4; + // Battery Total Voltage [V] [0|300] + optional double battery_voltage = 5; + // Battery Soc percentage [%] [0|100] + optional int32 battery_soc = 6; +} + +message Vin_resp1_514 { +// Report Message + // [] [0|255] + optional int32 vin07 = 1; + // [] [0|255] + optional int32 vin06 = 2; + // [] [0|255] + optional int32 vin05 = 3; + // [] [0|255] + optional int32 vin04 = 4; + // [] [0|255] + optional int32 vin03 = 5; + // [] [0|255] + optional int32 vin02 = 6; + // [] [0|255] + optional int32 vin01 = 7; + // [] [0|255] + optional int32 vin00 = 8; +} + +message Vin_resp2_515 { +// Report Message + // [] [0|255] + optional int32 vin15 = 1; + // [] [0|255] + optional int32 vin14 = 2; + // [] [0|255] + optional int32 vin13 = 3; + // [] [0|255] + optional int32 vin12 = 4; + // [] [0|255] + optional int32 vin11 = 5; + // [] [0|255] + optional int32 vin10 = 6; + // [] [0|255] + optional int32 vin09 = 7; + // [] [0|255] + optional int32 vin08 = 8; +} + +message Vin_resp3_516 { +// Report Message + // [] [0|255] + optional int32 vin16 = 1; +} + +message Demo { + optional Throttle_command_100 throttle_command_100 = 1; // control message + optional Brake_command_101 brake_command_101 = 2; // control message + optional Steering_command_102 steering_command_102 = 3; // control message + optional Gear_command_103 gear_command_103 = 4; // control message + optional Park_command_104 park_command_104 = 5; // control message + optional Vehicle_mode_command_105 vehicle_mode_command_105 = 6; // control message + optional Throttle_report_500 throttle_report_500 = 7; // report message + optional Brake_report_501 brake_report_501 = 8; // report message + optional Steering_report_502 steering_report_502 = 9; // report message + optional Gear_report_503 gear_report_503 = 10; // report message + optional Park_report_504 park_report_504 = 11; // report message + optional Vcu_report_505 vcu_report_505 = 12; // report message + optional Wheelspeed_report_506 wheelspeed_report_506 = 13; // report message + optional Ultr_sensor_1_507 ultr_sensor_1_507 = 14; // report message + optional Ultr_sensor_2_508 ultr_sensor_2_508 = 15; // report message + optional Ultr_sensor_3_509 ultr_sensor_3_509 = 16; // report message + optional Ultr_sensor_4_510 ultr_sensor_4_510 = 17; // report message + optional Ultr_sensor_5_511 ultr_sensor_5_511 = 18; // report message + optional Bms_report_512 bms_report_512 = 19; // report message + optional Vin_resp1_514 vin_resp1_514 = 20; // report message + optional Vin_resp2_515 vin_resp2_515 = 21; // report message + optional Vin_resp3_516 vin_resp3_516 = 22; // report message +} diff --git a/modules/canbus_vehicle/demo/protocol/bms_report_512.cc b/modules/canbus_vehicle/demo/protocol/bms_report_512.cc new file mode 100644 index 00000000000..edc43c8e2f2 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/bms_report_512.cc @@ -0,0 +1,139 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/bms_report_512.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Bmsreport512::Bmsreport512() {} +const int32_t Bmsreport512::ID = 0x512; + +void Bmsreport512::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_bms_report_512()->set_battery_flt_overtemp( + battery_flt_overtemp(bytes, length)); + chassis->mutable_bms_report_512()->set_battery_flt_lowtemp( + battery_flt_lowtemp(bytes, length)); + chassis->mutable_bms_report_512()->set_battery_inside_temperature( + battery_inside_temperature(bytes, length)); + chassis->mutable_bms_report_512()->set_battery_current( + battery_current(bytes, length)); + chassis->mutable_bms_report_512()->set_battery_voltage( + battery_voltage(bytes, length)); + chassis->mutable_bms_report_512()->set_battery_soc( + battery_soc(bytes, length)); +} + +// config detail: {'bit': 49, 'enum': {0: 'BATTERY_FLT_OVERTEMP_NO', 1: +// 'BATTERY_FLT_OVERTEMP_FAULT'}, 'is_signed_var': False, 'len': 1, 'name': +// 'battery_flt_overtemp', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Bms_report_512::Battery_flt_overtempType Bmsreport512::battery_flt_overtemp( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(1, 1); + + Bms_report_512::Battery_flt_overtempType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 48, 'enum': {0: 'BATTERY_FLT_LOWTEMP_NO', 1: +// 'BATTERY_FLT_LOWTEMP_FAULT'}, 'is_signed_var': False, 'len': 1, 'name': +// 'battery_flt_lowtemp', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Bms_report_512::Battery_flt_lowtempType Bmsreport512::battery_flt_lowtemp( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 1); + + Bms_report_512::Battery_flt_lowtempType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 47, 'description': 'Battery Total Voltage', +// 'is_signed_var': False, 'len': 8, 'name': 'battery_inside_temperature', +// 'offset': -40.0, 'order': 'motorola', 'physical_range': '[-40|215]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Bmsreport512::battery_inside_temperature(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + int ret = x + -40.000000; + return ret; +} + +// config detail: {'bit': 23, 'description': 'Battery Total Current', +// 'is_signed_var': False, 'len': 16, 'name': 'battery_current', 'offset': +// -3200.0, 'order': 'motorola', 'physical_range': '[-3200|3353.5]', +// 'physical_unit': 'A', 'precision': 0.1, 'type': 'double'} +double Bmsreport512::battery_current(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.100000 + -3200.000000; + return ret; +} + +// config detail: {'bit': 7, 'description': 'Battery Total Voltage', +// 'is_signed_var': False, 'len': 16, 'name': 'battery_voltage', 'offset': 0.0, +// 'order': 'motorola', 'physical_range': '[0|300]', 'physical_unit': 'V', +// 'precision': 0.01, 'type': 'double'} +double Bmsreport512::battery_voltage(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.010000; + return ret; +} + +// config detail: {'bit': 39, 'description': 'Battery Soc percentage', +// 'is_signed_var': False, 'len': 8, 'name': 'battery_soc', 'offset': 0.0, +// 'order': 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', +// 'precision': 1.0, 'type': 'int'} +int Bmsreport512::battery_soc(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/bms_report_512.h b/modules/canbus_vehicle/demo/protocol/bms_report_512.h new file mode 100644 index 00000000000..c4a83e7e375 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/bms_report_512.h @@ -0,0 +1,80 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Bmsreport512 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Bmsreport512(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 49, 'enum': {0: 'BATTERY_FLT_OVERTEMP_NO', 1: + // 'BATTERY_FLT_OVERTEMP_FAULT'}, 'is_signed_var': False, 'len': 1, 'name': + // 'Battery_Flt_OverTemp', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Bms_report_512::Battery_flt_overtempType battery_flt_overtemp( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 48, 'enum': {0: 'BATTERY_FLT_LOWTEMP_NO', 1: + // 'BATTERY_FLT_LOWTEMP_FAULT'}, 'is_signed_var': False, 'len': 1, 'name': + // 'Battery_Flt_LowTemp', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Bms_report_512::Battery_flt_lowtempType battery_flt_lowtemp( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 47, 'description': 'Battery Total Voltage', + // 'is_signed_var': False, 'len': 8, 'name': 'Battery_Inside_Temperature', + // 'offset': -40.0, 'order': 'motorola', 'physical_range': '[-40|215]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int battery_inside_temperature(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'description': 'Battery Total Current', + // 'is_signed_var': False, 'len': 16, 'name': 'Battery_Current', 'offset': + // -3200.0, 'order': 'motorola', 'physical_range': '[-3200|3353.5]', + // 'physical_unit': 'A', 'precision': 0.1, 'type': 'double'} + double battery_current(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 7, 'description': 'Battery Total Voltage', + // 'is_signed_var': False, 'len': 16, 'name': 'Battery_Voltage', 'offset': + // 0.0, 'order': 'motorola', 'physical_range': '[0|300]', 'physical_unit': + // 'V', 'precision': 0.01, 'type': 'double'} + double battery_voltage(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 39, 'description': 'Battery Soc percentage', + // 'is_signed_var': False, 'len': 8, 'name': 'Battery_Soc', 'offset': 0.0, + // 'order': 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', + // 'precision': 1.0, 'type': 'int'} + int battery_soc(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/brake_command_101.cc b/modules/canbus_vehicle/demo/protocol/brake_command_101.cc new file mode 100644 index 00000000000..ae2e7c29def --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/brake_command_101.cc @@ -0,0 +1,267 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/brake_command_101.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Brakecommand101::ID = 0x101; + +// public +Brakecommand101::Brakecommand101() { Reset(); } + +uint32_t Brakecommand101::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Brakecommand101::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_brake_command_101()->set_heartbeat_101( + heartbeat_101(bytes, length)); + chassis->mutable_brake_command_101()->set_aeb_en_ctrl( + aeb_en_ctrl(bytes, length)); + chassis->mutable_brake_command_101()->set_brake_dec(brake_dec(bytes, length)); + chassis->mutable_brake_command_101()->set_checksum_101( + checksum_101(bytes, length)); + chassis->mutable_brake_command_101()->set_brake_pedal_target( + brake_pedal_target(bytes, length)); + chassis->mutable_brake_command_101()->set_brake_en_ctrl( + brake_en_ctrl(bytes, length)); +} + +void Brakecommand101::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_101_; + heartbeat_101_ = (heartbeat_101_) % 16; + set_p_heartbeat_101(data, heartbeat_101_); + checksum_101_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_101(data, checksum_101_); +} + +void Brakecommand101::UpdateData(uint8_t* data) { + set_p_aeb_en_ctrl(data, aeb_en_ctrl_); + 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_); +} + +void Brakecommand101::Reset() { + // TODO(All) : you should check this manually + heartbeat_101_ = 0; + aeb_en_ctrl_ = Brake_command_101::AEB_EN_CTRL_DISABLE_AEB; + brake_dec_ = 0.0; + checksum_101_ = 0; + brake_pedal_target_ = 0.0; + brake_en_ctrl_ = Brake_command_101::BRAKE_EN_CTRL_DISABLE; +} + +Brakecommand101* Brakecommand101::set_heartbeat_101(int heartbeat_101) { + heartbeat_101_ = heartbeat_101; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Brakecommand101::set_p_heartbeat_101(uint8_t* data, int heartbeat_101) { + heartbeat_101 = ProtocolData::BoundedValue(0, 15, heartbeat_101); + int x = heartbeat_101; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Brakecommand101* Brakecommand101::set_aeb_en_ctrl( + Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl) { + aeb_en_ctrl_ = aeb_en_ctrl; + return this; +} + +// config detail: {'bit': 1, 'enum': {0: 'AEB_EN_CTRL_DISABLE_AEB', 1: +// 'AEB_EN_CTRL_ENABLE_AEB'}, 'is_signed_var': False, 'len': 1, 'name': +// 'AEB_EN_CTRL', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|0]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +void Brakecommand101::set_p_aeb_en_ctrl( + uint8_t* data, Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl) { + int x = aeb_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 1, 1); +} + +Brakecommand101* Brakecommand101::set_brake_dec(double brake_dec) { + brake_dec_ = brake_dec; + return this; +} + +// config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': +// 'Brake_Dec', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|10]', +// 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} +void Brakecommand101::set_p_brake_dec(uint8_t* data, double brake_dec) { + brake_dec = ProtocolData::BoundedValue(0.0, 10.0, brake_dec); + int x = brake_dec / 0.010000; + uint8_t t = 0; + + t = x & 0x3; + Byte to_set0(data + 2); + to_set0.set_value(t, 6, 2); + x >>= 2; + + t = x & 0xFF; + Byte to_set1(data + 1); + to_set1.set_value(t, 0, 8); +} + +Brakecommand101* Brakecommand101::set_checksum_101(int checksum_101) { + checksum_101_ = checksum_101; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Brakecommand101::set_p_checksum_101(uint8_t* data, int checksum_101) { + checksum_101 = ProtocolData::BoundedValue(0, 255, checksum_101); + int x = checksum_101; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +Brakecommand101* Brakecommand101::set_brake_pedal_target( + double brake_pedal_target) { + brake_pedal_target_ = brake_pedal_target; + return this; +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'Brake_Pedal_Target', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': 0.1, +// 'signal_type': 'command', 'type': 'double'} +void Brakecommand101::set_p_brake_pedal_target(uint8_t* data, + double brake_pedal_target) { + brake_pedal_target = + ProtocolData::BoundedValue(0.0, 100.0, brake_pedal_target); + int x = brake_pedal_target / 0.100000; + uint8_t t = 0; + + t = x & 0xFF; + Byte to_set0(data + 4); + to_set0.set_value(t, 0, 8); + x >>= 8; + + t = x & 0xFF; + Byte to_set1(data + 3); + to_set1.set_value(t, 0, 8); +} + +Brakecommand101* Brakecommand101::set_brake_en_ctrl( + Brake_command_101::Brake_en_ctrlType brake_en_ctrl) { + brake_en_ctrl_ = brake_en_ctrl; + return this; +} + +// config detail: {'bit': 0, 'description': 'enable', 'enum': {0: +// 'BRAKE_EN_CTRL_DISABLE', 1: 'BRAKE_EN_CTRL_ENABLE'}, 'is_signed_var': False, +// 'len': 1, 'name': 'Brake_EN_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'signal_type': 'enable', +// 'type': 'enum'} +void Brakecommand101::set_p_brake_en_ctrl( + uint8_t* data, Brake_command_101::Brake_en_ctrlType brake_en_ctrl) { + int x = brake_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 1); +} + +int Brakecommand101::heartbeat_101(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +Brake_command_101::Aeb_en_ctrlType Brakecommand101::aeb_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(1, 1); + + Brake_command_101::Aeb_en_ctrlType ret = + static_cast(x); + return ret; +} + +double Brakecommand101::brake_dec(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 2); + int32_t t = t1.get_byte(6, 2); + x <<= 2; + x |= t; + + double ret = x * 0.010000; + return ret; +} + +int Brakecommand101::checksum_101(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +double Brakecommand101::brake_pedal_target(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.100000; + return ret; +} + +Brake_command_101::Brake_en_ctrlType Brakecommand101::brake_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Brake_command_101::Brake_en_ctrlType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/brake_command_101.h b/modules/canbus_vehicle/demo/protocol/brake_command_101.h new file mode 100644 index 00000000000..a0ae3190111 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/brake_command_101.h @@ -0,0 +1,144 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Brakecommand101 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Brakecommand101(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Brakecommand101* set_heartbeat_101(int heartbeat_101); + + // config detail: {'bit': 1, 'enum': {0: 'AEB_EN_CTRL_DISABLE_AEB', 1: + // 'AEB_EN_CTRL_ENABLE_AEB'}, 'is_signed_var': False, 'len': 1, 'name': + // 'AEB_EN_CTRL', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + Brakecommand101* set_aeb_en_ctrl( + Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': + // 'Brake_Dec', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|10]', 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} + Brakecommand101* set_brake_dec(double brake_dec); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Brakecommand101* set_checksum_101(int checksum_101); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Brake_Pedal_Target', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + Brakecommand101* set_brake_pedal_target(double brake_pedal_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'BRAKE_EN_CTRL_DISABLE', 1: 'BRAKE_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Brake_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Brakecommand101* set_brake_en_ctrl( + Brake_command_101::Brake_en_ctrlType brake_en_ctrl); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_101(uint8_t* data, int heartbeat_101); + + // config detail: {'bit': 1, 'enum': {0: 'AEB_EN_CTRL_DISABLE_AEB', 1: + // 'AEB_EN_CTRL_ENABLE_AEB'}, 'is_signed_var': False, 'len': 1, 'name': + // 'AEB_EN_CTRL', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + void set_p_aeb_en_ctrl(uint8_t* data, + Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': + // 'Brake_Dec', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|10]', 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} + void set_p_brake_dec(uint8_t* data, double brake_dec); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_101', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_101(uint8_t* data, int checksum_101); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Brake_Pedal_Target', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + void set_p_brake_pedal_target(uint8_t* data, double brake_pedal_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'BRAKE_EN_CTRL_DISABLE', 1: 'BRAKE_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Brake_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + void set_p_brake_en_ctrl(uint8_t* data, + Brake_command_101::Brake_en_ctrlType brake_en_ctrl); + + int heartbeat_101(const std::uint8_t* bytes, const int32_t length) const; + + Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl(const std::uint8_t* bytes, + const int32_t length) const; + + double brake_dec(const std::uint8_t* bytes, const int32_t length) const; + + int checksum_101(const std::uint8_t* bytes, const int32_t length) const; + + double brake_pedal_target(const std::uint8_t* bytes, + const int32_t length) const; + + Brake_command_101::Brake_en_ctrlType brake_en_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + private: + int heartbeat_101_; + Brake_command_101::Aeb_en_ctrlType aeb_en_ctrl_; + double brake_dec_; + int checksum_101_; + double brake_pedal_target_; + Brake_command_101::Brake_en_ctrlType brake_en_ctrl_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/brake_report_501.cc b/modules/canbus_vehicle/demo/protocol/brake_report_501.cc new file mode 100644 index 00000000000..476d10ab049 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/brake_report_501.cc @@ -0,0 +1,111 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/brake_report_501.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Brakereport501::Brakereport501() {} +const int32_t Brakereport501::ID = 0x501; + +void Brakereport501::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_brake_report_501()->set_brake_pedal_actual( + brake_pedal_actual(bytes, length)); + chassis->mutable_brake_report_501()->set_brake_flt2( + brake_flt2(bytes, length)); + chassis->mutable_brake_report_501()->set_brake_flt1( + brake_flt1(bytes, length)); + chassis->mutable_brake_report_501()->set_brake_en_state( + brake_en_state(bytes, length)); +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'brake_pedal_actual', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': 0.1, +// 'signal_type': 'command', 'type': 'double'} +double Brakereport501::brake_pedal_actual(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.100000; + return ret; +} + +// config detail: {'bit': 23, 'description': 'Brake system communication fault', +// 'enum': {0: 'BRAKE_FLT2_NO_FAULT', 1: +// 'BRAKE_FLT2_BRAKE_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, 'len': +// 8, 'name': 'brake_flt2', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Brake_report_501::Brake_flt2Type Brakereport501::brake_flt2( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Brake_report_501::Brake_flt2Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 15, 'description': 'Brake system hardware fault', +// 'enum': {0: 'BRAKE_FLT1_NO_FAULT', 1: +// 'BRAKE_FLT1_BRAKE_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, 'len': 8, +// 'name': 'brake_flt1', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Brake_report_501::Brake_flt1Type Brakereport501::brake_flt1( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Brake_report_501::Brake_flt1Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 1, 'description': 'enable', 'enum': {0: +// 'BRAKE_EN_STATE_MANUAL', 1: 'BRAKE_EN_STATE_AUTO', 2: +// 'BRAKE_EN_STATE_TAKEOVER', 3: 'BRAKE_EN_STATE_STANDBY'}, 'is_signed_var': +// False, 'len': 2, 'name': 'brake_en_state', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'enable', 'type': 'enum'} +Brake_report_501::Brake_en_stateType Brakereport501::brake_en_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 2); + + Brake_report_501::Brake_en_stateType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/brake_report_501.h b/modules/canbus_vehicle/demo/protocol/brake_report_501.h new file mode 100644 index 00000000000..39e207ed05d --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/brake_report_501.h @@ -0,0 +1,73 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Brakereport501 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Brakereport501(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Brake_Pedal_Actual', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + double brake_pedal_actual(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'description': 'Brake system communication + // fault', 'enum': {0: 'BRAKE_FLT2_NO_FAULT', 1: + // 'BRAKE_FLT2_BRAKE_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, + // 'len': 8, 'name': 'Brake_FLT2', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Brake_report_501::Brake_flt2Type brake_flt2(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 15, 'description': 'Brake system hardware fault', + // 'enum': {0: 'BRAKE_FLT1_NO_FAULT', 1: + // 'BRAKE_FLT1_BRAKE_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, 'len': + // 8, 'name': 'Brake_FLT1', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Brake_report_501::Brake_flt1Type brake_flt1(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 1, 'description': 'enable', 'enum': {0: + // 'BRAKE_EN_STATE_MANUAL', 1: 'BRAKE_EN_STATE_AUTO', 2: + // 'BRAKE_EN_STATE_TAKEOVER', 3: 'BRAKE_EN_STATE_STANDBY'}, 'is_signed_var': + // False, 'len': 2, 'name': 'Brake_EN_state', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Brake_report_501::Brake_en_stateType brake_en_state( + const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/gear_command_103.cc b/modules/canbus_vehicle/demo/protocol/gear_command_103.cc new file mode 100644 index 00000000000..e877004994a --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/gear_command_103.cc @@ -0,0 +1,184 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/gear_command_103.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Gearcommand103::ID = 0x103; + +// public +Gearcommand103::Gearcommand103() { Reset(); } + +uint32_t Gearcommand103::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Gearcommand103::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_gear_command_103()->set_heartbeat_103( + heartbeat_103(bytes, length)); + chassis->mutable_gear_command_103()->set_gear_target( + gear_target(bytes, length)); + chassis->mutable_gear_command_103()->set_gear_en_ctrl( + gear_en_ctrl(bytes, length)); + chassis->mutable_gear_command_103()->set_checksum_103( + checksum_103(bytes, length)); +} + +void Gearcommand103::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_103_; + heartbeat_103_ = (heartbeat_103_) % 16; + set_p_heartbeat_103(data, heartbeat_103_); + checksum_103_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_103(data, checksum_103_); +} + +void Gearcommand103::UpdateData(uint8_t* data) { + set_p_gear_target(data, gear_target_); + set_p_gear_en_ctrl(data, gear_en_ctrl_); +} + +void Gearcommand103::Reset() { + // TODO(All) : you should check this manually + heartbeat_103_ = 0; + gear_target_ = Gear_command_103::GEAR_TARGET_INVALID; + gear_en_ctrl_ = Gear_command_103::GEAR_EN_CTRL_DISABLE; + checksum_103_ = 0; +} + +Gearcommand103* Gearcommand103::set_heartbeat_103(int heartbeat_103) { + heartbeat_103_ = heartbeat_103; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Gearcommand103::set_p_heartbeat_103(uint8_t* data, int heartbeat_103) { + heartbeat_103 = ProtocolData::BoundedValue(0, 15, heartbeat_103); + int x = heartbeat_103; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Gearcommand103* Gearcommand103::set_gear_target( + Gear_command_103::Gear_targetType gear_target) { + gear_target_ = gear_target; + return this; +} + +// config detail: {'bit': 10, 'description': 'command', 'enum': {0: +// 'GEAR_TARGET_INVALID', 1: 'GEAR_TARGET_PARK', 2: 'GEAR_TARGET_REVERSE', 3: +// 'GEAR_TARGET_NEUTRAL', 4: 'GEAR_TARGET_DRIVE'}, 'is_signed_var': False, +// 'len': 3, 'name': 'Gear_Target', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|4]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'command', 'type': 'enum'} +void Gearcommand103::set_p_gear_target( + uint8_t* data, Gear_command_103::Gear_targetType gear_target) { + int x = gear_target; + + Byte to_set(data + 1); + to_set.set_value(x, 0, 3); +} + +Gearcommand103* Gearcommand103::set_gear_en_ctrl( + Gear_command_103::Gear_en_ctrlType gear_en_ctrl) { + gear_en_ctrl_ = gear_en_ctrl; + return this; +} + +// config detail: {'bit': 0, 'description': 'enable', 'enum': {0: +// 'GEAR_EN_CTRL_DISABLE', 1: 'GEAR_EN_CTRL_ENABLE'}, 'is_signed_var': False, +// 'len': 1, 'name': 'Gear_EN_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'signal_type': 'enable', +// 'type': 'enum'} +void Gearcommand103::set_p_gear_en_ctrl( + uint8_t* data, Gear_command_103::Gear_en_ctrlType gear_en_ctrl) { + int x = gear_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 1); +} + +Gearcommand103* Gearcommand103::set_checksum_103(int checksum_103) { + checksum_103_ = checksum_103; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Gearcommand103::set_p_checksum_103(uint8_t* data, int checksum_103) { + checksum_103 = ProtocolData::BoundedValue(0, 255, checksum_103); + int x = checksum_103; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +int Gearcommand103::heartbeat_103(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +Gear_command_103::Gear_targetType Gearcommand103::gear_target( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 3); + + Gear_command_103::Gear_targetType ret = + static_cast(x); + return ret; +} + +Gear_command_103::Gear_en_ctrlType Gearcommand103::gear_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Gear_command_103::Gear_en_ctrlType ret = + static_cast(x); + return ret; +} + +int Gearcommand103::checksum_103(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/gear_command_103.h b/modules/canbus_vehicle/demo/protocol/gear_command_103.h new file mode 100644 index 00000000000..20914c4c19e --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/gear_command_103.h @@ -0,0 +1,119 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Gearcommand103 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Gearcommand103(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Gearcommand103* set_heartbeat_103(int heartbeat_103); + + // config detail: {'bit': 10, 'description': 'command', 'enum': {0: + // 'GEAR_TARGET_INVALID', 1: 'GEAR_TARGET_PARK', 2: 'GEAR_TARGET_REVERSE', 3: + // 'GEAR_TARGET_NEUTRAL', 4: 'GEAR_TARGET_DRIVE'}, 'is_signed_var': False, + // 'len': 3, 'name': 'Gear_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|4]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'command', 'type': 'enum'} + Gearcommand103* set_gear_target( + Gear_command_103::Gear_targetType gear_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'GEAR_EN_CTRL_DISABLE', 1: 'GEAR_EN_CTRL_ENABLE'}, 'is_signed_var': False, + // 'len': 1, 'name': 'Gear_EN_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'enable', 'type': 'enum'} + Gearcommand103* set_gear_en_ctrl( + Gear_command_103::Gear_en_ctrlType gear_en_ctrl); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Gearcommand103* set_checksum_103(int checksum_103); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_103(uint8_t* data, int heartbeat_103); + + // config detail: {'bit': 10, 'description': 'command', 'enum': {0: + // 'GEAR_TARGET_INVALID', 1: 'GEAR_TARGET_PARK', 2: 'GEAR_TARGET_REVERSE', 3: + // 'GEAR_TARGET_NEUTRAL', 4: 'GEAR_TARGET_DRIVE'}, 'is_signed_var': False, + // 'len': 3, 'name': 'Gear_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|4]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'command', 'type': 'enum'} + void set_p_gear_target(uint8_t* data, + Gear_command_103::Gear_targetType gear_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'GEAR_EN_CTRL_DISABLE', 1: 'GEAR_EN_CTRL_ENABLE'}, 'is_signed_var': False, + // 'len': 1, 'name': 'Gear_EN_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'enable', 'type': 'enum'} + void set_p_gear_en_ctrl(uint8_t* data, + Gear_command_103::Gear_en_ctrlType gear_en_ctrl); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_103', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_103(uint8_t* data, int checksum_103); + + int heartbeat_103(const std::uint8_t* bytes, const int32_t length) const; + + Gear_command_103::Gear_targetType gear_target(const std::uint8_t* bytes, + const int32_t length) const; + + Gear_command_103::Gear_en_ctrlType gear_en_ctrl(const std::uint8_t* bytes, + const int32_t length) const; + + int checksum_103(const std::uint8_t* bytes, const int32_t length) const; + + private: + int heartbeat_103_; + Gear_command_103::Gear_targetType gear_target_; + Gear_command_103::Gear_en_ctrlType gear_en_ctrl_; + int checksum_103_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/gear_report_503.cc b/modules/canbus_vehicle/demo/protocol/gear_report_503.cc new file mode 100644 index 00000000000..a34c674d9d9 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/gear_report_503.cc @@ -0,0 +1,71 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/gear_report_503.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Gearreport503::Gearreport503() {} +const int32_t Gearreport503::ID = 0x503; + +void Gearreport503::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_gear_report_503()->set_gear_flt(gear_flt(bytes, length)); + chassis->mutable_gear_report_503()->set_gear_actual( + gear_actual(bytes, length)); +} + +// config detail: {'bit': 15, 'description': 'fault', 'enum': {0: +// 'GEAR_FLT_NO_FAULT', 1: 'GEAR_FLT_FAULT'}, 'is_signed_var': False, 'len': 8, +// 'name': 'gear_flt', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Gear_report_503::Gear_fltType Gearreport503::gear_flt(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Gear_report_503::Gear_fltType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 2, 'description': 'command', 'enum': {0: +// 'GEAR_ACTUAL_INVALID', 1: 'GEAR_ACTUAL_PARK', 2: 'GEAR_ACTUAL_REVERSE', 3: +// 'GEAR_ACTUAL_NEUTRAL', 4: 'GEAR_ACTUAL_DRIVE'}, 'is_signed_var': False, +// 'len': 3, 'name': 'gear_actual', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|4]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'command', 'type': 'enum'} +Gear_report_503::Gear_actualType Gearreport503::gear_actual( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 3); + + Gear_report_503::Gear_actualType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/gear_report_503.h b/modules/canbus_vehicle/demo/protocol/gear_report_503.h new file mode 100644 index 00000000000..8fe49d20c21 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/gear_report_503.h @@ -0,0 +1,56 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Gearreport503 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Gearreport503(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 15, 'description': 'fault', 'enum': {0: + // 'GEAR_FLT_NO_FAULT', 1: 'GEAR_FLT_FAULT'}, 'is_signed_var': False, 'len': + // 8, 'name': 'Gear_FLT', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Gear_report_503::Gear_fltType gear_flt(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 2, 'description': 'command', 'enum': {0: + // 'GEAR_ACTUAL_INVALID', 1: 'GEAR_ACTUAL_PARK', 2: 'GEAR_ACTUAL_REVERSE', 3: + // 'GEAR_ACTUAL_NEUTRAL', 4: 'GEAR_ACTUAL_DRIVE'}, 'is_signed_var': False, + // 'len': 3, 'name': 'Gear_Actual', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|4]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'command', 'type': 'enum'} + Gear_report_503::Gear_actualType gear_actual(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/park_command_104.cc b/modules/canbus_vehicle/demo/protocol/park_command_104.cc new file mode 100644 index 00000000000..d139aa2467c --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/park_command_104.cc @@ -0,0 +1,182 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/park_command_104.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Parkcommand104::ID = 0x104; + +// public +Parkcommand104::Parkcommand104() { Reset(); } + +uint32_t Parkcommand104::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Parkcommand104::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_park_command_104()->set_heartbeat_104( + heartbeat_104(bytes, length)); + chassis->mutable_park_command_104()->set_checksum_104( + checksum_104(bytes, length)); + chassis->mutable_park_command_104()->set_park_target( + park_target(bytes, length)); + chassis->mutable_park_command_104()->set_park_en_ctrl( + park_en_ctrl(bytes, length)); +} + +void Parkcommand104::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_104_; + heartbeat_104_ = (heartbeat_104_) % 16; + set_p_heartbeat_104(data, heartbeat_104_); + checksum_104_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_104(data, checksum_104_); +} + +void Parkcommand104::UpdateData(uint8_t* data) { + set_p_park_target(data, park_target_); + set_p_park_en_ctrl(data, park_en_ctrl_); +} + +void Parkcommand104::Reset() { + // TODO(All) : you should check this manually + heartbeat_104_ = 0; + checksum_104_ = 0; + park_target_ = Park_command_104::PARK_TARGET_RELEASE; + park_en_ctrl_ = Park_command_104::PARK_EN_CTRL_DISABLE; +} + +Parkcommand104* Parkcommand104::set_heartbeat_104(int heartbeat_104) { + heartbeat_104_ = heartbeat_104; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Parkcommand104::set_p_heartbeat_104(uint8_t* data, int heartbeat_104) { + heartbeat_104 = ProtocolData::BoundedValue(0, 15, heartbeat_104); + int x = heartbeat_104; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Parkcommand104* Parkcommand104::set_checksum_104(int checksum_104) { + checksum_104_ = checksum_104; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Parkcommand104::set_p_checksum_104(uint8_t* data, int checksum_104) { + checksum_104 = ProtocolData::BoundedValue(0, 255, checksum_104); + int x = checksum_104; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +Parkcommand104* Parkcommand104::set_park_target( + Park_command_104::Park_targetType park_target) { + park_target_ = park_target; + return this; +} + +// config detail: {'bit': 8, 'description': 'command', 'enum': {0: +// 'PARK_TARGET_RELEASE', 1: 'PARK_TARGET_PARKING_TRIGGER'}, 'is_signed_var': +// False, 'len': 1, 'name': 'Park_Target', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'command', 'type': 'enum'} +void Parkcommand104::set_p_park_target( + uint8_t* data, Park_command_104::Park_targetType park_target) { + int x = park_target; + + Byte to_set(data + 1); + to_set.set_value(x, 0, 1); +} + +Parkcommand104* Parkcommand104::set_park_en_ctrl( + Park_command_104::Park_en_ctrlType park_en_ctrl) { + park_en_ctrl_ = park_en_ctrl; + return this; +} + +// config detail: {'bit': 0, 'description': 'enable', 'enum': {0: +// 'PARK_EN_CTRL_DISABLE', 1: 'PARK_EN_CTRL_ENABLE'}, 'is_signed_var': False, +// 'len': 1, 'name': 'Park_EN_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'enable', 'type': 'enum'} +void Parkcommand104::set_p_park_en_ctrl( + uint8_t* data, Park_command_104::Park_en_ctrlType park_en_ctrl) { + int x = park_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 1); +} + +int Parkcommand104::heartbeat_104(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +int Parkcommand104::checksum_104(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +Park_command_104::Park_targetType Parkcommand104::park_target( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 1); + + Park_command_104::Park_targetType ret = + static_cast(x); + return ret; +} + +Park_command_104::Park_en_ctrlType Parkcommand104::park_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Park_command_104::Park_en_ctrlType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/park_command_104.h b/modules/canbus_vehicle/demo/protocol/park_command_104.h new file mode 100644 index 00000000000..fa7c472ef98 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/park_command_104.h @@ -0,0 +1,117 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Parkcommand104 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Parkcommand104(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Parkcommand104* set_heartbeat_104(int heartbeat_104); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Parkcommand104* set_checksum_104(int checksum_104); + + // config detail: {'bit': 8, 'description': 'command', 'enum': {0: + // 'PARK_TARGET_RELEASE', 1: 'PARK_TARGET_PARKING_TRIGGER'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Park_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'command', 'type': 'enum'} + Parkcommand104* set_park_target( + Park_command_104::Park_targetType park_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'PARK_EN_CTRL_DISABLE', 1: 'PARK_EN_CTRL_ENABLE'}, 'is_signed_var': False, + // 'len': 1, 'name': 'Park_EN_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'enable', 'type': 'enum'} + Parkcommand104* set_park_en_ctrl( + Park_command_104::Park_en_ctrlType park_en_ctrl); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_104(uint8_t* data, int heartbeat_104); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_104', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_104(uint8_t* data, int checksum_104); + + // config detail: {'bit': 8, 'description': 'command', 'enum': {0: + // 'PARK_TARGET_RELEASE', 1: 'PARK_TARGET_PARKING_TRIGGER'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Park_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'command', 'type': 'enum'} + void set_p_park_target(uint8_t* data, + Park_command_104::Park_targetType park_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'PARK_EN_CTRL_DISABLE', 1: 'PARK_EN_CTRL_ENABLE'}, 'is_signed_var': False, + // 'len': 1, 'name': 'Park_EN_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, + // 'signal_type': 'enable', 'type': 'enum'} + void set_p_park_en_ctrl(uint8_t* data, + Park_command_104::Park_en_ctrlType park_en_ctrl); + + int heartbeat_104(const std::uint8_t* bytes, const int32_t length) const; + + int checksum_104(const std::uint8_t* bytes, const int32_t length) const; + + Park_command_104::Park_targetType park_target(const std::uint8_t* bytes, + const int32_t length) const; + + Park_command_104::Park_en_ctrlType park_en_ctrl(const std::uint8_t* bytes, + const int32_t length) const; + + private: + int heartbeat_104_; + int checksum_104_; + Park_command_104::Park_targetType park_target_; + Park_command_104::Park_en_ctrlType park_en_ctrl_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/park_report_504.cc b/modules/canbus_vehicle/demo/protocol/park_report_504.cc new file mode 100644 index 00000000000..98b2473764e --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/park_report_504.cc @@ -0,0 +1,70 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/park_report_504.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Parkreport504::Parkreport504() {} +const int32_t Parkreport504::ID = 0x504; + +void Parkreport504::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_park_report_504()->set_parking_actual( + parking_actual(bytes, length)); + chassis->mutable_park_report_504()->set_park_flt(park_flt(bytes, length)); +} + +// config detail: {'bit': 0, 'description': 'command', 'enum': {0: +// 'PARKING_ACTUAL_RELEASE', 1: 'PARKING_ACTUAL_PARKING_TRIGGER'}, +// 'is_signed_var': False, 'len': 1, 'name': 'parking_actual', 'offset': 0.0, +// 'order': 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', +// 'precision': 1.0, 'signal_type': 'command', 'type': 'enum'} +Park_report_504::Parking_actualType Parkreport504::parking_actual( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Park_report_504::Parking_actualType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 15, 'description': 'fault', 'enum': {0: +// 'PARK_FLT_NO_FAULT', 1: 'PARK_FLT_FAULT'}, 'is_signed_var': False, 'len': 8, +// 'name': 'park_flt', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Park_report_504::Park_fltType Parkreport504::park_flt(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Park_report_504::Park_fltType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/park_report_504.h b/modules/canbus_vehicle/demo/protocol/park_report_504.h new file mode 100644 index 00000000000..1c51a4374ad --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/park_report_504.h @@ -0,0 +1,55 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Parkreport504 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Parkreport504(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 0, 'description': 'command', 'enum': {0: + // 'PARKING_ACTUAL_RELEASE', 1: 'PARKING_ACTUAL_PARKING_TRIGGER'}, + // 'is_signed_var': False, 'len': 1, 'name': 'Parking_actual', 'offset': 0.0, + // 'order': 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'command', 'type': 'enum'} + Park_report_504::Parking_actualType parking_actual( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 15, 'description': 'fault', 'enum': {0: + // 'PARK_FLT_NO_FAULT', 1: 'PARK_FLT_FAULT'}, 'is_signed_var': False, 'len': + // 8, 'name': 'Park_FLT', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Park_report_504::Park_fltType park_flt(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/steering_command_102.cc b/modules/canbus_vehicle/demo/protocol/steering_command_102.cc new file mode 100644 index 00000000000..b4c0c72ad11 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/steering_command_102.cc @@ -0,0 +1,228 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/steering_command_102.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Steeringcommand102::ID = 0x102; + +// public +Steeringcommand102::Steeringcommand102() { Reset(); } + +uint32_t Steeringcommand102::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Steeringcommand102::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_steering_command_102()->set_heartbeat_102( + heartbeat_102(bytes, length)); + chassis->mutable_steering_command_102()->set_steer_en_ctrl( + steer_en_ctrl(bytes, length)); + chassis->mutable_steering_command_102()->set_steer_angle_target( + steer_angle_target(bytes, length)); + chassis->mutable_steering_command_102()->set_steer_angle_spd_target( + steer_angle_spd_target(bytes, length)); + chassis->mutable_steering_command_102()->set_checksum_102( + checksum_102(bytes, length)); +} + +void Steeringcommand102::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_102_; + heartbeat_102_ = (heartbeat_102_) % 16; + set_p_heartbeat_102(data, heartbeat_102_); + checksum_102_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_102(data, checksum_102_); +} + +void Steeringcommand102::UpdateData(uint8_t* data) { + set_p_steer_en_ctrl(data, steer_en_ctrl_); + set_p_steer_angle_target(data, steer_angle_target_); + set_p_steer_angle_spd_target(data, steer_angle_spd_target_); +} + +void Steeringcommand102::Reset() { + // TODO(All) : you should check this manually + heartbeat_102_ = 0; + steer_en_ctrl_ = Steering_command_102::STEER_EN_CTRL_DISABLE; + steer_angle_target_ = 0; + steer_angle_spd_target_ = 0; + checksum_102_ = 0; +} + +Steeringcommand102* Steeringcommand102::set_heartbeat_102(int heartbeat_102) { + heartbeat_102_ = heartbeat_102; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Steeringcommand102::set_p_heartbeat_102(uint8_t* data, int heartbeat_102) { + heartbeat_102 = ProtocolData::BoundedValue(0, 15, heartbeat_102); + int x = heartbeat_102; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Steeringcommand102* Steeringcommand102::set_steer_en_ctrl( + Steering_command_102::Steer_en_ctrlType steer_en_ctrl) { + steer_en_ctrl_ = steer_en_ctrl; + return this; +} + +// config detail: {'bit': 0, 'description': 'enable', 'enum': {0: +// 'STEER_EN_CTRL_DISABLE', 1: 'STEER_EN_CTRL_ENABLE'}, 'is_signed_var': False, +// 'len': 1, 'name': 'Steer_EN_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': +// '[0|1]', 'physical_unit': '', 'precision': 1.0, 'signal_type': 'enable', +// 'type': 'enum'} +void Steeringcommand102::set_p_steer_en_ctrl( + uint8_t* data, Steering_command_102::Steer_en_ctrlType steer_en_ctrl) { + int x = steer_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 1); +} + +Steeringcommand102* Steeringcommand102::set_steer_angle_target( + int steer_angle_target) { + steer_angle_target_ = steer_angle_target; + return this; +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'Steer_ANGLE_Target', 'offset': -500.0, 'order': +// 'motorola', 'physical_range': '[-500|500]', 'physical_unit': 'deg', +// 'precision': 1.0, 'signal_type': 'command', 'type': 'int'} +void Steeringcommand102::set_p_steer_angle_target(uint8_t* data, + int steer_angle_target) { + steer_angle_target = + ProtocolData::BoundedValue(-500, 500, steer_angle_target); + int x = (steer_angle_target - -500.000000); + uint8_t t = 0; + + t = x & 0xFF; + Byte to_set0(data + 4); + to_set0.set_value(t, 0, 8); + x >>= 8; + + t = x & 0xFF; + Byte to_set1(data + 3); + to_set1.set_value(t, 0, 8); +} + +Steeringcommand102* Steeringcommand102::set_steer_angle_spd_target( + int steer_angle_spd_target) { + steer_angle_spd_target_ = steer_angle_spd_target; + return this; +} + +// config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': +// 'Steer_ANGLE_SPD_Target', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|250]', 'physical_unit': 'deg/s', 'precision': 1.0, +// 'type': 'int'} +void Steeringcommand102::set_p_steer_angle_spd_target( + uint8_t* data, int steer_angle_spd_target) { + steer_angle_spd_target = + ProtocolData::BoundedValue(0, 250, steer_angle_spd_target); + int x = steer_angle_spd_target; + + Byte to_set(data + 1); + to_set.set_value(x, 0, 8); +} + +Steeringcommand102* Steeringcommand102::set_checksum_102(int checksum_102) { + checksum_102_ = checksum_102; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Steeringcommand102::set_p_checksum_102(uint8_t* data, int checksum_102) { + checksum_102 = ProtocolData::BoundedValue(0, 255, checksum_102); + int x = checksum_102; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +int Steeringcommand102::heartbeat_102(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +Steering_command_102::Steer_en_ctrlType Steeringcommand102::steer_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Steering_command_102::Steer_en_ctrlType ret = + static_cast(x); + return ret; +} + +int Steeringcommand102::steer_angle_target(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + int ret = x + -500.000000; + return ret; +} + +int Steeringcommand102::steer_angle_spd_target(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +int Steeringcommand102::checksum_102(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/steering_command_102.h b/modules/canbus_vehicle/demo/protocol/steering_command_102.h new file mode 100644 index 00000000000..90100d12d48 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/steering_command_102.h @@ -0,0 +1,128 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Steeringcommand102 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Steeringcommand102(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Steeringcommand102* set_heartbeat_102(int heartbeat_102); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'STEER_EN_CTRL_DISABLE', 1: 'STEER_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Steer_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Steeringcommand102* set_steer_en_ctrl( + Steering_command_102::Steer_en_ctrlType steer_en_ctrl); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Steer_ANGLE_Target', 'offset': -500.0, 'order': + // 'motorola', 'physical_range': '[-500|500]', 'physical_unit': 'deg', + // 'precision': 1.0, 'signal_type': 'command', 'type': 'int'} + Steeringcommand102* set_steer_angle_target(int steer_angle_target); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': + // 'Steer_ANGLE_SPD_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|250]', 'physical_unit': 'deg/s', 'precision': 1.0, + // 'type': 'int'} + Steeringcommand102* set_steer_angle_spd_target(int steer_angle_spd_target); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Steeringcommand102* set_checksum_102(int checksum_102); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_102(uint8_t* data, int heartbeat_102); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'STEER_EN_CTRL_DISABLE', 1: 'STEER_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Steer_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + void set_p_steer_en_ctrl( + uint8_t* data, Steering_command_102::Steer_en_ctrlType steer_en_ctrl); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Steer_ANGLE_Target', 'offset': -500.0, 'order': + // 'motorola', 'physical_range': '[-500|500]', 'physical_unit': 'deg', + // 'precision': 1.0, 'signal_type': 'command', 'type': 'int'} + void set_p_steer_angle_target(uint8_t* data, int steer_angle_target); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': + // 'Steer_ANGLE_SPD_Target', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|250]', 'physical_unit': 'deg/s', 'precision': 1.0, + // 'type': 'int'} + void set_p_steer_angle_spd_target(uint8_t* data, int steer_angle_spd_target); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_102', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_102(uint8_t* data, int checksum_102); + + int heartbeat_102(const std::uint8_t* bytes, const int32_t length) const; + + Steering_command_102::Steer_en_ctrlType steer_en_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + int steer_angle_target(const std::uint8_t* bytes, const int32_t length) const; + + int steer_angle_spd_target(const std::uint8_t* bytes, + const int32_t length) const; + + int checksum_102(const std::uint8_t* bytes, const int32_t length) const; + + private: + int heartbeat_102_; + Steering_command_102::Steer_en_ctrlType steer_en_ctrl_; + int steer_angle_target_; + int steer_angle_spd_target_; + int checksum_102_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/steering_report_502.cc b/modules/canbus_vehicle/demo/protocol/steering_report_502.cc new file mode 100644 index 00000000000..639e1963acf --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/steering_report_502.cc @@ -0,0 +1,146 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/steering_report_502.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Steeringreport502::Steeringreport502() {} +const int32_t Steeringreport502::ID = 0x502; + +void Steeringreport502::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_steering_report_502()->set_steer_angle_rear_actual( + steer_angle_rear_actual(bytes, length)); + chassis->mutable_steering_report_502()->set_steer_angle_spd_actual( + steer_angle_spd_actual(bytes, length)); + chassis->mutable_steering_report_502()->set_steer_flt2( + steer_flt2(bytes, length)); + chassis->mutable_steering_report_502()->set_steer_flt1( + steer_flt1(bytes, length)); + chassis->mutable_steering_report_502()->set_steer_en_state( + steer_en_state(bytes, length)); + chassis->mutable_steering_report_502()->set_steer_angle_actual( + steer_angle_actual(bytes, length)); +} + +// config detail: {'bit': 47, 'is_signed_var': False, 'len': 16, 'name': +// 'steer_angle_rear_actual', 'offset': -500.0, 'order': 'motorola', +// 'physical_range': '[-500|500]', 'physical_unit': 'deg', 'precision': 1.0, +// 'type': 'int'} +int Steeringreport502::steer_angle_rear_actual(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 6); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + int ret = x + -500.000000; + return ret; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'steer_angle_spd_actual', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|0]', 'physical_unit': 'deg/s', 'precision': 1.0, +// 'type': 'int'} +int Steeringreport502::steer_angle_spd_actual(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 23, 'description': 'Steer system communication fault', +// 'enum': {0: 'STEER_FLT2_NO_FAULT', 1: +// 'STEER_FLT2_STEER_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, 'len': +// 8, 'name': 'steer_flt2', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Steering_report_502::Steer_flt2Type Steeringreport502::steer_flt2( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Steering_report_502::Steer_flt2Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 15, 'description': 'Steer system hardware fault', +// 'enum': {0: 'STEER_FLT1_NO_FAULT', 1: +// 'STEER_FLT1_STEER_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, 'len': 8, +// 'name': 'steer_flt1', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Steering_report_502::Steer_flt1Type Steeringreport502::steer_flt1( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Steering_report_502::Steer_flt1Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 1, 'description': 'enable', 'enum': {0: +// 'STEER_EN_STATE_MANUAL', 1: 'STEER_EN_STATE_AUTO', 2: +// 'STEER_EN_STATE_TAKEOVER', 3: 'STEER_EN_STATE_STANDBY'}, 'is_signed_var': +// False, 'len': 2, 'name': 'steer_en_state', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'enable', 'type': 'enum'} +Steering_report_502::Steer_en_stateType Steeringreport502::steer_en_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 2); + + Steering_report_502::Steer_en_stateType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'steer_angle_actual', 'offset': -500.0, 'order': +// 'motorola', 'physical_range': '[-500|500]', 'physical_unit': 'deg', +// 'precision': 1.0, 'signal_type': 'command', 'type': 'int'} +int Steeringreport502::steer_angle_actual(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + int ret = x + -500.000000; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/steering_report_502.h b/modules/canbus_vehicle/demo/protocol/steering_report_502.h new file mode 100644 index 00000000000..ce020252973 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/steering_report_502.h @@ -0,0 +1,86 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Steeringreport502 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Steeringreport502(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 16, 'name': + // 'Steer_ANGLE_Rear_Actual', 'offset': -500.0, 'order': 'motorola', + // 'physical_range': '[-500|500]', 'physical_unit': 'deg', 'precision': 1.0, + // 'type': 'int'} + int steer_angle_rear_actual(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'Steer_ANGLE_SPD_Actual', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|0]', 'physical_unit': 'deg/s', 'precision': 1.0, + // 'type': 'int'} + int steer_angle_spd_actual(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'description': 'Steer system communication + // fault', 'enum': {0: 'STEER_FLT2_NO_FAULT', 1: + // 'STEER_FLT2_STEER_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, + // 'len': 8, 'name': 'Steer_FLT2', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Steering_report_502::Steer_flt2Type steer_flt2(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 15, 'description': 'Steer system hardware fault', + // 'enum': {0: 'STEER_FLT1_NO_FAULT', 1: + // 'STEER_FLT1_STEER_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, 'len': + // 8, 'name': 'Steer_FLT1', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Steering_report_502::Steer_flt1Type steer_flt1(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 1, 'description': 'enable', 'enum': {0: + // 'STEER_EN_STATE_MANUAL', 1: 'STEER_EN_STATE_AUTO', 2: + // 'STEER_EN_STATE_TAKEOVER', 3: 'STEER_EN_STATE_STANDBY'}, 'is_signed_var': + // False, 'len': 2, 'name': 'Steer_EN_state', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Steering_report_502::Steer_en_stateType steer_en_state( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Steer_ANGLE_Actual', 'offset': -500.0, 'order': + // 'motorola', 'physical_range': '[-500|500]', 'physical_unit': 'deg', + // 'precision': 1.0, 'signal_type': 'command', 'type': 'int'} + int steer_angle_actual(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/throttle_command_100.cc b/modules/canbus_vehicle/demo/protocol/throttle_command_100.cc new file mode 100644 index 00000000000..abd606f8367 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/throttle_command_100.cc @@ -0,0 +1,279 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/throttle_command_100.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Throttlecommand100::ID = 0x100; + +// public +Throttlecommand100::Throttlecommand100() { Reset(); } + +uint32_t Throttlecommand100::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Throttlecommand100::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_throttle_command_100()->set_heartbeat_100( + heartbeat_100(bytes, length)); + chassis->mutable_throttle_command_100()->set_speed_target( + speed_target(bytes, length)); + chassis->mutable_throttle_command_100()->set_throttle_acc( + throttle_acc(bytes, length)); + chassis->mutable_throttle_command_100()->set_checksum_100( + checksum_100(bytes, length)); + chassis->mutable_throttle_command_100()->set_throttle_pedal_target( + throttle_pedal_target(bytes, length)); + chassis->mutable_throttle_command_100()->set_throttle_en_ctrl( + throttle_en_ctrl(bytes, length)); +} + +void Throttlecommand100::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_100_; + heartbeat_100_ = (heartbeat_100_) % 16; + set_p_heartbeat_100(data, heartbeat_100_); + checksum_100_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_100(data, checksum_100_); +} + +void Throttlecommand100::UpdateData(uint8_t* data) { + set_p_heartbeat_100(data, heartbeat_100_); + set_p_throttle_acc(data, throttle_acc_); + set_p_throttle_pedal_target(data, throttle_pedal_target_); + set_p_throttle_en_ctrl(data, throttle_en_ctrl_); +} + +void Throttlecommand100::Reset() { + // TODO(All) : you should check this manually + heartbeat_100_ = 0; + speed_target_ = 0.0; + throttle_acc_ = 0.0; + checksum_100_ = 0; + throttle_pedal_target_ = 0.0; + throttle_en_ctrl_ = Throttle_command_100::THROTTLE_EN_CTRL_DISABLE; +} + +Throttlecommand100* Throttlecommand100::set_heartbeat_100(int heartbeat_100) { + heartbeat_100_ = heartbeat_100; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Throttlecommand100::set_p_heartbeat_100(uint8_t* data, int heartbeat_100) { + heartbeat_100 = ProtocolData::BoundedValue(0, 15, heartbeat_100); + int x = heartbeat_100; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Throttlecommand100* Throttlecommand100::set_speed_target(double speed_target) { + speed_target_ = speed_target; + return this; +} + +// config detail: {'bit': 47, 'is_signed_var': False, 'len': 12, 'name': +// 'Speed_Target', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|40.95]', 'physical_unit': 'm/s', 'precision': 0.01, 'type': 'double'} +void Throttlecommand100::set_p_speed_target(uint8_t* data, + double speed_target) { + speed_target = ProtocolData::BoundedValue(0.0, 40.95, speed_target); + int x = speed_target / 0.010000; + uint8_t t = 0; + + t = x & 0xF; + Byte to_set0(data + 6); + to_set0.set_value(t, 4, 4); + x >>= 4; + + t = x & 0xFF; + Byte to_set1(data + 5); + to_set1.set_value(t, 0, 8); +} + +Throttlecommand100* Throttlecommand100::set_throttle_acc(double throttle_acc) { + throttle_acc_ = throttle_acc; + return this; +} + +// config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': +// 'Throttle_Acc', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|10]', 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} +void Throttlecommand100::set_p_throttle_acc(uint8_t* data, + double throttle_acc) { + throttle_acc = ProtocolData::BoundedValue(0.0, 10.0, throttle_acc); + int x = throttle_acc / 0.010000; + uint8_t t = 0; + + t = x & 0x3; + Byte to_set0(data + 2); + to_set0.set_value(t, 6, 2); + x >>= 2; + + t = x & 0xFF; + Byte to_set1(data + 1); + to_set1.set_value(t, 0, 8); +} + +Throttlecommand100* Throttlecommand100::set_checksum_100(int checksum_100) { + checksum_100_ = checksum_100; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Throttlecommand100::set_p_checksum_100(uint8_t* data, int checksum_100) { + checksum_100 = ProtocolData::BoundedValue(0, 255, checksum_100); + int x = checksum_100; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +Throttlecommand100* Throttlecommand100::set_throttle_pedal_target( + double throttle_pedal_target) { + throttle_pedal_target_ = throttle_pedal_target; + return this; +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'Throttle_Pedal_Target', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': +// 0.1, 'signal_type': 'command', 'type': 'double'} +void Throttlecommand100::set_p_throttle_pedal_target( + uint8_t* data, double throttle_pedal_target) { + throttle_pedal_target = + ProtocolData::BoundedValue(0.0, 100.0, throttle_pedal_target); + int x = throttle_pedal_target / 0.100000; + uint8_t t = 0; + + t = x & 0xFF; + Byte to_set0(data + 4); + to_set0.set_value(t, 0, 8); + x >>= 8; + + t = x & 0xFF; + Byte to_set1(data + 3); + to_set1.set_value(t, 0, 8); +} + +Throttlecommand100* Throttlecommand100::set_throttle_en_ctrl( + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl) { + throttle_en_ctrl_ = throttle_en_ctrl; + return this; +} + +// config detail: {'bit': 0, 'description': 'enable', 'enum': {0: +// 'THROTTLE_EN_CTRL_DISABLE', 1: 'THROTTLE_EN_CTRL_ENABLE'}, 'is_signed_var': +// False, 'len': 1, 'name': 'Throttle_EN_CTRL', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'enable', 'type': 'enum'} +void Throttlecommand100::set_p_throttle_en_ctrl( + uint8_t* data, + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl) { + int x = throttle_en_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 1); +} + +int Throttlecommand100::heartbeat_100(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +double Throttlecommand100::speed_target(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 6); + int32_t t = t1.get_byte(4, 4); + x <<= 4; + x |= t; + + double ret = x * 0.010000; + return ret; +} + +double Throttlecommand100::throttle_acc(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 2); + int32_t t = t1.get_byte(6, 2); + x <<= 2; + x |= t; + + double ret = x * 0.010000; + return ret; +} + +int Throttlecommand100::checksum_100(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +double Throttlecommand100::throttle_pedal_target(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.100000; + return ret; +} + +Throttle_command_100::Throttle_en_ctrlType Throttlecommand100::throttle_en_ctrl( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 1); + + Throttle_command_100::Throttle_en_ctrlType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/throttle_command_100.h b/modules/canbus_vehicle/demo/protocol/throttle_command_100.h new file mode 100644 index 00000000000..ae6052b7d71 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/throttle_command_100.h @@ -0,0 +1,140 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Throttlecommand100 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Throttlecommand100(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Throttlecommand100* set_heartbeat_100(int heartbeat_100); + + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 12, 'name': + // 'Speed_Target', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|40.95]', 'physical_unit': 'm/s', 'precision': 0.01, 'type': 'double'} + Throttlecommand100* set_speed_target(double speed_target); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': + // 'Throttle_Acc', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|10]', 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} + Throttlecommand100* set_throttle_acc(double throttle_acc); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Throttlecommand100* set_checksum_100(int checksum_100); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Throttle_Pedal_Target', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + Throttlecommand100* set_throttle_pedal_target(double throttle_pedal_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'THROTTLE_EN_CTRL_DISABLE', 1: 'THROTTLE_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Throttle_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Throttlecommand100* set_throttle_en_ctrl( + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_100(uint8_t* data, int heartbeat_100); + + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 12, 'name': + // 'Speed_Target', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|40.95]', 'physical_unit': 'm/s', 'precision': 0.01, 'type': 'double'} + void set_p_speed_target(uint8_t* data, double speed_target); + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 10, 'name': + // 'Throttle_Acc', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|10]', 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} + void set_p_throttle_acc(uint8_t* data, double throttle_acc); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_100', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_100(uint8_t* data, int checksum_100); + + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Throttle_Pedal_Target', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + void set_p_throttle_pedal_target(uint8_t* data, double throttle_pedal_target); + + // config detail: {'bit': 0, 'description': 'enable', 'enum': {0: + // 'THROTTLE_EN_CTRL_DISABLE', 1: 'THROTTLE_EN_CTRL_ENABLE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'Throttle_EN_CTRL', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + void set_p_throttle_en_ctrl( + uint8_t* data, + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl); + + int heartbeat_100(const std::uint8_t* bytes, const int32_t length) const; + + double speed_target(const std::uint8_t* bytes, const int32_t length) const; + + double throttle_acc(const std::uint8_t* bytes, const int32_t length) const; + + int checksum_100(const std::uint8_t* bytes, const int32_t length) const; + + double throttle_pedal_target(const std::uint8_t* bytes, + const int32_t length) const; + + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + private: + int heartbeat_100_; + double speed_target_; + double throttle_acc_; + int checksum_100_; + double throttle_pedal_target_; + Throttle_command_100::Throttle_en_ctrlType throttle_en_ctrl_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/throttle_report_500.cc b/modules/canbus_vehicle/demo/protocol/throttle_report_500.cc new file mode 100644 index 00000000000..bf25a5c8c61 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/throttle_report_500.cc @@ -0,0 +1,112 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/throttle_report_500.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Throttlereport500::Throttlereport500() {} +const int32_t Throttlereport500::ID = 0x500; + +void Throttlereport500::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_throttle_report_500()->set_throttle_pedal_actual( + throttle_pedal_actual(bytes, length)); + chassis->mutable_throttle_report_500()->set_throttle_flt2( + throttle_flt2(bytes, length)); + chassis->mutable_throttle_report_500()->set_throttle_flt1( + throttle_flt1(bytes, length)); + chassis->mutable_throttle_report_500()->set_throttle_en_state( + throttle_en_state(bytes, length)); +} + +// config detail: {'bit': 31, 'description': 'command', 'is_signed_var': False, +// 'len': 16, 'name': 'throttle_pedal_actual', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': +// 0.1, 'signal_type': 'command', 'type': 'double'} +double Throttlereport500::throttle_pedal_actual(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 4); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.100000; + return ret; +} + +// config detail: {'bit': 23, 'description': 'Drive system communication fault', +// 'enum': {0: 'THROTTLE_FLT2_NO_FAULT', 1: +// 'THROTTLE_FLT2_DRIVE_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, +// 'len': 8, 'name': 'throttle_flt2', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Throttle_report_500::Throttle_flt2Type Throttlereport500::throttle_flt2( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Throttle_report_500::Throttle_flt2Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 15, 'description': 'Drive system hardware fault', +// 'enum': {0: 'THROTTLE_FLT1_NO_FAULT', 1: +// 'THROTTLE_FLT1_DRIVE_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, 'len': +// 8, 'name': 'throttle_flt1', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Throttle_report_500::Throttle_flt1Type Throttlereport500::throttle_flt1( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + Throttle_report_500::Throttle_flt1Type ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 1, 'description': 'enable', 'enum': {0: +// 'THROTTLE_EN_STATE_MANUAL', 1: 'THROTTLE_EN_STATE_AUTO', 2: +// 'THROTTLE_EN_STATE_TAKEOVER', 3: 'THROTTLE_EN_STATE_STANDBY'}, +// 'is_signed_var': False, 'len': 2, 'name': 'throttle_en_state', 'offset': 0.0, +// 'order': 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', +// 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} +Throttle_report_500::Throttle_en_stateType Throttlereport500::throttle_en_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 2); + + Throttle_report_500::Throttle_en_stateType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/throttle_report_500.h b/modules/canbus_vehicle/demo/protocol/throttle_report_500.h new file mode 100644 index 00000000000..b4045c902e0 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/throttle_report_500.h @@ -0,0 +1,73 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Throttlereport500 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Throttlereport500(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 31, 'description': 'command', 'is_signed_var': + // False, 'len': 16, 'name': 'Throttle_Pedal_Actual', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|100]', 'physical_unit': '%', 'precision': + // 0.1, 'signal_type': 'command', 'type': 'double'} + double throttle_pedal_actual(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'description': 'Drive system communication + // fault', 'enum': {0: 'THROTTLE_FLT2_NO_FAULT', 1: + // 'THROTTLE_FLT2_DRIVE_SYSTEM_COMUNICATION_FAULT'}, 'is_signed_var': False, + // 'len': 8, 'name': 'Throttle_FLT2', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Throttle_report_500::Throttle_flt2Type throttle_flt2( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 15, 'description': 'Drive system hardware fault', + // 'enum': {0: 'THROTTLE_FLT1_NO_FAULT', 1: + // 'THROTTLE_FLT1_DRIVE_SYSTEM_HARDWARE_FAULT'}, 'is_signed_var': False, + // 'len': 8, 'name': 'Throttle_FLT1', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Throttle_report_500::Throttle_flt1Type throttle_flt1( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 1, 'description': 'enable', 'enum': {0: + // 'THROTTLE_EN_STATE_MANUAL', 1: 'THROTTLE_EN_STATE_AUTO', 2: + // 'THROTTLE_EN_STATE_TAKEOVER', 3: 'THROTTLE_EN_STATE_STANDBY'}, + // 'is_signed_var': False, 'len': 2, 'name': 'Throttle_EN_state', 'offset': + // 0.0, 'order': 'motorola', 'physical_range': '[0|2]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'enable', 'type': 'enum'} + Throttle_report_500::Throttle_en_stateType throttle_en_state( + const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.cc b/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.cc new file mode 100644 index 00000000000..41dd737af75 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.cc @@ -0,0 +1,114 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Ultrsensor1507::Ultrsensor1507() {} +const int32_t Ultrsensor1507::ID = 0x507; + +void Ultrsensor1507::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_ultr_sensor_1_507()->set_uiuss9_tof_direct( + uiuss9_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_1_507()->set_uiuss8_tof_direct( + uiuss8_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_1_507()->set_uiuss11_tof_direct( + uiuss11_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_1_507()->set_uiuss10_tof_direct( + uiuss10_tof_direct(bytes, length)); +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss9_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor1507::uiuss9_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss8_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor1507::uiuss8_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss11_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor1507::uiuss11_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss10_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor1507::uiuss10_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h b/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h new file mode 100644 index 00000000000..5644de9a4a4 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_1_507.h @@ -0,0 +1,63 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Ultrsensor1507 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Ultrsensor1507(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS9_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss9_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS8_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss8_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS11_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss11_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS10_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss10_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.cc b/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.cc new file mode 100644 index 00000000000..06f03859de6 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.cc @@ -0,0 +1,114 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Ultrsensor2508::Ultrsensor2508() {} +const int32_t Ultrsensor2508::ID = 0x508; + +void Ultrsensor2508::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_ultr_sensor_2_508()->set_uiuss9_tof_indirect( + uiuss9_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_2_508()->set_uiuss8_tof_indirect( + uiuss8_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_2_508()->set_uiuss11_tof_indirect( + uiuss11_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_2_508()->set_uiuss10_tof_indirect( + uiuss10_tof_indirect(bytes, length)); +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss9_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor2508::uiuss9_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss8_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor2508::uiuss8_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss11_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor2508::uiuss11_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss10_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor2508::uiuss10_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h b/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h new file mode 100644 index 00000000000..296e0b037fe --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_2_508.h @@ -0,0 +1,67 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Ultrsensor2508 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Ultrsensor2508(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS9_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss9_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS8_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss8_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS11_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss11_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS10_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss10_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.cc b/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.cc new file mode 100644 index 00000000000..b20f17a2e94 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.cc @@ -0,0 +1,114 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Ultrsensor3509::Ultrsensor3509() {} +const int32_t Ultrsensor3509::ID = 0x509; + +void Ultrsensor3509::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_ultr_sensor_3_509()->set_uiuss5_tof_direct( + uiuss5_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_3_509()->set_uiuss4_tof_direct( + uiuss4_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_3_509()->set_uiuss3_tof_direct( + uiuss3_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_3_509()->set_uiuss2_tof_direct( + uiuss2_tof_direct(bytes, length)); +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss5_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor3509::uiuss5_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss4_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor3509::uiuss4_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss3_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor3509::uiuss3_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss2_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor3509::uiuss2_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h b/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h new file mode 100644 index 00000000000..53615589659 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_3_509.h @@ -0,0 +1,63 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Ultrsensor3509 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Ultrsensor3509(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS5_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss5_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS4_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss4_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS3_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss3_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS2_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss2_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.cc b/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.cc new file mode 100644 index 00000000000..01949953d9c --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.cc @@ -0,0 +1,114 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Ultrsensor4510::Ultrsensor4510() {} +const int32_t Ultrsensor4510::ID = 0x510; + +void Ultrsensor4510::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_ultr_sensor_4_510()->set_uiuss5_tof_indirect( + uiuss5_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_4_510()->set_uiuss4_tof_indirect( + uiuss4_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_4_510()->set_uiuss3_tof_indirect( + uiuss3_tof_indirect(bytes, length)); + chassis->mutable_ultr_sensor_4_510()->set_uiuss2_tof_indirect( + uiuss2_tof_indirect(bytes, length)); +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss5_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor4510::uiuss5_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss4_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor4510::uiuss4_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss3_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor4510::uiuss3_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss2_tof_indirect', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor4510::uiuss2_tof_indirect(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h b/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h new file mode 100644 index 00000000000..96ebfc156d4 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_4_510.h @@ -0,0 +1,67 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Ultrsensor4510 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Ultrsensor4510(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS5_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss5_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS4_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss4_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS3_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss3_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS2_ToF_Indirect', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, + // 'type': 'double'} + double uiuss2_tof_indirect(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.cc b/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.cc new file mode 100644 index 00000000000..81a364e66f8 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.cc @@ -0,0 +1,114 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Ultrsensor5511::Ultrsensor5511() {} +const int32_t Ultrsensor5511::ID = 0x511; + +void Ultrsensor5511::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_ultr_sensor_5_511()->set_uiuss7_tof_direct( + uiuss7_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_5_511()->set_uiuss6_tof_direct( + uiuss6_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_5_511()->set_uiuss1_tof_direct( + uiuss1_tof_direct(bytes, length)); + chassis->mutable_ultr_sensor_5_511()->set_uiuss0_tof_direct( + uiuss0_tof_direct(bytes, length)); +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss7_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor5511::uiuss7_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss6_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor5511::uiuss6_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss1_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor5511::uiuss1_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': +// 'uiuss0_tof_direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} +double Ultrsensor5511::uiuss0_tof_direct(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.017240; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h b/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h new file mode 100644 index 00000000000..3debaad0439 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/ultr_sensor_5_511.h @@ -0,0 +1,63 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Ultrsensor5511 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Ultrsensor5511(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS7_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss7_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS6_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss6_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS1_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss1_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': + // 'uiUSS0_ToF_Direct', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|65535]', 'physical_unit': 'cm', 'precision': 0.01724, 'type': 'double'} + double uiuss0_tof_direct(const std::uint8_t* bytes, + const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vcu_report_505.cc b/modules/canbus_vehicle/demo/protocol/vcu_report_505.cc new file mode 100644 index 00000000000..5a1c4a2c0b6 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vcu_report_505.cc @@ -0,0 +1,248 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/vcu_report_505.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Vcureport505::Vcureport505() {} +const int32_t Vcureport505::ID = 0x505; + +void Vcureport505::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_vcu_report_505()->set_vehicle_mode_state( + vehicle_mode_state(bytes, length)); + chassis->mutable_vcu_report_505()->set_aeb_mode(aeb_mode(bytes, length)); + chassis->mutable_vcu_report_505()->set_brake_light_actual( + brake_light_actual(bytes, length)); + chassis->mutable_vcu_report_505()->set_turn_light_actual( + turn_light_actual(bytes, length)); + chassis->mutable_vcu_report_505()->set_chassis_errcode( + chassis_errcode(bytes, length)); + chassis->mutable_vcu_report_505()->set_drive_mode_sts( + drive_mode_sts(bytes, length)); + chassis->mutable_vcu_report_505()->set_steer_mode_sts( + steer_mode_sts(bytes, length)); + chassis->mutable_vcu_report_505()->set_frontcrash_state( + frontcrash_state(bytes, length)); + chassis->mutable_vcu_report_505()->set_backcrash_state( + backcrash_state(bytes, length)); + chassis->mutable_vcu_report_505()->set_aeb_brake_state( + aeb_brake_state(bytes, length)); + chassis->mutable_vcu_report_505()->set_acc(acc(bytes, length)); + chassis->mutable_vcu_report_505()->set_speed(speed(bytes, length)); +} + +// config detail: {'bit': 36, 'enum': {0: +// 'VEHICLE_MODE_STATE_MANUAL_REMOTE_MODE', 1: 'VEHICLE_MODE_STATE_AUTO_MODE', +// 2: 'VEHICLE_MODE_STATE_EMERGENCY_MODE', 3: +// 'VEHICLE_MODE_STATE_STANDBY_MODE'}, 'is_signed_var': False, 'len': 2, 'name': +// 'vehicle_mode_state', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Vcu_report_505::Vehicle_mode_stateType Vcureport505::vehicle_mode_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(3, 2); + + Vcu_report_505::Vehicle_mode_stateType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 58, 'description': 'describle the vehicle AEB mode +// whether was set', 'enum': {0: 'AEB_MODE_DISABLE', 1: 'AEB_MODE_ENABLE'}, +// 'is_signed_var': False, 'len': 1, 'name': 'aeb_mode', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, +// 'type': 'enum'} +Vcu_report_505::Aeb_modeType Vcureport505::aeb_mode(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(2, 1); + + Vcu_report_505::Aeb_modeType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 11, 'enum': {0: 'BRAKE_LIGHT_ACTUAL_BRAKELIGHT_OFF', +// 1: 'BRAKE_LIGHT_ACTUAL_BRAKELIGHT_ON'}, 'is_signed_var': False, 'len': 1, +// 'name': 'brake_light_actual', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Vcu_report_505::Brake_light_actualType Vcureport505::brake_light_actual( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(3, 1); + + Vcu_report_505::Brake_light_actualType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 57, 'enum': {0: 'TURN_LIGHT_ACTUAL_TURNLAMPSTS_OFF', +// 1: 'TURN_LIGHT_ACTUAL_LEFT_TURNLAMPSTS_ON', 2: +// 'TURN_LIGHT_ACTUAL_RIGHT_TURNLAMPSTS_ON', 3: +// 'TURN_LIGHT_ACTUAL_HAZARD_WARNING_LAMPSTS_ON'}, 'is_signed_var': False, +// 'len': 2, 'name': 'turn_light_actual', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Vcu_report_505::Turn_light_actualType Vcureport505::turn_light_actual( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 2); + + Vcu_report_505::Turn_light_actualType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': +// 'chassis_errcode', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vcureport505::chassis_errcode(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 39, 'enum': {0: +// 'DRIVE_MODE_STS_THROTTLE_PADDLE_DRIVE_MODE', 1: +// 'DRIVE_MODE_STS_SPEED_DRIVE_MODE'}, 'is_signed_var': False, 'len': 3, 'name': +// 'drive_mode_sts', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Vcu_report_505::Drive_mode_stsType Vcureport505::drive_mode_sts( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(5, 3); + + Vcu_report_505::Drive_mode_stsType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 10, 'enum': {0: 'STEER_MODE_STS_STANDARD_STEER_MODE', +// 1: 'STEER_MODE_STS_NON_DIRECTION_STEER_MODE', 2: +// 'STEER_MODE_STS_SYNC_DIRECTION_STEER_MODE'}, 'is_signed_var': False, 'len': +// 3, 'name': 'steer_mode_sts', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +Vcu_report_505::Steer_mode_stsType Vcureport505::steer_mode_sts( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 3); + + Vcu_report_505::Steer_mode_stsType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 33, 'enum': {0: 'FRONTCRASH_STATE_NO_EVENT', 1: +// 'FRONTCRASH_STATE_CRASH_EVENT'}, 'is_signed_var': False, 'len': 1, 'name': +// 'frontcrash_state', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Vcu_report_505::Frontcrash_stateType Vcureport505::frontcrash_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(1, 1); + + Vcu_report_505::Frontcrash_stateType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 34, 'enum': {0: 'BACKCRASH_STATE_NO_EVENT', 1: +// 'BACKCRASH_STATE_CRASH_EVENT'}, 'is_signed_var': False, 'len': 1, 'name': +// 'backcrash_state', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +Vcu_report_505::Backcrash_stateType Vcureport505::backcrash_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(2, 1); + + Vcu_report_505::Backcrash_stateType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 32, 'description': 'describe the vehicle e-brake +// command whether was triggered by AEB', 'enum': {0: +// 'AEB_BRAKE_STATE_INACTIVE', 1: 'AEB_BRAKE_STATE_ACTIVE'}, 'is_signed_var': +// False, 'len': 1, 'name': 'aeb_brake_state', 'offset': 0.0, 'order': +// 'motorola', 'physical_range': '[0|0]', 'physical_unit': '', 'precision': 1.0, +// 'signal_type': 'command', 'type': 'enum'} +Vcu_report_505::Aeb_brake_stateType Vcureport505::aeb_brake_state( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 1); + + Vcu_report_505::Aeb_brake_stateType ret = + static_cast(x); + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': True, 'len': 12, 'name': 'acc', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[-10|10]', +// 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} +double Vcureport505::acc(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(4, 4); + x <<= 4; + x |= t; + + x <<= 20; + x >>= 20; + + double ret = x * 0.010000; + return ret; +} + +// config detail: {'bit': 23, 'description': 'speed', 'is_signed_var': True, +// 'len': 16, 'name': 'speed', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[-32.768|32.767]', 'physical_unit': 'm/s', 'precision': +// 0.001, 'signal_type': 'speed', 'type': 'double'} +double Vcureport505::speed(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + x <<= 16; + x >>= 16; + + double ret = x * 0.001000; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vcu_report_505.h b/modules/canbus_vehicle/demo/protocol/vcu_report_505.h new file mode 100644 index 00000000000..4e63b0b02e9 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vcu_report_505.h @@ -0,0 +1,133 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Vcureport505 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Vcureport505(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 36, 'enum': {0: + // 'VEHICLE_MODE_STATE_MANUAL_REMOTE_MODE', 1: 'VEHICLE_MODE_STATE_AUTO_MODE', + // 2: 'VEHICLE_MODE_STATE_EMERGENCY_MODE', 3: + // 'VEHICLE_MODE_STATE_STANDBY_MODE'}, 'is_signed_var': False, 'len': 2, + // 'name': 'Vehicle_Mode_State', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vcu_report_505::Vehicle_mode_stateType vehicle_mode_state( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 58, 'description': 'describle the vehicle AEB mode + // whether was set', 'enum': {0: 'AEB_MODE_DISABLE', 1: 'AEB_MODE_ENABLE'}, + // 'is_signed_var': False, 'len': 1, 'name': 'AEB_Mode', 'offset': 0.0, + // 'order': 'motorola', 'physical_range': '[0|1]', 'physical_unit': '', + // 'precision': 1.0, 'type': 'enum'} + Vcu_report_505::Aeb_modeType aeb_mode(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 11, 'enum': {0: 'BRAKE_LIGHT_ACTUAL_BRAKELIGHT_OFF', + // 1: 'BRAKE_LIGHT_ACTUAL_BRAKELIGHT_ON'}, 'is_signed_var': False, 'len': 1, + // 'name': 'Brake_Light_Actual', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|1]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vcu_report_505::Brake_light_actualType brake_light_actual( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 57, 'enum': {0: 'TURN_LIGHT_ACTUAL_TURNLAMPSTS_OFF', + // 1: 'TURN_LIGHT_ACTUAL_LEFT_TURNLAMPSTS_ON', 2: + // 'TURN_LIGHT_ACTUAL_RIGHT_TURNLAMPSTS_ON', 3: + // 'TURN_LIGHT_ACTUAL_HAZARD_WARNING_LAMPSTS_ON'}, 'is_signed_var': False, + // 'len': 2, 'name': 'Turn_Light_Actual', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vcu_report_505::Turn_light_actualType turn_light_actual( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': + // 'Chassis_errcode', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int chassis_errcode(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 39, 'enum': {0: + // 'DRIVE_MODE_STS_THROTTLE_PADDLE_DRIVE_MODE', 1: + // 'DRIVE_MODE_STS_SPEED_DRIVE_MODE'}, 'is_signed_var': False, 'len': 3, + // 'name': 'Drive_Mode_STS', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vcu_report_505::Drive_mode_stsType drive_mode_sts(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 10, 'enum': {0: + // 'STEER_MODE_STS_STANDARD_STEER_MODE', 1: + // 'STEER_MODE_STS_NON_DIRECTION_STEER_MODE', 2: + // 'STEER_MODE_STS_SYNC_DIRECTION_STEER_MODE'}, 'is_signed_var': False, 'len': + // 3, 'name': 'Steer_Mode_STS', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vcu_report_505::Steer_mode_stsType steer_mode_sts(const std::uint8_t* bytes, + const int32_t length) const; + + // config detail: {'bit': 33, 'enum': {0: 'FRONTCRASH_STATE_NO_EVENT', 1: + // 'FRONTCRASH_STATE_CRASH_EVENT'}, 'is_signed_var': False, 'len': 1, 'name': + // 'FrontCrash_State', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + Vcu_report_505::Frontcrash_stateType frontcrash_state( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 34, 'enum': {0: 'BACKCRASH_STATE_NO_EVENT', 1: + // 'BACKCRASH_STATE_CRASH_EVENT'}, 'is_signed_var': False, 'len': 1, 'name': + // 'BackCrash_State', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|0]', 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + Vcu_report_505::Backcrash_stateType backcrash_state( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 32, 'description': 'describe the vehicle e-brake + // command whether was triggered by AEB', 'enum': {0: + // 'AEB_BRAKE_STATE_INACTIVE', 1: 'AEB_BRAKE_STATE_ACTIVE'}, 'is_signed_var': + // False, 'len': 1, 'name': 'AEB_Brake_State', 'offset': 0.0, 'order': + // 'motorola', 'physical_range': '[0|0]', 'physical_unit': '', + // 'precision': 1.0, 'signal_type': 'command', 'type': 'enum'} + Vcu_report_505::Aeb_brake_stateType aeb_brake_state( + const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': True, 'len': 12, 'name': 'ACC', + // 'offset': 0.0, 'order': 'motorola', 'physical_range': '[-10|10]', + // 'physical_unit': 'm/s^2', 'precision': 0.01, 'type': 'double'} + double acc(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 23, 'description': 'speed', 'is_signed_var': True, + // 'len': 16, 'name': 'SPEED', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[-32.768|32.767]', 'physical_unit': 'm/s', 'precision': + // 0.001, 'signal_type': 'speed', 'type': 'double'} + double speed(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.cc b/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.cc new file mode 100644 index 00000000000..e5f971672d1 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.cc @@ -0,0 +1,260 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h" + +#include "modules/drivers/canbus/common/byte.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +const int32_t Vehiclemodecommand105::ID = 0x105; + +// public +Vehiclemodecommand105::Vehiclemodecommand105() { Reset(); } + +uint32_t Vehiclemodecommand105::GetPeriod() const { + // TODO(All) : modify every protocol's period manually + static const uint32_t PERIOD = 20 * 1000; + return PERIOD; +} + +void Vehiclemodecommand105::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_vehicle_mode_command_105()->set_heartbeat_105( + heartbeat_105(bytes, length)); + chassis->mutable_vehicle_mode_command_105()->set_checksum_105( + checksum_105(bytes, length)); + chassis->mutable_vehicle_mode_command_105()->set_turn_light_ctrl( + turn_light_ctrl(bytes, length)); + chassis->mutable_vehicle_mode_command_105()->set_vin_req( + vin_req(bytes, length)); + chassis->mutable_vehicle_mode_command_105()->set_drive_mode_ctrl( + drive_mode_ctrl(bytes, length)); + chassis->mutable_vehicle_mode_command_105()->set_steer_mode_ctrl( + steer_mode_ctrl(bytes, length)); +} + +void Vehiclemodecommand105::UpdateData_Heartbeat(uint8_t* data) { + // TODO(All) : you should add the heartbeat manually + ++heartbeat_105_; + heartbeat_105_ = (heartbeat_105_) % 16; + set_p_heartbeat_105(data, heartbeat_105_); + checksum_105_ = + data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[6]; + set_p_checksum_105(data, checksum_105_); +} + +void Vehiclemodecommand105::UpdateData(uint8_t* data) { + set_p_turn_light_ctrl(data, turn_light_ctrl_); + set_p_vin_req(data, vin_req_); + set_p_drive_mode_ctrl(data, drive_mode_ctrl_); + set_p_steer_mode_ctrl(data, steer_mode_ctrl_); +} + +void Vehiclemodecommand105::Reset() { + // TODO(All) : you should check this manually + heartbeat_105_ = 0; + checksum_105_ = 0; + turn_light_ctrl_ = Vehicle_mode_command_105::TURN_LIGHT_CTRL_TURNLAMP_OFF; + vin_req_ = Vehicle_mode_command_105::VIN_REQ_VIN_REQ_DISABLE; + drive_mode_ctrl_ = + Vehicle_mode_command_105::DRIVE_MODE_CTRL_THROTTLE_PADDLE_DRIVE; + steer_mode_ctrl_ = Vehicle_mode_command_105::STEER_MODE_CTRL_STANDARD_STEER; +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_heartbeat_105( + int heartbeat_105) { + heartbeat_105_ = heartbeat_105; + return this; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': +// 'Heartbeat_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Vehiclemodecommand105::set_p_heartbeat_105(uint8_t* data, + int heartbeat_105) { + heartbeat_105 = ProtocolData::BoundedValue(0, 15, heartbeat_105); + int x = heartbeat_105; + + Byte to_set(data + 0); + to_set.set_value(x, 4, 4); +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_checksum_105( + int checksum_105) { + checksum_105_ = checksum_105; + return this; +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': +// 'CheckSum_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': +// '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +void Vehiclemodecommand105::set_p_checksum_105(uint8_t* data, + int checksum_105) { + checksum_105 = ProtocolData::BoundedValue(0, 255, checksum_105); + int x = checksum_105; + + Byte to_set(data + 7); + to_set.set_value(x, 0, 8); +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_turn_light_ctrl( + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl) { + turn_light_ctrl_ = turn_light_ctrl; + return this; +} + +// config detail: {'bit': 17, 'enum': {0: 'TURN_LIGHT_CTRL_TURNLAMP_OFF', 1: +// 'TURN_LIGHT_CTRL_LEFT_TURNLAMP_ON', 2: 'TURN_LIGHT_CTRL_RIGHT_TURNLAMP_ON', +// 3: 'TURN_LIGHT_CTRL_HAZARD_WARNING_LAMPSTS_ON'}, 'is_signed_var': False, +// 'len': 2, 'name': 'Turn_Light_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +void Vehiclemodecommand105::set_p_turn_light_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl) { + int x = turn_light_ctrl; + + Byte to_set(data + 2); + to_set.set_value(x, 0, 2); +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_vin_req( + Vehicle_mode_command_105::Vin_reqType vin_req) { + vin_req_ = vin_req; + return this; +} + +// config detail: {'bit': 24, 'enum': {0: 'VIN_REQ_VIN_REQ_DISABLE', 1: +// 'VIN_REQ_VIN_REQ_ENABLE'}, 'is_signed_var': False, 'len': 1, 'name': +// 'VIN_Req', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|1]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} +void Vehiclemodecommand105::set_p_vin_req( + uint8_t* data, Vehicle_mode_command_105::Vin_reqType vin_req) { + int x = vin_req; + + Byte to_set(data + 3); + to_set.set_value(x, 0, 1); +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_drive_mode_ctrl( + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl) { + drive_mode_ctrl_ = drive_mode_ctrl; + return this; +} + +// config detail: {'bit': 10, 'enum': {0: +// 'DRIVE_MODE_CTRL_THROTTLE_PADDLE_DRIVE', 1: 'DRIVE_MODE_CTRL_SPEED_DRIVE'}, +// 'is_signed_var': False, 'len': 3, 'name': 'Drive_Mode_CTRL', 'offset': 0.0, +// 'order': 'motorola', 'physical_range': '[0|7]', 'physical_unit': '', +// 'precision': 1.0, 'type': 'enum'} +void Vehiclemodecommand105::set_p_drive_mode_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl) { + int x = drive_mode_ctrl; + + Byte to_set(data + 1); + to_set.set_value(x, 0, 3); +} + +Vehiclemodecommand105* Vehiclemodecommand105::set_steer_mode_ctrl( + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl) { + steer_mode_ctrl_ = steer_mode_ctrl; + return this; +} + +// config detail: {'bit': 2, 'enum': {0: 'STEER_MODE_CTRL_STANDARD_STEER', 1: +// 'STEER_MODE_CTRL_NON_DIRECTION_STEER', 2: +// 'STEER_MODE_CTRL_SYNC_DIRECTION_STEER'}, 'is_signed_var': False, 'len': 3, +// 'name': 'Steer_Mode_CTRL', 'offset': 0.0, 'order': 'motorola', +// 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': +// 'enum'} +void Vehiclemodecommand105::set_p_steer_mode_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl) { + int x = steer_mode_ctrl; + + Byte to_set(data + 0); + to_set.set_value(x, 0, 3); +} + +int Vehiclemodecommand105::heartbeat_105(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(4, 4); + + int ret = x; + return ret; +} + +int Vehiclemodecommand105::checksum_105(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +Vehicle_mode_command_105::Turn_light_ctrlType +Vehiclemodecommand105::turn_light_ctrl(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 2); + + Vehicle_mode_command_105::Turn_light_ctrlType ret = + static_cast(x); + return ret; +} + +Vehicle_mode_command_105::Vin_reqType Vehiclemodecommand105::vin_req( + const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 1); + + Vehicle_mode_command_105::Vin_reqType ret = + static_cast(x); + return ret; +} + +Vehicle_mode_command_105::Drive_mode_ctrlType +Vehiclemodecommand105::drive_mode_ctrl(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 3); + + Vehicle_mode_command_105::Drive_mode_ctrlType ret = + static_cast(x); + return ret; +} + +Vehicle_mode_command_105::Steer_mode_ctrlType +Vehiclemodecommand105::steer_mode_ctrl(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 3); + + Vehicle_mode_command_105::Steer_mode_ctrlType ret = + static_cast(x); + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h b/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h new file mode 100644 index 00000000000..26795c4fc75 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vehicle_mode_command_105.h @@ -0,0 +1,162 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Vehiclemodecommand105 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + + Vehiclemodecommand105(); + + uint32_t GetPeriod() const override; + + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + void UpdateData_Heartbeat(uint8_t* data) override; + + void UpdateData(uint8_t* data) override; + + void Reset() override; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Vehiclemodecommand105* set_heartbeat_105(int heartbeat_105); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + Vehiclemodecommand105* set_checksum_105(int checksum_105); + + // config detail: {'bit': 17, 'enum': {0: 'TURN_LIGHT_CTRL_TURNLAMP_OFF', 1: + // 'TURN_LIGHT_CTRL_LEFT_TURNLAMP_ON', 2: 'TURN_LIGHT_CTRL_RIGHT_TURNLAMP_ON', + // 3: 'TURN_LIGHT_CTRL_HAZARD_WARNING_LAMPSTS_ON'}, 'is_signed_var': False, + // 'len': 2, 'name': 'Turn_Light_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vehiclemodecommand105* set_turn_light_ctrl( + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl); + + // config detail: {'bit': 24, 'enum': {0: 'VIN_REQ_VIN_REQ_DISABLE', 1: + // 'VIN_REQ_VIN_REQ_ENABLE'}, 'is_signed_var': False, 'len': 1, 'name': + // 'VIN_Req', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|1]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + Vehiclemodecommand105* set_vin_req( + Vehicle_mode_command_105::Vin_reqType vin_req); + + // config detail: {'bit': 10, 'enum': {0: + // 'DRIVE_MODE_CTRL_THROTTLE_PADDLE_DRIVE', 1: 'DRIVE_MODE_CTRL_SPEED_DRIVE'}, + // 'is_signed_var': False, 'len': 3, 'name': 'Drive_Mode_CTRL', 'offset': 0.0, + // 'order': 'motorola', 'physical_range': '[0|7]', 'physical_unit': '', + // 'precision': 1.0, 'type': 'enum'} + Vehiclemodecommand105* set_drive_mode_ctrl( + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl); + + // config detail: {'bit': 2, 'enum': {0: 'STEER_MODE_CTRL_STANDARD_STEER', 1: + // 'STEER_MODE_CTRL_NON_DIRECTION_STEER', 2: + // 'STEER_MODE_CTRL_SYNC_DIRECTION_STEER'}, 'is_signed_var': False, 'len': 3, + // 'name': 'Steer_Mode_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + Vehiclemodecommand105* set_steer_mode_ctrl( + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl); + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 4, 'name': + // 'Heartbeat_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|15]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_heartbeat_105(uint8_t* data, int heartbeat_105); + + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'CheckSum_105', 'offset': 0.0, 'order': 'motorola', 'physical_range': + // '[0|255]', 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + void set_p_checksum_105(uint8_t* data, int checksum_105); + + // config detail: {'bit': 17, 'enum': {0: 'TURN_LIGHT_CTRL_TURNLAMP_OFF', 1: + // 'TURN_LIGHT_CTRL_LEFT_TURNLAMP_ON', 2: 'TURN_LIGHT_CTRL_RIGHT_TURNLAMP_ON', + // 3: 'TURN_LIGHT_CTRL_HAZARD_WARNING_LAMPSTS_ON'}, 'is_signed_var': False, + // 'len': 2, 'name': 'Turn_Light_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + void set_p_turn_light_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl); + + // config detail: {'bit': 24, 'enum': {0: 'VIN_REQ_VIN_REQ_DISABLE', 1: + // 'VIN_REQ_VIN_REQ_ENABLE'}, 'is_signed_var': False, 'len': 1, 'name': + // 'VIN_Req', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|1]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'enum'} + void set_p_vin_req(uint8_t* data, + Vehicle_mode_command_105::Vin_reqType vin_req); + + // config detail: {'bit': 10, 'enum': {0: + // 'DRIVE_MODE_CTRL_THROTTLE_PADDLE_DRIVE', 1: 'DRIVE_MODE_CTRL_SPEED_DRIVE'}, + // 'is_signed_var': False, 'len': 3, 'name': 'Drive_Mode_CTRL', 'offset': 0.0, + // 'order': 'motorola', 'physical_range': '[0|7]', 'physical_unit': '', + // 'precision': 1.0, 'type': 'enum'} + void set_p_drive_mode_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl); + + // config detail: {'bit': 2, 'enum': {0: 'STEER_MODE_CTRL_STANDARD_STEER', 1: + // 'STEER_MODE_CTRL_NON_DIRECTION_STEER', 2: + // 'STEER_MODE_CTRL_SYNC_DIRECTION_STEER'}, 'is_signed_var': False, 'len': 3, + // 'name': 'Steer_Mode_CTRL', 'offset': 0.0, 'order': 'motorola', + // 'physical_range': '[0|7]', 'physical_unit': '', 'precision': 1.0, 'type': + // 'enum'} + void set_p_steer_mode_ctrl( + uint8_t* data, + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl); + + int heartbeat_105(const std::uint8_t* bytes, const int32_t length) const; + + int checksum_105(const std::uint8_t* bytes, const int32_t length) const; + + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + Vehicle_mode_command_105::Vin_reqType vin_req(const std::uint8_t* bytes, + const int32_t length) const; + + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl( + const std::uint8_t* bytes, const int32_t length) const; + + private: + int heartbeat_105_; + int checksum_105_; + Vehicle_mode_command_105::Turn_light_ctrlType turn_light_ctrl_; + Vehicle_mode_command_105::Vin_reqType vin_req_; + Vehicle_mode_command_105::Drive_mode_ctrlType drive_mode_ctrl_; + Vehicle_mode_command_105::Steer_mode_ctrlType steer_mode_ctrl_; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp1_514.cc b/modules/canbus_vehicle/demo/protocol/vin_resp1_514.cc new file mode 100644 index 00000000000..4b4fb1b9c49 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp1_514.cc @@ -0,0 +1,134 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/vin_resp1_514.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Vinresp1514::Vinresp1514() {} +const int32_t Vinresp1514::ID = 0x514; + +void Vinresp1514::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_vin_resp1_514()->set_vin07(vin07(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin06(vin06(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin05(vin05(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin04(vin04(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin03(vin03(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin02(vin02(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin01(vin01(bytes, length)); + chassis->mutable_vin_resp1_514()->set_vin00(vin00(bytes, length)); +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': 'vin07', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin07(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 8, 'name': 'vin06', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin06(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': 'vin05', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin05(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 8, 'name': 'vin04', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin04(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 31, 'is_signed_var': False, 'len': 8, 'name': 'vin03', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin03(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 8, 'name': 'vin02', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin02(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': 'vin01', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin01(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': 'vin00', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp1514::vin00(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp1_514.h b/modules/canbus_vehicle/demo/protocol/vin_resp1_514.h new file mode 100644 index 00000000000..2bdec396d70 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp1_514.h @@ -0,0 +1,79 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Vinresp1514 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Vinresp1514(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN07', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin07(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN06', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin06(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN05', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin05(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN04', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin04(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 31, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN03', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin03(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN02', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin02(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN01', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin01(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN00', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin00(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp2_515.cc b/modules/canbus_vehicle/demo/protocol/vin_resp2_515.cc new file mode 100644 index 00000000000..374e7f5f77f --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp2_515.cc @@ -0,0 +1,134 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/vin_resp2_515.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Vinresp2515::Vinresp2515() {} +const int32_t Vinresp2515::ID = 0x515; + +void Vinresp2515::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_vin_resp2_515()->set_vin15(vin15(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin14(vin14(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin13(vin13(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin12(vin12(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin11(vin11(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin10(vin10(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin09(vin09(bytes, length)); + chassis->mutable_vin_resp2_515()->set_vin08(vin08(bytes, length)); +} + +// config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': 'vin15', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin15(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 7); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 8, 'name': 'vin14', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin14(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': 'vin13', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin13(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 5); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 8, 'name': 'vin12', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin12(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 31, 'is_signed_var': False, 'len': 8, 'name': 'vin11', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin11(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 8, 'name': 'vin10', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin10(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': 'vin09', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin09(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': 'vin08', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp2515::vin08(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp2_515.h b/modules/canbus_vehicle/demo/protocol/vin_resp2_515.h new file mode 100644 index 00000000000..0a82ad5bd4c --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp2_515.h @@ -0,0 +1,79 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Vinresp2515 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Vinresp2515(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 63, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN15', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin15(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN14', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin14(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 47, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN13', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin13(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN12', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin12(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 31, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN11', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin11(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN10', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin10(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 15, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN09', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin09(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN08', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin08(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp3_516.cc b/modules/canbus_vehicle/demo/protocol/vin_resp3_516.cc new file mode 100644 index 00000000000..204cc7ccbd2 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp3_516.cc @@ -0,0 +1,50 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/vin_resp3_516.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Vinresp3516::Vinresp3516() {} +const int32_t Vinresp3516::ID = 0x516; + +void Vinresp3516::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_vin_resp3_516()->set_vin16(vin16(bytes, length)); +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': 'vin16', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', +// 'physical_unit': '', 'precision': 1.0, 'type': 'int'} +int Vinresp3516::vin16(const std::uint8_t* bytes, int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + int ret = x; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/vin_resp3_516.h b/modules/canbus_vehicle/demo/protocol/vin_resp3_516.h new file mode 100644 index 00000000000..d84327bc3e5 --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/vin_resp3_516.h @@ -0,0 +1,44 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Vinresp3516 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Vinresp3516(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 8, 'name': + // 'VIN16', 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|255]', + // 'physical_unit': '', 'precision': 1.0, 'type': 'int'} + int vin16(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.cc b/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.cc new file mode 100644 index 00000000000..2a7db0b34cf --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.cc @@ -0,0 +1,110 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h" + +#include "glog/logging.h" + +#include "modules/drivers/canbus/common/byte.h" +#include "modules/drivers/canbus/common/canbus_consts.h" + +namespace apollo { +namespace canbus { +namespace demo { + +using ::apollo::drivers::canbus::Byte; + +Wheelspeedreport506::Wheelspeedreport506() {} +const int32_t Wheelspeedreport506::ID = 0x506; + +void Wheelspeedreport506::Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const { + chassis->mutable_wheelspeed_report_506()->set_rr(rr(bytes, length)); + chassis->mutable_wheelspeed_report_506()->set_rl(rl(bytes, length)); + chassis->mutable_wheelspeed_report_506()->set_fr(fr(bytes, length)); + chassis->mutable_wheelspeed_report_506()->set_fl(fl(bytes, length)); +} + +// config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': 'rr', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', +// 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} +double Wheelspeedreport506::rr(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 6); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 7); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.001000; + return ret; +} + +// config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': 'rl', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', +// 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} +double Wheelspeedreport506::rl(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 4); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 5); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.001000; + return ret; +} + +// config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': 'fr', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', +// 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} +double Wheelspeedreport506::fr(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.001000; + return ret; +} + +// config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': 'fl', +// 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', +// 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} +double Wheelspeedreport506::fl(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 0); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 1); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = x * 0.001000; + return ret; +} +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h b/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h new file mode 100644 index 00000000000..46f363ec8be --- /dev/null +++ b/modules/canbus_vehicle/demo/protocol/wheelspeed_report_506.h @@ -0,0 +1,59 @@ +/****************************************************************************** + * Copyright 2023 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include "modules/canbus_vehicle/demo/proto/demo.pb.h" + +#include "modules/drivers/canbus/can_comm/protocol_data.h" + +namespace apollo { +namespace canbus { +namespace demo { + +class Wheelspeedreport506 + : public ::apollo::drivers::canbus::ProtocolData<::apollo::canbus::Demo> { + public: + static const int32_t ID; + Wheelspeedreport506(); + void Parse(const std::uint8_t* bytes, int32_t length, + Demo* chassis) const override; + + private: + // config detail: {'bit': 55, 'is_signed_var': False, 'len': 16, 'name': 'RR', + // 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', + // 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} + double rr(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 39, 'is_signed_var': False, 'len': 16, 'name': 'RL', + // 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', + // 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} + double rl(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 23, 'is_signed_var': False, 'len': 16, 'name': 'FR', + // 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', + // 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} + double fr(const std::uint8_t* bytes, const int32_t length) const; + + // config detail: {'bit': 7, 'is_signed_var': False, 'len': 16, 'name': 'FL', + // 'offset': 0.0, 'order': 'motorola', 'physical_range': '[0|65.535]', + // 'physical_unit': 'm/s', 'precision': 0.001, 'type': 'double'} + double fl(const std::uint8_t* bytes, const int32_t length) const; +}; + +} // namespace demo +} // namespace canbus +} // namespace apollo diff --git a/modules/canbus_vehicle/devkit/devkit_controller.cc b/modules/canbus_vehicle/devkit/devkit_controller.cc index 5f4bb19f3ff..59232d0c747 100644 --- a/modules/canbus_vehicle/devkit/devkit_controller.cc +++ b/modules/canbus_vehicle/devkit/devkit_controller.cc @@ -45,10 +45,19 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; bool emergency_brake = false; } // namespace +void DevkitController::AddSendMessage() { + can_sender_->AddMessage(Throttlecommand100::ID, throttle_command_100_, false); + can_sender_->AddMessage(Brakecommand101::ID, brake_command_101_, false); + can_sender_->AddMessage(Gearcommand103::ID, gear_command_103_, false); + can_sender_->AddMessage(Parkcommand104::ID, park_command_104_, false); + can_sender_->AddMessage(Steeringcommand102::ID, steering_command_102_, false); + can_sender_->AddMessage(Vehiclemodecommand105::ID, vehicle_mode_command_105_, + false); +} + ErrorCode DevkitController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Devkit>* const can_sender, - CanReceiver<::apollo::canbus::Devkit>* const can_receiver, MessageManager<::apollo::canbus::Devkit>* const message_manager) { if (is_initialized_) { AINFO << "DevkitController has already been initiated."; @@ -68,12 +77,6 @@ ErrorCode DevkitController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; @@ -124,15 +127,8 @@ ErrorCode DevkitController::Init( return ErrorCode::CANBUS_ERROR; } - can_sender_->AddMessage(Throttlecommand100::ID, throttle_command_100_, false); - can_sender_->AddMessage(Brakecommand101::ID, brake_command_101_, false); - can_sender_->AddMessage(Gearcommand103::ID, gear_command_103_, false); - can_sender_->AddMessage(Parkcommand104::ID, park_command_104_, false); - can_sender_->AddMessage(Steeringcommand102::ID, steering_command_102_, false); - can_sender_->AddMessage(Vehiclemodecommand105::ID, vehicle_mode_command_105_, - false); + AddSendMessage(); - // need sleep to ensure all messages received AINFO << "DevkitController is initialized."; is_initialized_ = true; @@ -168,10 +164,9 @@ void DevkitController::Stop() { Chassis DevkitController::chassis() { chassis_.Clear(); - Devkit chassis_detail; - message_manager_->GetSensorData(&chassis_detail); + Devkit chassis_detail = GetNewRecvChassisDetail(); - // 21, 22, previously 1, 2 + // 1, 2 // if (driving_mode() == Chassis::EMERGENCY_MODE) { // set_chassis_error_code(Chassis::NO_ERROR); // } @@ -309,16 +304,17 @@ Chassis DevkitController::chassis() { chassis_detail.bms_report_512().has_battery_soc_percentage()) { chassis_.set_battery_soc_percentage( chassis_detail.bms_report_512().battery_soc_percentage()); + // 15 give engage_advice based on battery low soc warn + if (chassis_detail.bms_report_512().is_battery_soc_low()) { + chassis_.mutable_engage_advice()->set_advice( + apollo::common::EngageAdvice::DISALLOW_ENGAGE); + chassis_.mutable_engage_advice()->set_reason( + "Battery soc percentage is lower than 15%, please charge it " + "quickly!"); + } } else { chassis_.set_battery_soc_percentage(0); } - // 15 give engage_advice based on battery low soc warn - if (chassis_.battery_soc_percentage() <= 15) { - chassis_.mutable_engage_advice()->set_advice( - apollo::common::EngageAdvice::DISALLOW_ENGAGE); - chassis_.mutable_engage_advice()->set_reason( - "Battery soc percentage is lower than 15%, please charge it quickly!"); - } // 16 sonor list // to do(ALL):check your vehicle type, confirm your sonar position because of // every vhechle has different sonars assembly. @@ -451,6 +447,7 @@ Chassis DevkitController::chassis() { "Chassis has some fault, please check the chassis_detail."); } + // check the chassis detail lost if (is_chassis_communication_error_) { chassis_.mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::DISALLOW_ENGAGE); @@ -751,68 +748,13 @@ void DevkitController::ResetProtocol() { message_manager_->ResetSendMessages(); } -bool DevkitController::CheckChassisCommunicationError() { - Devkit chassis_detail_receiver; - ADEBUG << "Can receiver finished recv once: " - << can_receiver_->IsFinishRecvOnce(); - if (message_manager_->GetSensorRecvData(&chassis_detail_receiver) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_receiver is " - << chassis_detail_receiver.ShortDebugString(); - size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); - ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; - // check receiver data is null - if (receiver_data_size < 2) { - if (is_need_count_) { - lost_chassis_reveive_detail_count_++; - } - } else { - lost_chassis_reveive_detail_count_ = 0; - is_need_count_ = true; - } - ADEBUG << "lost_chassis_reveive_detail_count_ is " - << lost_chassis_reveive_detail_count_; - // check receive data lost threshold is (100 * 10)ms - if (lost_chassis_reveive_detail_count_ > 100) { - is_need_count_ = false; - is_chassis_communication_error_ = true; - AERROR << "neolix chassis detail is lost, please check the communication " - "error."; - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorData(); - return true; - } else { - is_chassis_communication_error_ = false; - } - - Devkit chassis_detail_sender; - if (message_manager_->GetSensorSenderData(&chassis_detail_sender) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_sender is " - << chassis_detail_sender.ShortDebugString(); - size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); - ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; - - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorSenderData(); - return false; -} - bool DevkitController::CheckChassisError() { if (is_chassis_communication_error_) { AERROR_EVERY(100) << "ChassisDetail has no devkit vehicle info."; return false; } - Devkit chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis detail failed."; - } + Devkit chassis_detail = GetNewRecvChassisDetail(); // steer fault if (chassis_detail.has_steering_report_502()) { if (Steering_report_502::STEER_FLT1_STEER_SYSTEM_HARDWARE_FAULT == @@ -937,6 +879,7 @@ void DevkitController::SecurityDogThreadFunc() { emergency_brake = false; } + // recove error code if (!emergency_mode && !is_chassis_communication_error_ && mode == Chassis::EMERGENCY_MODE) { set_chassis_error_code(Chassis::NO_ERROR); @@ -962,10 +905,6 @@ bool DevkitController::CheckResponse(const int32_t flags, bool need_wait) { bool is_esp_online = 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_.has_check_response() && @@ -995,9 +934,14 @@ bool DevkitController::CheckResponse(const int32_t flags, bool need_wait) { } } while (need_wait && retry_num); - AINFO << "check_response fail: is_eps_online:" << is_eps_online - << ", is_vcu_online:" << is_vcu_online - << ", is_esp_online:" << is_esp_online; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } return false; } diff --git a/modules/canbus_vehicle/devkit/devkit_controller.h b/modules/canbus_vehicle/devkit/devkit_controller.h index 3d0aac182a8..bd1ca36c97d 100755 --- a/modules/canbus_vehicle/devkit/devkit_controller.h +++ b/modules/canbus_vehicle/devkit/devkit_controller.h @@ -47,7 +47,6 @@ class DevkitController final ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Devkit>* const can_sender, - CanReceiver<::apollo::canbus::Devkit>* const can_receiver, MessageManager<::apollo::canbus::Devkit>* const message_manager) override; bool Start() override; @@ -64,9 +63,9 @@ class DevkitController final Chassis chassis() override; /** - * @brief ccheck the chassis detail received or lost. + * @brief add the sender message. */ - bool CheckChassisCommunicationError(); + void AddSendMessage() override; /** * for test @@ -154,9 +153,6 @@ class DevkitController final std::mutex chassis_mask_mutex_; int32_t chassis_error_mask_ = 0; - uint32_t lost_chassis_reveive_detail_count_ = 0; - bool is_need_count_ = true; - bool is_chassis_communication_error_ = false; }; } // namespace devkit diff --git a/modules/canbus_vehicle/devkit/devkit_controller_test.cc b/modules/canbus_vehicle/devkit/devkit_controller_test.cc index 0da83aa4160..cadaec34873 100755 --- a/modules/canbus_vehicle/devkit/devkit_controller_test.cc +++ b/modules/canbus_vehicle/devkit/devkit_controller_test.cc @@ -53,15 +53,13 @@ class DevkitControllerTest : public ::testing::Test { ControlCommand control_cmd_; VehicleSignal vehicle_signal_; CanSender<::apollo::canbus::Devkit> sender_; - CanReceiver<::apollo::canbus::Devkit>* receiver_; DevkitMessageManager msg_manager_; CanbusConf canbus_conf_; VehicleParameter params_; }; TEST_F(DevkitControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -69,14 +67,14 @@ TEST_F(DevkitControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); EXPECT_EQ(controller_.SetDrivingMode(chassis.driving_mode()), ErrorCode::OK); } TEST_F(DevkitControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); controller_.SetHorn(control_cmd_.signal()); @@ -86,7 +84,7 @@ TEST_F(DevkitControllerTest, Status) { } TEST_F(DevkitControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), ErrorCode::OK); diff --git a/modules/canbus_vehicle/devkit/devkit_vehicle_factory.cc b/modules/canbus_vehicle/devkit/devkit_vehicle_factory.cc index 48766ce8667..90e3d649241 100755 --- a/modules/canbus_vehicle/devkit/devkit_vehicle_factory.cc +++ b/modules/canbus_vehicle/devkit/devkit_vehicle_factory.cc @@ -69,7 +69,6 @@ bool DevkitVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -84,6 +83,9 @@ bool DevkitVehicleFactory::Init(const CanbusConf *canbus_conf) { chassis_detail_writer_ = node_->CreateWriter<::apollo::canbus::Devkit>(FLAGS_chassis_detail_topic); + chassis_detail_sender_writer_ = node_->CreateWriter<::apollo::canbus::Devkit>( + FLAGS_chassis_detail_sender_topic); + return true; } @@ -152,12 +154,21 @@ Chassis DevkitVehicleFactory::publish_chassis() { } void DevkitVehicleFactory::PublishChassisDetail() { - Devkit chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - ADEBUG << chassis_detail.ShortDebugString(); + Devkit chassis_detail = vehicle_controller_->GetNewRecvChassisDetail(); + ADEBUG << "latest chassis_detail is " << chassis_detail.ShortDebugString(); chassis_detail_writer_->Write(chassis_detail); } +void DevkitVehicleFactory::PublishChassisDetailSender() { + Devkit sender_chassis_detail = + vehicle_controller_->GetNewSenderChassisDetail(); + ADEBUG << "latest sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + chassis_detail_sender_writer_->Write(sender_chassis_detail); +} + +void DevkitVehicleFactory::UpdateHeartbeat() { can_sender_.Update_Heartbeat(); } + bool DevkitVehicleFactory::CheckChassisCommunicationFault() { if (vehicle_controller_->CheckChassisCommunicationError()) { return true; @@ -165,9 +176,26 @@ bool DevkitVehicleFactory::CheckChassisCommunicationFault() { return false; } -std::unique_ptr +void DevkitVehicleFactory::AddSendProtocol() { + vehicle_controller_->AddSendMessage(); +} + +void DevkitVehicleFactory::ClearSendProtocol() { can_sender_.ClearMessage(); } + +bool DevkitVehicleFactory::IsSendProtocolClear() { + if (can_sender_.IsMessageClear()) { + return true; + } + return false; +} + +Chassis::DrivingMode DevkitVehicleFactory::Driving_Mode() { + return vehicle_controller_->driving_mode(); +} + +std::unique_ptr> DevkitVehicleFactory::CreateVehicleController() { - return std::unique_ptr( + return std::unique_ptr>( new devkit::DevkitController()); } diff --git a/modules/canbus_vehicle/devkit/devkit_vehicle_factory.h b/modules/canbus_vehicle/devkit/devkit_vehicle_factory.h index 95b6982c870..cd819fc071f 100755 --- a/modules/canbus_vehicle/devkit/devkit_vehicle_factory.h +++ b/modules/canbus_vehicle/devkit/devkit_vehicle_factory.h @@ -90,19 +90,53 @@ class DevkitVehicleFactory : public AbstractVehicleFactory { */ Chassis publish_chassis() override; + /** + * @brief create cansender heartbeat + */ + void UpdateHeartbeat() override; + /** * @brief publish chassis for vehicle messages */ void PublishChassisDetail() override; - bool CheckChassisCommunicationFault(); + /** + * @brief publish chassis for apollo sender messages + */ + void PublishChassisDetailSender() override; + + /** + * @brief check chassis can receiver lost + */ + bool CheckChassisCommunicationFault() override; + + /** + * @brief add the can sender messages + */ + void AddSendProtocol() override; + + /** + * @brief clear the can sender messages + */ + void ClearSendProtocol() override; + + /** + * @brief check the sender message clear or not + */ + bool IsSendProtocolClear() override; + + /** + * @brief get the latest chassis driving mode + */ + Chassis::DrivingMode Driving_Mode() override; private: /** * @brief create devkit vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create devkit message manager @@ -116,10 +150,13 @@ class DevkitVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Devkit> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Devkit> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Devkit>> chassis_detail_writer_; + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Devkit>> + chassis_detail_sender_writer_; }; CYBER_REGISTER_VEHICLEFACTORY(DevkitVehicleFactory) diff --git a/modules/canbus_vehicle/ge3/ge3_controller.cc b/modules/canbus_vehicle/ge3/ge3_controller.cc index dd50e54ec06..a686fd28b84 100644 --- a/modules/canbus_vehicle/ge3/ge3_controller.cc +++ b/modules/canbus_vehicle/ge3/ge3_controller.cc @@ -42,7 +42,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode Ge3Controller::Init( const VehicleParameter& params, CanSender* const can_sender, - CanReceiver* const can_receiver, MessageManager<::apollo::canbus::Ge3>* const message_manager) { if (is_initialized_) { AINFO << "Ge3Controller has already been initiated."; @@ -63,12 +62,6 @@ ErrorCode Ge3Controller::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/ge3/ge3_controller.h b/modules/canbus_vehicle/ge3/ge3_controller.h index 328a14768ec..a55f23776ca 100644 --- a/modules/canbus_vehicle/ge3/ge3_controller.h +++ b/modules/canbus_vehicle/ge3/ge3_controller.h @@ -43,7 +43,6 @@ class Ge3Controller final : public VehicleController<::apollo::canbus::Ge3> { ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Ge3>* const can_sender, - CanReceiver* const can_receiver, MessageManager<::apollo::canbus::Ge3>* const message_manager) override; bool Start() override; diff --git a/modules/canbus_vehicle/ge3/ge3_controller_test.cc b/modules/canbus_vehicle/ge3/ge3_controller_test.cc index ae09fef1bb8..ff5225dcd85 100644 --- a/modules/canbus_vehicle/ge3/ge3_controller_test.cc +++ b/modules/canbus_vehicle/ge3/ge3_controller_test.cc @@ -19,12 +19,10 @@ #include #include "gtest/gtest.h" - #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/canbus_vehicle/ge3/proto/ge3.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/common/file.h" #include "modules/canbus_vehicle/ge3/ge3_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -48,7 +46,6 @@ class Ge3ControllerTest : public ::testing::Test { protected: Ge3Controller controller_; CanSender<::apollo::canbus::Ge3> sender_; - CanReceiver<::apollo::canbus::Ge3> receiver_; CanbusConf canbus_conf_; VehicleParameter params_; Ge3MessageManager msg_manager_; @@ -56,8 +53,7 @@ class Ge3ControllerTest : public ::testing::Test { }; TEST_F(Ge3ControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -65,7 +61,7 @@ TEST_F(Ge3ControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); @@ -73,7 +69,7 @@ TEST_F(Ge3ControllerTest, SetDrivingMode) { } TEST_F(Ge3ControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); @@ -85,7 +81,7 @@ TEST_F(Ge3ControllerTest, Status) { } TEST_F(Ge3ControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), diff --git a/modules/canbus_vehicle/ge3/ge3_vehicle_factory.cc b/modules/canbus_vehicle/ge3/ge3_vehicle_factory.cc index a73129f4fca..407a172633e 100644 --- a/modules/canbus_vehicle/ge3/ge3_vehicle_factory.cc +++ b/modules/canbus_vehicle/ge3/ge3_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/ge3/ge3_controller.h" +#include "modules/canbus_vehicle/ge3/ge3_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool Ge3VehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,10 @@ void Ge3VehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> Ge3VehicleFactory::CreateVehicleController() { - return std::unique_ptr(new ge3::Ge3Controller()); + return std::unique_ptr>( + new ge3::Ge3Controller()); } std::unique_ptr> diff --git a/modules/canbus_vehicle/ge3/ge3_vehicle_factory.h b/modules/canbus_vehicle/ge3/ge3_vehicle_factory.h index 1051296722f..34d5d7f6d8f 100644 --- a/modules/canbus_vehicle/ge3/ge3_vehicle_factory.h +++ b/modules/canbus_vehicle/ge3/ge3_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/ge3/proto/ge3.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/ge3/ge3_controller.h" -#include "modules/canbus_vehicle/ge3/ge3_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class Ge3VehicleFactory : public AbstractVehicleFactory { * @brief create ge3 vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create ge3 message manager @@ -113,7 +111,7 @@ class Ge3VehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Ge3> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Ge3> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Ge3>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/gem/gem_controller.cc b/modules/canbus_vehicle/gem/gem_controller.cc index fb47d215bc1..5233fca0044 100644 --- a/modules/canbus_vehicle/gem/gem_controller.cc +++ b/modules/canbus_vehicle/gem/gem_controller.cc @@ -48,7 +48,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode GemController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Gem>* const can_sender, - CanReceiver<::apollo::canbus::Gem>* const can_receiver, MessageManager<::apollo::canbus::Gem>* const message_manager) { if (is_initialized_) { AINFO << "GemController has already been initialized."; @@ -68,12 +67,6 @@ ErrorCode GemController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "Protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/gem/gem_controller.h b/modules/canbus_vehicle/gem/gem_controller.h index d3bd774a2a3..9c454b223c4 100644 --- a/modules/canbus_vehicle/gem/gem_controller.h +++ b/modules/canbus_vehicle/gem/gem_controller.h @@ -47,7 +47,6 @@ class GemController final : public VehicleController<::apollo::canbus::Gem> { ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Gem>* const can_sender, - CanReceiver<::apollo::canbus::Gem>* const can_receiver, MessageManager<::apollo::canbus::Gem>* const message_manager) override; bool Start() override; diff --git a/modules/canbus_vehicle/gem/gem_controller_test.cc b/modules/canbus_vehicle/gem/gem_controller_test.cc index 9efe80ec364..4c67d13a9bd 100644 --- a/modules/canbus_vehicle/gem/gem_controller_test.cc +++ b/modules/canbus_vehicle/gem/gem_controller_test.cc @@ -47,7 +47,6 @@ class GemControllerTest : public ::testing::Test { protected: GemController controller_; CanSender<::apollo::canbus::Gem> sender_; - CanReceiver<::apollo::canbus::Gem> receiver_; CanbusConf canbus_conf_; VehicleParameter params_; GemMessageManager msg_manager_; @@ -55,8 +54,7 @@ class GemControllerTest : public ::testing::Test { }; TEST_F(GemControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -64,7 +62,7 @@ TEST_F(GemControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); @@ -72,7 +70,7 @@ TEST_F(GemControllerTest, SetDrivingMode) { } TEST_F(GemControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); @@ -84,7 +82,7 @@ TEST_F(GemControllerTest, Status) { } TEST_F(GemControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), diff --git a/modules/canbus_vehicle/gem/gem_vehicle_factory.cc b/modules/canbus_vehicle/gem/gem_vehicle_factory.cc index 6a7c14bc90f..6287b6481f6 100644 --- a/modules/canbus_vehicle/gem/gem_vehicle_factory.cc +++ b/modules/canbus_vehicle/gem/gem_vehicle_factory.cc @@ -17,6 +17,9 @@ #include "modules/canbus_vehicle/gem/gem_vehicle_factory.h" #include "cyber/common/log.h" +// #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/gem/gem_controller.h" +#include "modules/canbus_vehicle/gem/gem_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -68,7 +71,6 @@ bool GemVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -157,9 +159,10 @@ void GemVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> GemVehicleFactory::CreateVehicleController() { - return std::unique_ptr(new gem::GemController()); + return std::unique_ptr>( + new gem::GemController()); } std::unique_ptr> diff --git a/modules/canbus_vehicle/gem/gem_vehicle_factory.h b/modules/canbus_vehicle/gem/gem_vehicle_factory.h index b1b685e50e5..8989c334ad1 100644 --- a/modules/canbus_vehicle/gem/gem_vehicle_factory.h +++ b/modules/canbus_vehicle/gem/gem_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/gem/proto/gem.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/gem/gem_controller.h" -#include "modules/canbus_vehicle/gem/gem_message_manager.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -99,7 +96,8 @@ class GemVehicleFactory : public AbstractVehicleFactory { * @brief create Gem vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create Gem message manager @@ -112,7 +110,7 @@ class GemVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Gem> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Gem> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Gem>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/lexus/lexus_controller.cc b/modules/canbus_vehicle/lexus/lexus_controller.cc index 4ea7797a5a1..f7f099fd3b7 100644 --- a/modules/canbus_vehicle/lexus/lexus_controller.cc +++ b/modules/canbus_vehicle/lexus/lexus_controller.cc @@ -44,7 +44,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode LexusController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Lexus>* const can_sender, - CanReceiver<::apollo::canbus::Lexus>* const can_receiver, MessageManager<::apollo::canbus::Lexus>* const message_manager) { if (is_initialized_) { AINFO << "LexusController has already been initiated."; @@ -64,12 +63,6 @@ ErrorCode LexusController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "Protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/lexus/lexus_controller.h b/modules/canbus_vehicle/lexus/lexus_controller.h index 50c1177e05f..dc8ad032d6c 100644 --- a/modules/canbus_vehicle/lexus/lexus_controller.h +++ b/modules/canbus_vehicle/lexus/lexus_controller.h @@ -52,7 +52,6 @@ class LexusController final ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Lexus>* const can_sender, - CanReceiver<::apollo::canbus::Lexus>* const can_receiver, MessageManager<::apollo::canbus::Lexus>* const message_manager) override; bool Start() override; diff --git a/modules/canbus_vehicle/lexus/lexus_controller_test.cc b/modules/canbus_vehicle/lexus/lexus_controller_test.cc index f7184ea22e0..67aa0417708 100644 --- a/modules/canbus_vehicle/lexus/lexus_controller_test.cc +++ b/modules/canbus_vehicle/lexus/lexus_controller_test.cc @@ -16,14 +16,13 @@ #include "modules/canbus_vehicle/lexus/lexus_controller.h" #include "gtest/gtest.h" +#include "cyber/common/file.h" #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/canbus_vehicle/lexus/proto/lexus.pb.h" #include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - -#include "cyber/common/file.h" #include "modules/canbus_vehicle/lexus/lexus_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -53,15 +52,13 @@ class LexusControllerTest : public ::testing::Test { ControlCommand control_cmd_; VehicleSignal vehicle_signal_; CanSender<::apollo::canbus::Lexus> sender_; - CanReceiver<::apollo::canbus::Lexus> receiver_; LexusMessageManager msg_manager_; CanbusConf canbus_conf_; VehicleParameter params_; }; TEST_F(LexusControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -69,14 +66,14 @@ TEST_F(LexusControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); EXPECT_EQ(controller_.SetDrivingMode(chassis.driving_mode()), ErrorCode::OK); } TEST_F(LexusControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); controller_.SetHorn(control_cmd_.signal()); @@ -87,7 +84,7 @@ TEST_F(LexusControllerTest, Status) { } TEST_F(LexusControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), ErrorCode::OK); diff --git a/modules/canbus_vehicle/lexus/lexus_vehicle_factory.cc b/modules/canbus_vehicle/lexus/lexus_vehicle_factory.cc index f49414c96fa..4a79bebef32 100644 --- a/modules/canbus_vehicle/lexus/lexus_vehicle_factory.cc +++ b/modules/canbus_vehicle/lexus/lexus_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/lexus/lexus_controller.h" +#include "modules/canbus_vehicle/lexus/lexus_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool LexusVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,10 @@ void LexusVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> LexusVehicleFactory::CreateVehicleController() { - return std::unique_ptr(new lexus::LexusController()); + return std::unique_ptr>( + new lexus::LexusController()); } std::unique_ptr> diff --git a/modules/canbus_vehicle/lexus/lexus_vehicle_factory.h b/modules/canbus_vehicle/lexus/lexus_vehicle_factory.h index 1bcc15c1c49..53c86083a2d 100644 --- a/modules/canbus_vehicle/lexus/lexus_vehicle_factory.h +++ b/modules/canbus_vehicle/lexus/lexus_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/lexus/proto/lexus.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/lexus/lexus_controller.h" -#include "modules/canbus_vehicle/lexus/lexus_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class LexusVehicleFactory : public AbstractVehicleFactory { * @brief create lexus vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create lexus message manager @@ -114,7 +112,8 @@ class LexusVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Lexus> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Lexus> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Lexus>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/lincoln/lincoln_controller.cc b/modules/canbus_vehicle/lincoln/lincoln_controller.cc index 32d5f813450..592999da22f 100644 --- a/modules/canbus_vehicle/lincoln/lincoln_controller.cc +++ b/modules/canbus_vehicle/lincoln/lincoln_controller.cc @@ -50,7 +50,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode LincolnController::Init( const VehicleParameter ¶ms, CanSender<::apollo::canbus::Lincoln> *const can_sender, - CanReceiver<::apollo::canbus::Lincoln> *const can_receiver, MessageManager<::apollo::canbus::Lincoln> *const message_manager) { if (is_initialized_) { AINFO << "LincolnController has already been initiated."; @@ -70,12 +69,6 @@ ErrorCode LincolnController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "Protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/lincoln/lincoln_controller.h b/modules/canbus_vehicle/lincoln/lincoln_controller.h index 42b49667821..89c9b8b7cb4 100644 --- a/modules/canbus_vehicle/lincoln/lincoln_controller.h +++ b/modules/canbus_vehicle/lincoln/lincoln_controller.h @@ -61,12 +61,10 @@ class LincolnController final * @brief initialize the lincoln vehicle controller. * @return init error_code */ - common::ErrorCode Init( - const VehicleParameter ¶ms, - CanSender<::apollo::canbus::Lincoln> *const can_sender, - CanReceiver<::apollo::canbus::Lincoln> *const can_receiver, - MessageManager<::apollo::canbus::Lincoln> *const message_manager) - override; + common::ErrorCode Init(const VehicleParameter ¶ms, + CanSender<::apollo::canbus::Lincoln> *const can_sender, + MessageManager<::apollo::canbus::Lincoln> + *const message_manager) override; /** * @brief start the vehicle controller. diff --git a/modules/canbus_vehicle/lincoln/lincoln_controller_test.cc b/modules/canbus_vehicle/lincoln/lincoln_controller_test.cc index adb50df3a6c..1d335f5d6a9 100644 --- a/modules/canbus_vehicle/lincoln/lincoln_controller_test.cc +++ b/modules/canbus_vehicle/lincoln/lincoln_controller_test.cc @@ -17,14 +17,13 @@ #include "modules/canbus_vehicle/lincoln/lincoln_controller.h" #include "gtest/gtest.h" +#include "cyber/common/file.h" #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/canbus_vehicle/lincoln/proto/lincoln.pb.h" #include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - -#include "cyber/common/file.h" #include "modules/canbus_vehicle/lincoln/lincoln_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -54,15 +53,13 @@ class LincolnControllerTest : public ::testing::Test { ControlCommand control_cmd_; VehicleSignal vehicle_signal_; CanSender<::apollo::canbus::Lincoln> sender_; - CanReceiver<::apollo::canbus::Lincoln> receiver_; LincolnMessageManager msg_manager_; CanbusConf canbus_conf_; VehicleParameter params_; }; TEST_F(LincolnControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -70,14 +67,14 @@ TEST_F(LincolnControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); EXPECT_EQ(controller_.SetDrivingMode(chassis.driving_mode()), ErrorCode::OK); } TEST_F(LincolnControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); controller_.SetHorn(control_cmd_.signal()); @@ -88,7 +85,7 @@ TEST_F(LincolnControllerTest, Status) { } TEST_F(LincolnControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), ErrorCode::OK); diff --git a/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.cc b/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.cc index b66c9075080..9056dbe3122 100644 --- a/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.cc +++ b/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/lincoln/lincoln_controller.h" +#include "modules/canbus_vehicle/lincoln/lincoln_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool LincolnVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,9 @@ void LincolnVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> LincolnVehicleFactory::CreateVehicleController() { - return std::unique_ptr( + return std::unique_ptr>( new lincoln::LincolnController()); } diff --git a/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.h b/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.h index 438315872b4..b21bb159aa2 100644 --- a/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.h +++ b/modules/canbus_vehicle/lincoln/lincoln_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/lincoln/proto/lincoln.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/lincoln/lincoln_controller.h" -#include "modules/canbus_vehicle/lincoln/lincoln_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class LincolnVehicleFactory : public AbstractVehicleFactory { * @brief create Lincoln vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create lincoln message manager @@ -114,7 +112,8 @@ class LincolnVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Lincoln> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Lincoln> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Lincoln>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.cc b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.cc index d31f7ec05e3..26b1e8f18a5 100644 --- a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.cc +++ b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.cc @@ -41,10 +41,18 @@ const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1; const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; } // namespace +void Neolix_eduController::AddSendMessage() { + can_sender_->AddMessage(Adsbrakecommand46::ID, ads_brake_command_46_, false); + can_sender_->AddMessage(Adsdiagnosis628::ID, ads_diagnosis_628_, false); + can_sender_->AddMessage(Adsdrivecommand50::ID, ads_drive_command_50_, false); + can_sender_->AddMessage(Adsepscommand56::ID, ads_eps_command_56_, false); + can_sender_->AddMessage(Adslighthorncommand310::ID, + ads_light_horn_command_310_, false); +} + ErrorCode Neolix_eduController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Neolix_edu>* const can_sender, - CanReceiver<::apollo::canbus::Neolix_edu>* const can_receiver, MessageManager<::apollo::canbus::Neolix_edu>* const message_manager) { if (is_initialized_) { AINFO << "Neolix_eduController has already been initiated."; @@ -64,12 +72,6 @@ ErrorCode Neolix_eduController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; @@ -115,14 +117,8 @@ ErrorCode Neolix_eduController::Init( return ErrorCode::CANBUS_ERROR; } - can_sender_->AddMessage(Adsbrakecommand46::ID, ads_brake_command_46_, false); - can_sender_->AddMessage(Adsdiagnosis628::ID, ads_diagnosis_628_, false); - can_sender_->AddMessage(Adsdrivecommand50::ID, ads_drive_command_50_, false); - can_sender_->AddMessage(Adsepscommand56::ID, ads_eps_command_56_, false); - can_sender_->AddMessage(Adslighthorncommand310::ID, - ads_light_horn_command_310_, false); + AddSendMessage(); - // need sleep to ensure all messages received AINFO << "Neolix_eduController is initialized."; is_initialized_ = true; @@ -158,10 +154,9 @@ void Neolix_eduController::Stop() { Chassis Neolix_eduController::chassis() { chassis_.Clear(); - Neolix_edu chassis_detail; - message_manager_->GetSensorData(&chassis_detail); + Neolix_edu chassis_detail = GetNewRecvChassisDetail(); - // 21, 22, previously 1, 2 + // 1, 2 // if (driving_mode() == Chassis::EMERGENCY_MODE) { // set_chassis_error_code(Chassis::NO_ERROR); // } @@ -301,6 +296,7 @@ Chassis Neolix_eduController::chassis() { "Chassis has some fault, please check the chassis_detail."); } + // check the chassis detail lost if (is_chassis_communication_error_) { chassis_.mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::DISALLOW_ENGAGE); @@ -508,57 +504,6 @@ void Neolix_eduController::ResetProtocol() { message_manager_->ResetSendMessages(); } -bool Neolix_eduController::CheckChassisCommunicationError() { - Neolix_edu chassis_detail_receiver; - ADEBUG << "Can receiver finished recv once: " - << can_receiver_->IsFinishRecvOnce(); - if (message_manager_->GetSensorRecvData(&chassis_detail_receiver) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_receiver is " - << chassis_detail_receiver.ShortDebugString(); - size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); - ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; - // check receiver data is null - if (receiver_data_size < 2) { - if (is_need_count_) { - lost_chassis_reveive_detail_count_++; - } - } else { - lost_chassis_reveive_detail_count_ = 0; - is_need_count_ = true; - } - ADEBUG << "lost_chassis_reveive_detail_count_ is " - << lost_chassis_reveive_detail_count_; - // check receive data lost threshold is (100 * 10)ms - if (lost_chassis_reveive_detail_count_ > 100) { - is_need_count_ = false; - is_chassis_communication_error_ = true; - AERROR << "neolix chassis detail is lost, please check the communication " - "error."; - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorData(); - return true; - } else { - is_chassis_communication_error_ = false; - } - - Neolix_edu chassis_detail_sender; - if (message_manager_->GetSensorSenderData(&chassis_detail_sender) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_sender is " - << chassis_detail_sender.ShortDebugString(); - size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); - ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; - - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorSenderData(); - return false; -} - bool Neolix_eduController::CheckChassisError() { if (is_chassis_communication_error_) { AERROR_EVERY(100) << "ChassisDetail has no neolix vehicle info."; @@ -630,6 +575,7 @@ void Neolix_eduController::SecurityDogThreadFunc() { can_sender_->Update(); } + // recove error code if (!emergency_mode && !is_chassis_communication_error_ && mode == Chassis::EMERGENCY_MODE) { set_chassis_error_code(Chassis::NO_ERROR); @@ -653,13 +599,8 @@ bool Neolix_eduController::CheckResponse(const int32_t flags, bool need_wait) { bool is_eps_online = false; bool is_vcu_online = false; bool is_esp_online = false; - Chassis chassis = Chassis(); 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_.has_check_response() && @@ -689,9 +630,14 @@ bool Neolix_eduController::CheckResponse(const int32_t flags, bool need_wait) { } } while (need_wait && retry_num); - AINFO << "check_response fail: is_eps_online:" << is_eps_online - << ", is_vcu_online:" << is_vcu_online - << ", is_esp_online:" << is_esp_online; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } return false; } diff --git a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.h b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.h index 9f97196400c..1b33d0a1775 100644 --- a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.h +++ b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller.h @@ -44,7 +44,6 @@ class Neolix_eduController final ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Neolix_edu>* const can_sender, - CanReceiver<::apollo::canbus::Neolix_edu>* const can_receiver, MessageManager<::apollo::canbus::Neolix_edu>* const message_manager) override; @@ -60,12 +59,19 @@ class Neolix_eduController final * @returns a copy of chassis. Use copy here to avoid multi-thread issues. */ Chassis chassis() override; + + /** + * @brief add the sender message. + */ + void AddSendMessage() override; + + /** + * for test + */ FRIEND_TEST(Neolix_eduControllerTest, SetDrivingMode); FRIEND_TEST(Neolix_eduControllerTest, Status); FRIEND_TEST(Neolix_eduControllerTest, UpdateDrivingMode); - bool CheckChassisCommunicationError(); - private: // main logical function for operation the car enter or exit the auto driving void Emergency() override; @@ -136,9 +142,6 @@ class Neolix_eduController final std::mutex chassis_mask_mutex_; int32_t chassis_error_mask_ = 0; - uint32_t lost_chassis_reveive_detail_count_ = 0; - bool is_need_count_ = true; - bool is_chassis_communication_error_ = false; }; } // namespace neolix_edu diff --git a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller_test.cc b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller_test.cc index 459704f78ae..af8577da96e 100644 --- a/modules/canbus_vehicle/neolix_edu/neolix_edu_controller_test.cc +++ b/modules/canbus_vehicle/neolix_edu/neolix_edu_controller_test.cc @@ -54,15 +54,13 @@ class Neolix_eduControllerTest : public ::testing::Test { ControlCommand control_cmd_; VehicleSignal vehicle_signal_; CanSender<::apollo::canbus::Neolix_edu> sender_; - CanReceiver<::apollo::canbus::Neolix_edu> receiver_; Neolix_eduMessageManager msg_manager_; CanbusConf canbus_conf_; VehicleParameter params_; }; TEST_F(Neolix_eduControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -70,14 +68,14 @@ TEST_F(Neolix_eduControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); EXPECT_EQ(controller_.SetDrivingMode(chassis.driving_mode()), ErrorCode::OK); } TEST_F(Neolix_eduControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); controller_.SetHorn(control_cmd_.signal()); @@ -88,7 +86,7 @@ TEST_F(Neolix_eduControllerTest, Status) { } TEST_F(Neolix_eduControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), ErrorCode::OK); diff --git a/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.cc b/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.cc index 0195e626406..b8ad55c2bc2 100644 --- a/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.cc +++ b/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.cc @@ -69,7 +69,6 @@ bool Neolix_eduVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -84,6 +83,10 @@ bool Neolix_eduVehicleFactory::Init(const CanbusConf *canbus_conf) { chassis_detail_writer_ = node_->CreateWriter<::apollo::canbus::Neolix_edu>( FLAGS_chassis_detail_topic); + chassis_detail_sender_writer_ = + node_->CreateWriter<::apollo::canbus::Neolix_edu>( + FLAGS_chassis_detail_sender_topic); + return true; } @@ -152,12 +155,23 @@ Chassis Neolix_eduVehicleFactory::publish_chassis() { } void Neolix_eduVehicleFactory::PublishChassisDetail() { - Neolix_edu chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - ADEBUG << chassis_detail.ShortDebugString(); + Neolix_edu chassis_detail = vehicle_controller_->GetNewRecvChassisDetail(); + ADEBUG << "latest chassis_detail is " << chassis_detail.ShortDebugString(); chassis_detail_writer_->Write(chassis_detail); } +void Neolix_eduVehicleFactory::PublishChassisDetailSender() { + Neolix_edu sender_chassis_detail = + vehicle_controller_->GetNewSenderChassisDetail(); + ADEBUG << "latest sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + chassis_detail_sender_writer_->Write(sender_chassis_detail); +} + +void Neolix_eduVehicleFactory::UpdateHeartbeat() { + can_sender_.Update_Heartbeat(); +} + bool Neolix_eduVehicleFactory::CheckChassisCommunicationFault() { if (vehicle_controller_->CheckChassisCommunicationError()) { return true; @@ -165,9 +179,28 @@ bool Neolix_eduVehicleFactory::CheckChassisCommunicationFault() { return false; } -std::unique_ptr +void Neolix_eduVehicleFactory::AddSendProtocol() { + vehicle_controller_->AddSendMessage(); +} + +void Neolix_eduVehicleFactory::ClearSendProtocol() { + can_sender_.ClearMessage(); +} + +bool Neolix_eduVehicleFactory::IsSendProtocolClear() { + if (can_sender_.IsMessageClear()) { + return true; + } + return false; +} + +Chassis::DrivingMode Neolix_eduVehicleFactory::Driving_Mode() { + return vehicle_controller_->driving_mode(); +} + +std::unique_ptr> Neolix_eduVehicleFactory::CreateVehicleController() { - return std::unique_ptr( + return std::unique_ptr>( new neolix_edu::Neolix_eduController()); } diff --git a/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.h b/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.h index 7eb9609c88d..60a36fe7190 100644 --- a/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.h +++ b/modules/canbus_vehicle/neolix_edu/neolix_edu_vehicle_factory.h @@ -90,19 +90,53 @@ class Neolix_eduVehicleFactory : public AbstractVehicleFactory { */ Chassis publish_chassis() override; + /** + * @brief create cansender heartbeat + */ + void UpdateHeartbeat() override; + /** * @brief publish chassis for vehicle messages */ void PublishChassisDetail() override; - bool CheckChassisCommunicationFault(); + /** + * @brief publish chassis for apollo sender messages + */ + void PublishChassisDetailSender() override; + + /** + * @brief check chassis can receiver lost + */ + bool CheckChassisCommunicationFault() override; + + /** + * @brief add the can sender messages + */ + void AddSendProtocol() override; + + /** + * @brief clear the can sender messages + */ + void ClearSendProtocol() override; + + /** + * @brief check the sender message clear or not + */ + bool IsSendProtocolClear() override; + + /** + * @brief get the latest chassis driving mode + */ + Chassis::DrivingMode Driving_Mode() override; private: /** * @brief create Neolix_edu vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create Neolix_edu message manager @@ -118,10 +152,13 @@ class Neolix_eduVehicleFactory : public AbstractVehicleFactory { can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Neolix_edu>> chassis_detail_writer_; + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Neolix_edu>> + chassis_detail_sender_writer_; }; CYBER_REGISTER_VEHICLEFACTORY(Neolix_eduVehicleFactory) diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.cc b/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.cc index cbc6b393f79..26aec65493a 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.cc +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.cc @@ -36,9 +36,13 @@ uint32_t Adsbrakecommand46::GetPeriod() const { } void Adsbrakecommand46::Parse(const std::uint8_t* bytes, int32_t length, - Neolix_edu* chassis) const { + Neolix_edu* chassis) const { chassis->mutable_ads_brake_command_46()->set_drive_enable( drive_enable(bytes, length)); + chassis->mutable_ads_brake_command_46()->set_auto_brake_command( + auto_brake_command(bytes, length)); + chassis->mutable_ads_brake_command_46()->set_auto_parking_command( + auto_parking_command(bytes, length)); } void Adsbrakecommand46::UpdateData(uint8_t* data) { @@ -80,8 +84,8 @@ void Adsbrakecommand46::set_p_drive_enable(uint8_t* data, bool drive_enable) { to_set.set_value(x, 0, 1); } -bool Adsbrakecommand46::drive_enable( - const std::uint8_t* bytes, int32_t length) const { +bool Adsbrakecommand46::drive_enable(const std::uint8_t* bytes, + int32_t length) const { Byte t0(bytes + 0); int32_t x = t0.get_byte(0, 1); @@ -107,6 +111,15 @@ void Adsbrakecommand46::set_p_auto_brake_command(uint8_t* data, to_set.set_value(x, 0, 8); } +int32_t Adsbrakecommand46::auto_brake_command(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + int32_t ret = x; + return ret; +} + Adsbrakecommand46* Adsbrakecommand46::set_auto_parking_command( bool auto_parking_command) { auto_parking_command_ = auto_parking_command; @@ -125,6 +138,15 @@ void Adsbrakecommand46::set_p_auto_parking_command(uint8_t* data, to_set.set_value(x, 0, 1); } +bool Adsbrakecommand46::auto_parking_command(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 3); + int32_t x = t0.get_byte(0, 1); + + bool ret = x; + return ret; +} + Adsbrakecommand46* Adsbrakecommand46::set_epb_rampauxiliarycommand( bool epb_rampauxiliarycommand) { epb_rampauxiliarycommand_ = epb_rampauxiliarycommand; diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.h b/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.h index 0a430c6c368..b5cc50072a1 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.h +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_brake_command_46.h @@ -17,6 +17,7 @@ #pragma once #include "modules/canbus_vehicle/neolix_edu/proto/neolix_edu.pb.h" + #include "modules/drivers/canbus/can_comm/protocol_data.h" namespace apollo { @@ -119,6 +120,10 @@ class Adsbrakecommand46 : public ::apollo::drivers::canbus::ProtocolData< bool drive_enable(const std::uint8_t* bytes, const int32_t length) const; + bool auto_parking_command(const std::uint8_t* bytes, int32_t length) const; + + int32_t auto_brake_command(const std::uint8_t* bytes, int32_t length) const; + private: bool drive_enable_; int auto_brake_command_; diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.cc b/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.cc index 835c733314d..010e5294e63 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.cc +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.cc @@ -36,9 +36,13 @@ uint32_t Adsdrivecommand50::GetPeriod() const { } void Adsdrivecommand50::Parse(const std::uint8_t* bytes, int32_t length, - Neolix_edu* chassis) const { + Neolix_edu* chassis) const { chassis->mutable_ads_drive_command_50()->set_drive_enable( drive_enable(bytes, length)); + chassis->mutable_ads_drive_command_50()->set_auto_shift_command( + auto_shift_command(bytes, length)); + chassis->mutable_ads_drive_command_50()->set_auto_drive_torque( + auto_drive_torque(bytes, length)); } void Adsdrivecommand50::UpdateData(uint8_t* data) { @@ -78,8 +82,8 @@ void Adsdrivecommand50::set_p_drive_enable(uint8_t* data, bool drive_enable) { to_set.set_value(x, 0, 1); } -bool Adsdrivecommand50::drive_enable( - const std::uint8_t* bytes, int32_t length) const { +bool Adsdrivecommand50::drive_enable(const std::uint8_t* bytes, + int32_t length) const { Byte t0(bytes + 0); int32_t x = t0.get_byte(0, 1); @@ -108,6 +112,17 @@ void Adsdrivecommand50::set_p_auto_shift_command( to_set.set_value(x, 0, 2); } +Ads_drive_command_50::Auto_shift_commandType +Adsdrivecommand50::auto_shift_command(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 1); + int32_t x = t0.get_byte(0, 2); + + Ads_drive_command_50::Auto_shift_commandType ret = + static_cast(x); + return ret; +} + Adsdrivecommand50* Adsdrivecommand50::set_auto_drive_torque( double auto_drive_torque) { auto_drive_torque_ = auto_drive_torque; @@ -133,6 +148,20 @@ void Adsdrivecommand50::set_p_auto_drive_torque(uint8_t* data, to_set1.set_value(t, 0, 8); } +double Adsdrivecommand50::auto_drive_torque(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = (x * 0.020000) + -665.000000; + return ret; +} + Adsdrivecommand50* Adsdrivecommand50::set_auto_drivercmd_alivecounter( int auto_drivercmd_alivecounter) { auto_drivercmd_alivecounter_ = auto_drivercmd_alivecounter; diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.h b/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.h index dc6f5c219e4..fdccad4e290 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.h +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_drive_command_50.h @@ -17,6 +17,7 @@ #pragma once #include "modules/canbus_vehicle/neolix_edu/proto/neolix_edu.pb.h" + #include "modules/drivers/canbus/can_comm/protocol_data.h" namespace apollo { @@ -110,6 +111,11 @@ class Adsdrivecommand50 : public ::apollo::drivers::canbus::ProtocolData< bool drive_enable(const std::uint8_t* bytes, const int32_t length) const; + Ads_drive_command_50::Auto_shift_commandType auto_shift_command( + const std::uint8_t* bytes, int32_t length) const; + + double auto_drive_torque(const std::uint8_t* bytes, int32_t length) const; + private: bool drive_enable_; Ads_drive_command_50::Auto_shift_commandType auto_shift_command_; diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.cc b/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.cc index 05fb23565b8..1909f7d7244 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.cc +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.cc @@ -36,9 +36,11 @@ uint32_t Adsepscommand56::GetPeriod() const { } void Adsepscommand56::Parse(const std::uint8_t* bytes, int32_t length, - Neolix_edu* chassis) const { + Neolix_edu* chassis) const { chassis->mutable_ads_eps_command_56()->set_drive_enable( drive_enable(bytes, length)); + chassis->mutable_ads_eps_command_56()->set_auto_target_angle( + auto_target_angle(bytes, length)); } void Adsepscommand56::UpdateData(uint8_t* data) { @@ -76,8 +78,8 @@ void Adsepscommand56::set_p_drive_enable(uint8_t* data, bool drive_enable) { to_set.set_value(x, 0, 1); } -bool Adsepscommand56::drive_enable( - const std::uint8_t* bytes, int32_t length) const { +bool Adsepscommand56::drive_enable(const std::uint8_t* bytes, + int32_t length) const { Byte t0(bytes + 0); int32_t x = t0.get_byte(0, 1); @@ -111,6 +113,20 @@ void Adsepscommand56::set_p_auto_target_angle(uint8_t* data, to_set1.set_value(t, 0, 8); } +double Adsepscommand56::auto_target_angle(const std::uint8_t* bytes, + int32_t length) const { + Byte t0(bytes + 2); + int32_t x = t0.get_byte(0, 8); + + Byte t1(bytes + 3); + int32_t t = t1.get_byte(0, 8); + x <<= 8; + x |= t; + + double ret = 0.0625 * x + -2048; + return ret; +} + Adsepscommand56* Adsepscommand56::set_auto_drivercmd_alivecounter( int auto_drivercmd_alivecounter) { auto_drivercmd_alivecounter_ = auto_drivercmd_alivecounter; diff --git a/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.h b/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.h index 08f49da09f6..356e943fa72 100644 --- a/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.h +++ b/modules/canbus_vehicle/neolix_edu/protocol/ads_eps_command_56.h @@ -17,6 +17,7 @@ #pragma once #include "modules/canbus_vehicle/neolix_edu/proto/neolix_edu.pb.h" + #include "modules/drivers/canbus/can_comm/protocol_data.h" namespace apollo { @@ -93,6 +94,9 @@ class Adsepscommand56 : public ::apollo::drivers::canbus::ProtocolData< bool drive_enable(const std::uint8_t* bytes, const int32_t length) const; + double auto_target_angle(const std::uint8_t* bytes, + const int32_t length) const; + private: bool drive_enable_; double auto_target_angle_; diff --git a/modules/canbus_vehicle/transit/transit_controller.cc b/modules/canbus_vehicle/transit/transit_controller.cc index ba35be17184..a0e8f47b227 100644 --- a/modules/canbus_vehicle/transit/transit_controller.cc +++ b/modules/canbus_vehicle/transit/transit_controller.cc @@ -45,7 +45,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode TransitController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Transit>* const can_sender, - CanReceiver<::apollo::canbus::Transit>* const can_receiver, MessageManager<::apollo::canbus::Transit>* const message_manager) { if (is_initialized_) { AINFO << "TransitController has already been initialized."; @@ -67,12 +66,6 @@ ErrorCode TransitController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/transit/transit_controller.h b/modules/canbus_vehicle/transit/transit_controller.h index 086c8996edb..09ac67849c8 100644 --- a/modules/canbus_vehicle/transit/transit_controller.h +++ b/modules/canbus_vehicle/transit/transit_controller.h @@ -47,7 +47,6 @@ class TransitController final ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Transit>* const can_sender, - CanReceiver<::apollo::canbus::Transit>* const can_receiver, MessageManager<::apollo::canbus::Transit>* const message_manager) override; diff --git a/modules/canbus_vehicle/transit/transit_controller_test.cc b/modules/canbus_vehicle/transit/transit_controller_test.cc index 11536c0ba16..8358fa66a4f 100644 --- a/modules/canbus_vehicle/transit/transit_controller_test.cc +++ b/modules/canbus_vehicle/transit/transit_controller_test.cc @@ -19,12 +19,11 @@ #include #include "gtest/gtest.h" +#include "cyber/common/file.h" #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - -#include "cyber/common/file.h" #include "modules/canbus_vehicle/transit/transit_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -47,7 +46,6 @@ class TransitControllerTest : public ::testing::Test { protected: TransitController controller_; CanSender<::apollo::canbus::Transit> sender_; - CanReceiver<::apollo::canbus::Transit> receiver_; CanbusConf canbus_conf_; VehicleParameter params_; TransitMessageManager msg_manager_; @@ -55,8 +53,7 @@ class TransitControllerTest : public ::testing::Test { }; TEST_F(TransitControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -64,7 +61,7 @@ TEST_F(TransitControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); @@ -72,7 +69,7 @@ TEST_F(TransitControllerTest, SetDrivingMode) { } TEST_F(TransitControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); @@ -84,7 +81,7 @@ TEST_F(TransitControllerTest, Status) { } TEST_F(TransitControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), diff --git a/modules/canbus_vehicle/transit/transit_vehicle_factory.cc b/modules/canbus_vehicle/transit/transit_vehicle_factory.cc index 44c9fca3f3d..2beec9c771e 100644 --- a/modules/canbus_vehicle/transit/transit_vehicle_factory.cc +++ b/modules/canbus_vehicle/transit/transit_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/transit/transit_controller.h" +#include "modules/canbus_vehicle/transit/transit_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool TransitVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,9 @@ void TransitVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> TransitVehicleFactory::CreateVehicleController() { - return std::unique_ptr( + return std::unique_ptr>( new transit::TransitController()); } diff --git a/modules/canbus_vehicle/transit/transit_vehicle_factory.h b/modules/canbus_vehicle/transit/transit_vehicle_factory.h index e7b82ed2622..b196d5232d5 100644 --- a/modules/canbus_vehicle/transit/transit_vehicle_factory.h +++ b/modules/canbus_vehicle/transit/transit_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/transit/proto/transit.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/transit/transit_controller.h" -#include "modules/canbus_vehicle/transit/transit_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class TransitVehicleFactory : public AbstractVehicleFactory { * @brief create transit vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create transit message manager @@ -114,7 +112,8 @@ class TransitVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Transit> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Transit> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Transit>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/wey/wey_controller.cc b/modules/canbus_vehicle/wey/wey_controller.cc index 74594be4e71..278d27917b4 100644 --- a/modules/canbus_vehicle/wey/wey_controller.cc +++ b/modules/canbus_vehicle/wey/wey_controller.cc @@ -43,7 +43,6 @@ double angle_init = 0; ErrorCode WeyController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Wey>* const can_sender, - CanReceiver<::apollo::canbus::Wey>* const can_receiver, MessageManager<::apollo::canbus::Wey>* const message_manager) { if (is_initialized_) { AINFO << "WeyController has already been initialized."; @@ -63,12 +62,6 @@ ErrorCode WeyController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "Protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/wey/wey_controller.h b/modules/canbus_vehicle/wey/wey_controller.h index 272246bc682..e4b30eb5588 100644 --- a/modules/canbus_vehicle/wey/wey_controller.h +++ b/modules/canbus_vehicle/wey/wey_controller.h @@ -45,7 +45,6 @@ class WeyController final : public VehicleController<::apollo::canbus::Wey> { ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Wey>* const can_sender, - CanReceiver<::apollo::canbus::Wey>* const can_receiver, MessageManager<::apollo::canbus::Wey>* const message_manager) override; bool Start() override; diff --git a/modules/canbus_vehicle/wey/wey_controller_test.cc b/modules/canbus_vehicle/wey/wey_controller_test.cc index 26e52245f14..ea61a3beaca 100644 --- a/modules/canbus_vehicle/wey/wey_controller_test.cc +++ b/modules/canbus_vehicle/wey/wey_controller_test.cc @@ -19,13 +19,12 @@ #include #include "gtest/gtest.h" +#include "cyber/common/file.h" #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/canbus_vehicle/wey/proto/wey.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - -#include "cyber/common/file.h" #include "modules/canbus_vehicle/wey/wey_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -48,7 +47,6 @@ class WeyControllerTest : public ::testing::Test { protected: WeyController controller_; CanSender<::apollo::canbus::Wey> sender_; - CanReceiver<::apollo::canbus::Wey> receiver_; CanbusConf canbus_conf_; VehicleParameter params_; WeyMessageManager msg_manager_; @@ -56,8 +54,7 @@ class WeyControllerTest : public ::testing::Test { }; TEST_F(WeyControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -65,7 +62,7 @@ TEST_F(WeyControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); @@ -73,7 +70,7 @@ TEST_F(WeyControllerTest, SetDrivingMode) { } TEST_F(WeyControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); @@ -85,7 +82,7 @@ TEST_F(WeyControllerTest, Status) { } TEST_F(WeyControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), diff --git a/modules/canbus_vehicle/wey/wey_vehicle_factory.cc b/modules/canbus_vehicle/wey/wey_vehicle_factory.cc index 6dc4e51a10e..bfb137f0940 100644 --- a/modules/canbus_vehicle/wey/wey_vehicle_factory.cc +++ b/modules/canbus_vehicle/wey/wey_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/wey/wey_controller.h" +#include "modules/canbus_vehicle/wey/wey_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool WeyVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,10 @@ void WeyVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> WeyVehicleFactory::CreateVehicleController() { - return std::unique_ptr(new wey::WeyController()); + return std::unique_ptr>( + new wey::WeyController()); } std::unique_ptr> diff --git a/modules/canbus_vehicle/wey/wey_vehicle_factory.h b/modules/canbus_vehicle/wey/wey_vehicle_factory.h index 8dda89a2a21..6abbe2a79ea 100644 --- a/modules/canbus_vehicle/wey/wey_vehicle_factory.h +++ b/modules/canbus_vehicle/wey/wey_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/wey/proto/wey.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/wey/wey_controller.h" -#include "modules/canbus_vehicle/wey/wey_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class WeyVehicleFactory : public AbstractVehicleFactory { * @brief create wey vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create wey message manager @@ -113,7 +111,7 @@ class WeyVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::Wey> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::Wey> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Wey>> chassis_detail_writer_; diff --git a/modules/canbus_vehicle/zhongyun/zhongyun_controller.cc b/modules/canbus_vehicle/zhongyun/zhongyun_controller.cc index 0562a79bf24..0aa3b9c0f12 100644 --- a/modules/canbus_vehicle/zhongyun/zhongyun_controller.cc +++ b/modules/canbus_vehicle/zhongyun/zhongyun_controller.cc @@ -42,7 +42,6 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; ErrorCode ZhongyunController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::Zhongyun>* const can_sender, - CanReceiver<::apollo::canbus::Zhongyun>* const can_receiver, MessageManager<::apollo::canbus::Zhongyun>* const message_manager) { if (is_initialized_) { AINFO << "ZhongyunController has already been initialized."; @@ -62,12 +61,6 @@ ErrorCode ZhongyunController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "Protocol manager is null."; return ErrorCode::CANBUS_ERROR; diff --git a/modules/canbus_vehicle/zhongyun/zhongyun_controller.h b/modules/canbus_vehicle/zhongyun/zhongyun_controller.h index 6386f28891b..7340d2d8529 100644 --- a/modules/canbus_vehicle/zhongyun/zhongyun_controller.h +++ b/modules/canbus_vehicle/zhongyun/zhongyun_controller.h @@ -46,7 +46,6 @@ class ZhongyunController final ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::Zhongyun>* const can_sender, - CanReceiver<::apollo::canbus::Zhongyun>* const can_receiver, MessageManager<::apollo::canbus::Zhongyun>* const message_manager) override; diff --git a/modules/canbus_vehicle/zhongyun/zhongyun_controller_test.cc b/modules/canbus_vehicle/zhongyun/zhongyun_controller_test.cc index 0bf1781e60c..f5bf21061e3 100644 --- a/modules/canbus_vehicle/zhongyun/zhongyun_controller_test.cc +++ b/modules/canbus_vehicle/zhongyun/zhongyun_controller_test.cc @@ -14,8 +14,6 @@ * limitations under the License. *****************************************************************************/ -#include "modules/canbus_vehicle/zhongyun/zhongyun_controller.h" - #include #include "gtest/gtest.h" @@ -24,8 +22,8 @@ #include "modules/canbus_vehicle/zhongyun/proto/zhongyun.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/common/file.h" +#include "modules/canbus_vehicle/zhongyun/zhongyun_controller.h" #include "modules/canbus_vehicle/zhongyun/zhongyun_message_manager.h" #include "modules/drivers/canbus/can_comm/can_sender.h" @@ -48,7 +46,6 @@ class ZhongyunControllerTest : public ::testing::Test { protected: ZhongyunController controller_; CanSender sender_; - CanReceiver receiver_; CanbusConf canbus_conf_; VehicleParameter params_; ZhongyunMessageManager msg_manager_; @@ -56,8 +53,7 @@ class ZhongyunControllerTest : public ::testing::Test { }; TEST_F(ZhongyunControllerTest, Init) { - ErrorCode ret = - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + ErrorCode ret = controller_.Init(params_, &sender_, &msg_manager_); EXPECT_EQ(ret, ErrorCode::OK); } @@ -65,7 +61,7 @@ TEST_F(ZhongyunControllerTest, SetDrivingMode) { Chassis chassis; chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(chassis.driving_mode()); EXPECT_EQ(controller_.driving_mode(), chassis.driving_mode()); @@ -73,7 +69,7 @@ TEST_F(ZhongyunControllerTest, SetDrivingMode) { } TEST_F(ZhongyunControllerTest, Status) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.Update(control_cmd_), ErrorCode::OK); @@ -85,7 +81,7 @@ TEST_F(ZhongyunControllerTest, Status) { } TEST_F(ZhongyunControllerTest, UpdateDrivingMode) { - controller_.Init(params_, &sender_, &receiver_, &msg_manager_); + controller_.Init(params_, &sender_, &msg_manager_); controller_.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE); EXPECT_EQ(controller_.SetDrivingMode(Chassis::COMPLETE_MANUAL), diff --git a/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.cc b/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.cc index 52db011c145..09588ddcd6f 100644 --- a/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.cc +++ b/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.cc @@ -18,6 +18,8 @@ #include "cyber/common/log.h" #include "modules/canbus/common/canbus_gflags.h" +#include "modules/canbus_vehicle/zhongyun/zhongyun_controller.h" +#include "modules/canbus_vehicle/zhongyun/zhongyun_message_manager.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/util.h" #include "modules/drivers/canbus/can_client/can_client_factory.h" @@ -69,7 +71,6 @@ bool ZhongyunVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -158,9 +159,9 @@ void ZhongyunVehicleFactory::PublishChassisDetail() { chassis_detail_writer_->Write(chassis_detail); } -std::unique_ptr +std::unique_ptr> ZhongyunVehicleFactory::CreateVehicleController() { - return std::unique_ptr( + return std::unique_ptr>( new zhongyun::ZhongyunController()); } diff --git a/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.h b/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.h index 223c26734b2..213261a97c8 100644 --- a/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.h +++ b/modules/canbus_vehicle/zhongyun/zhongyun_vehicle_factory.h @@ -26,12 +26,9 @@ #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus_vehicle/zhongyun/proto/zhongyun.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" - #include "cyber/cyber.h" #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/canbus/vehicle/vehicle_controller.h" -#include "modules/canbus_vehicle/zhongyun/zhongyun_controller.h" -#include "modules/canbus_vehicle/zhongyun/zhongyun_message_manager.h" #include "modules/common/status/status.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" @@ -100,7 +97,8 @@ class ZhongyunVehicleFactory : public AbstractVehicleFactory { * @brief create zhongyun vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create zhongyun message manager @@ -115,7 +113,8 @@ class ZhongyunVehicleFactory : public AbstractVehicleFactory { apollo::drivers::canbus::CanReceiver<::apollo::canbus::Zhongyun> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr vehicle_controller_; + std::unique_ptr> + vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::Zhongyun>> chassis_detail_writer_; diff --git a/modules/common/adapters/adapter_gflags.cc b/modules/common/adapters/adapter_gflags.cc index 1cf46de6d75..c144b701ca0 100644 --- a/modules/common/adapters/adapter_gflags.cc +++ b/modules/common/adapters/adapter_gflags.cc @@ -27,6 +27,9 @@ DEFINE_string(audio_detection_topic, "/apollo/audio_detection", DEFINE_string(chassis_topic, "/apollo/canbus/chassis", "chassis topic name"); DEFINE_string(chassis_detail_topic, "/apollo/canbus/chassis_detail", "chassis detail topic name"); +DEFINE_string(chassis_detail_sender_topic, + "/apollo/canbus/chassis_detail_sender", + "chassis detail sender_topic name"); DEFINE_string(chassis_command_topic, "/apollo/chassis_control", "chassis command topic name"); DEFINE_string(localization_topic, "/apollo/localization/pose", diff --git a/modules/common/adapters/adapter_gflags.h b/modules/common/adapters/adapter_gflags.h index 7a653dc5241..98d22192329 100644 --- a/modules/common/adapters/adapter_gflags.h +++ b/modules/common/adapters/adapter_gflags.h @@ -25,6 +25,7 @@ DECLARE_string(raw_imu_topic); DECLARE_string(audio_detection_topic); DECLARE_string(chassis_topic); DECLARE_string(chassis_detail_topic); +DECLARE_string(chassis_detail_sender_topic); DECLARE_string(chassis_command_topic); DECLARE_string(localization_topic); DECLARE_string(planning_learning_data_topic); diff --git a/modules/drivers/canbus/BUILD b/modules/drivers/canbus/BUILD index acc15fb453b..8c60ff151be 100644 --- a/modules/drivers/canbus/BUILD +++ b/modules/drivers/canbus/BUILD @@ -13,7 +13,7 @@ apollo_cc_library( "can_client/hermes_can/hermes_can_client.cc", "can_client/socket/socket_can_client_raw.cc", "common/byte.cc", - ] + if_esd_can(["can_client/esd/esd_can_client.cc",]), + ], hdrs = [ "sensor_gflags.h", "can_client/can_client.h", @@ -30,8 +30,7 @@ apollo_cc_library( "common/byte.cc", "common/byte.h", "common/canbus_consts.h", - ] + if_esd_can(["can_client/esd/esd_can_client.h",]), - copts = copts_if_esd_can(), + ], deps = [ "//cyber", "//modules/common/util:common_util", @@ -41,9 +40,7 @@ apollo_cc_library( "//third_party/can_card_library/hermes_can", "@com_github_gflags_gflags//:gflags", "@com_google_googletest//:gtest", - ] + if_esd_can([ - "//third_party/can_card_library/esd_can", - ]), + ], ) apollo_cc_test( diff --git a/modules/drivers/canbus/can_comm/can_receiver.h b/modules/drivers/canbus/can_comm/can_receiver.h index d8f9b42a57c..04eff75a062 100644 --- a/modules/drivers/canbus/can_comm/can_receiver.h +++ b/modules/drivers/canbus/can_comm/can_receiver.h @@ -178,12 +178,11 @@ void CanReceiver::RecvThreadFunc() { } receive_none_count = 0; - bool is_recv_prase = true; for (const auto &frame : buf) { uint8_t len = frame.len; uint32_t uid = frame.id; const uint8_t *data = frame.data; - pt_manager_->Parse(uid, data, len, is_recv_prase); + pt_manager_->Parse(uid, data, len); if (enable_log_) { AINFO << "recv_can_frame#" << frame.CanFrameString(); } diff --git a/modules/drivers/canbus/can_comm/can_sender.h b/modules/drivers/canbus/can_comm/can_sender.h index bea18e82b6b..ca717a4ac1d 100644 --- a/modules/drivers/canbus/can_comm/can_sender.h +++ b/modules/drivers/canbus/can_comm/can_sender.h @@ -169,6 +169,10 @@ class CanSender { void AddMessage(uint32_t message_id, ProtocolData *protocol_data, bool init_with_one = false); + void ClearMessage(); + + bool IsMessageClear(); + /** * @brief Start the CAN sender. * @return The error code indicating the status of this action. @@ -336,11 +340,10 @@ void CanSender::PowerSendThreadFunc() { AINFO << "send_can_frame#" << can_frame.CanFrameString(); } { - bool is_recv_prase = false; uint32_t uid = can_frame.id; const uint8_t *data = can_frame.data; uint8_t len = can_frame.len; - pt_manager_->Parse(uid, data, len, is_recv_prase); + pt_manager_->ParseSender(uid, data, len); } } delta_period = new_delta_period; @@ -421,6 +424,19 @@ void CanSender::Update() { } } +template +void CanSender::ClearMessage() { + send_messages_.clear(); +} + +template +bool CanSender::IsMessageClear() { + if (send_messages_.empty()) { + return true; + } + return false; +} + template void CanSender::Stop() { if (is_running_) { diff --git a/modules/drivers/canbus/can_comm/message_manager.h b/modules/drivers/canbus/can_comm/message_manager.h index 8fce226598c..d0ab5b51fa3 100644 --- a/modules/drivers/canbus/can_comm/message_manager.h +++ b/modules/drivers/canbus/can_comm/message_manager.h @@ -78,17 +78,28 @@ class MessageManager { virtual ~MessageManager() = default; /** - * @brief parse data and store parsed info in protocol data + * @brief parse data and store parsed info in receive protocol data * @param message_id the id of the message * @param data a pointer to the data array to be parsed * @param length the length of data array */ virtual void Parse(const uint32_t message_id, const uint8_t *data, - int32_t length, bool is_recv_prase); + int32_t length); + + /** + * @brief parse data and store parsed info in send protocol data + * @param message_id the id of the message + * @param data a pointer to the data array to be parsed + * @param length the length of data array + */ + virtual void ParseSender(const uint32_t message_id, const uint8_t *data, + int32_t length); void ClearSensorData(); void ClearSensorRecvData(); + void ClearSensorCheckRecvData(); void ClearSensorSenderData(); + void ClearSensorCheckSenderData(); std::condition_variable *GetMutableCVar(); @@ -114,6 +125,13 @@ class MessageManager { */ common::ErrorCode GetSensorRecvData(SensorType *const sensor_recv_data); + /** + * @brief get chassis recv detail. used lock_guard in this function to avoid + * concurrent read/write issue. + * @param chassis_detail chassis_detail to be filled. + */ + common::ErrorCode GetSensorCheckRecvData(SensorType *const sensor_recv_data); + /** * @brief get chassis sender detail. used lock_guard in this function to avoid * concurrent read/write issue. @@ -121,6 +139,14 @@ class MessageManager { */ common::ErrorCode GetSensorSenderData(SensorType *const sensor_sender_data); + /** + * @brief get chassis sender detail. used lock_guard in this function to avoid + * concurrent read/write issue. + * @param chassis_detail chassis_detail to be filled. + */ + common::ErrorCode GetSensorCheckSenderData( + SensorType *const sensor_sender_data); + /* * @brief reset send messages */ @@ -149,8 +175,12 @@ class MessageManager { SensorType sensor_data_; std::mutex sensor_data_recv_mutex_; SensorType sensor_recv_data_; + std::mutex sensor_data_check_recv_mutex_; + SensorType sensor_check_recv_data_; std::mutex sensor_data_sender_mutex_; SensorType sensor_sender_data_; + std::mutex sensor_data_check_sender_mutex_; + SensorType sensor_check_sender_data_; bool is_received_on_time_ = false; std::condition_variable cvar_; @@ -207,8 +237,7 @@ MessageManager::GetMutableProtocolDataById( template void MessageManager::Parse(const uint32_t message_id, - const uint8_t *data, int32_t length, - bool is_recv_prase) { + const uint8_t *data, int32_t length) { ProtocolData *protocol_data = GetMutableProtocolDataById(message_id); if (protocol_data == nullptr) { @@ -219,26 +248,62 @@ void MessageManager::Parse(const uint32_t message_id, std::lock_guard lock(sensor_data_mutex_); protocol_data->Parse(data, length, &sensor_data_); } - if (is_recv_prase) { - // parse revceiver - { - std::lock_guard lock(sensor_data_recv_mutex_); - protocol_data->Parse(data, length, &sensor_recv_data_); - } - if (recv_protocol_data_map_.find(message_id) == - recv_protocol_data_map_.end()) { - AERROR << "Failed to get recv data, " << "message is " << message_id; - } - } else { - // parse sender - { - std::lock_guard lock(sensor_data_sender_mutex_); - protocol_data->Parse(data, length, &sensor_sender_data_); - } - if (sender_protocol_data_map_.find(message_id) == - sender_protocol_data_map_.end()) { - AERROR << "Failed to get recv data, " << "message is " << message_id; + + // parse revceiver + { + std::lock_guard lock(sensor_data_recv_mutex_); + protocol_data->Parse(data, length, &sensor_recv_data_); + } + { + std::lock_guard lock(sensor_data_check_recv_mutex_); + protocol_data->Parse(data, length, &sensor_check_recv_data_); + } + if (recv_protocol_data_map_.find(message_id) == + recv_protocol_data_map_.end()) { + AERROR << "Failed to get recv data, " << "message is " << message_id; + } + + received_ids_.insert(message_id); + // check if need to check period + const auto it = check_ids_.find(message_id); + if (it != check_ids_.end()) { + const int64_t time = Time::Now().ToNanosecond() / 1e3; + it->second.real_period = time - it->second.last_time; + // if period 1.5 large than base period, inc error_count + const double period_multiplier = 1.5; + if (static_cast(it->second.real_period) > + (static_cast(it->second.period) * period_multiplier)) { + it->second.error_count += 1; + } else { + it->second.error_count = 0; } + it->second.last_time = time; + } +} + +template +void MessageManager::ParseSender(const uint32_t message_id, + const uint8_t *data, + int32_t length) { + ProtocolData *protocol_data = + GetMutableProtocolDataById(message_id); + if (protocol_data == nullptr) { + return; + } + + // parse sender + { + std::lock_guard lock(sensor_data_sender_mutex_); + protocol_data->Parse(data, length, &sensor_sender_data_); + } + { + std::lock_guard lock(sensor_data_check_sender_mutex_); + protocol_data->Parse(data, length, &sensor_check_sender_data_); + } + if (sender_protocol_data_map_.find(message_id) == + sender_protocol_data_map_.end()) { + AERROR << "Failed to get prase sender data, " << "message is " + << message_id; } received_ids_.insert(message_id); @@ -271,12 +336,24 @@ void MessageManager::ClearSensorRecvData() { sensor_recv_data_.Clear(); } +template +void MessageManager::ClearSensorCheckRecvData() { + std::lock_guard lock(sensor_data_check_recv_mutex_); + sensor_check_recv_data_.Clear(); +} + template void MessageManager::ClearSensorSenderData() { std::lock_guard lock(sensor_data_sender_mutex_); sensor_sender_data_.Clear(); } +template +void MessageManager::ClearSensorCheckSenderData() { + std::lock_guard lock(sensor_data_check_sender_mutex_); + sensor_check_sender_data_.Clear(); +} + template std::condition_variable *MessageManager::GetMutableCVar() { return &cvar_; @@ -306,6 +383,18 @@ ErrorCode MessageManager::GetSensorRecvData( return ErrorCode::OK; } +template +ErrorCode MessageManager::GetSensorCheckRecvData( + SensorType *const sensor_recv_data) { + if (sensor_recv_data == nullptr) { + AERROR << "Failed to get receiver data due to nullptr."; + return ErrorCode::CANBUS_ERROR; + } + std::lock_guard lock(sensor_data_check_recv_mutex_); + sensor_recv_data->CopyFrom(sensor_check_recv_data_); + return ErrorCode::OK; +} + template ErrorCode MessageManager::GetSensorSenderData( SensorType *const sensor_sender_data) { @@ -318,6 +407,18 @@ ErrorCode MessageManager::GetSensorSenderData( return ErrorCode::OK; } +template +ErrorCode MessageManager::GetSensorCheckSenderData( + SensorType *const sensor_sender_data) { + if (sensor_sender_data == nullptr) { + AERROR << "Failed to get sender data due to nullptr."; + return ErrorCode::CANBUS_ERROR; + } + std::lock_guard lock(sensor_data_check_sender_mutex_); + sensor_sender_data->CopyFrom(sensor_check_sender_data_); + return ErrorCode::OK; +} + template void MessageManager::ResetSendMessages() { for (auto &protocol_data : send_protocol_data_) { diff --git a/modules/tools/gen_vehicle_protocol/apollo_demo.dbc b/modules/tools/gen_vehicle_protocol/apollo_demo.dbc new file mode 100644 index 00000000000..f7128c148af --- /dev/null +++ b/modules/tools/gen_vehicle_protocol/apollo_demo.dbc @@ -0,0 +1,438 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: ACU VCU +VAL_TABLE_ Battery_Flt_SocLow 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_InsulationLow 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_UnderVoltage 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_OverCurrent 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_OverVoltage 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_OverTemp 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Flt_LowTemp 1 "Fault" 0 "No" ; +VAL_TABLE_ Battery_Fault 2 "Low Temperature Warning" 1 "High Temperature Warning" 0 "No Fault" ; +VAL_TABLE_ AEB_Mode 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Brake_Light_Actual 1 "BrakeLight_ON" 0 "BrakeLight_OFF" ; +VAL_TABLE_ Turn_Light_Actual 3 "Hazard_Warning_Lampsts_ON" 2 "Right_Turnlampsts_ON" 1 "Left_Turnlampsts_ON" 0 "Turnlampsts_OFF" ; +VAL_TABLE_ VIN_Req 1 "Vin_req_enable" 0 "Vin_req_disable" ; +VAL_TABLE_ Drive_Mode_STS 1 "Speed Drive Mode" 0 "Throttle Paddle Drive Mode" ; +VAL_TABLE_ Steer_Mode_STS 2 "Sync Direction Steer Mode" 1 "Non Direction Steer Mode" 0 "Standard Steer Mode" ; +VAL_TABLE_ Steer_Mode_CTRL 2 "Sync Direction Steer" 1 "Non Direction Steer" 0 "Standard Steer" ; +VAL_TABLE_ Drive_Mode_CTRL 1 "Speed Drive" 0 "Throttle Paddle Drive" ; +VAL_TABLE_ AEB_EN_CTRL 1 "Enable AEB" 0 "Disable AEB" ; +VAL_TABLE_ Vehicle_Mode_State 3 "Standby Mode" 2 "Emergency Mode" 1 "Auto Mode" 0 "Manual Remote Mode" ; +VAL_TABLE_ BackCrash_State 1 "Crash Event" 0 "No Event" ; +VAL_TABLE_ FrontCrash_State 1 "Crash Event" 0 "No Event" ; +VAL_TABLE_ AEB_State 1 "Active" 0 "Inctive" ; +VAL_TABLE_ Brake_FLT2 1 "Brake system comunication fault" 0 "No fault " ; +VAL_TABLE_ Brake_FLT1 1 "Brake system hardware fault" 0 "No fault" ; +VAL_TABLE_ Brake_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Brake_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_TABLE_ Park_FLT 1 "Fault" 0 "No Fault" ; +VAL_TABLE_ Parking_actual 1 "Parking_trigger" 0 "Release" ; +VAL_TABLE_ Park_Target 1 "Parking_trigger" 0 "Release" ; +VAL_TABLE_ Park_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Gear_FLT 1 "Fault" 0 "No Fault" ; +VAL_TABLE_ Gear_Actual 4 "D" 3 "N" 2 "R" 1 "P" 0 "Invaild" ; +VAL_TABLE_ Gear_Target 4 "D" 3 "N" 2 "R" 1 "P" 0 "Invalid" ; +VAL_TABLE_ Gear_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Throttle_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_TABLE_ Throttle_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Steer_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_TABLE_ Steer_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_TABLE_ Throttle_FLT2 1 "Drive system comunication fault" 0 "No fault" ; +VAL_TABLE_ Throttle_FLT1 1 "Drive system hardware fault" 0 "No fault" ; +VAL_TABLE_ Steer_FLT2 1 "Steer system comunication fault" 0 "No fault" ; +VAL_TABLE_ Steer_FLT1 1 "Steer system hardware fault" 0 "No fault" ; + + +BO_ 1302 VIN_Resp3: 8 Vector__XXX + SG_ VIN16 : 7|8@0+ (1,0) [0|255] "" Vector__XXX + +BO_ 1301 VIN_Resp2: 8 Vector__XXX + SG_ VIN15 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN14 : 55|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN13 : 47|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN12 : 39|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN11 : 31|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN10 : 23|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN09 : 15|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN08 : 7|8@0+ (1,0) [0|255] "" Vector__XXX + +BO_ 1300 VIN_Resp1: 8 Vector__XXX + SG_ VIN07 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN06 : 55|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN05 : 47|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN04 : 39|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN03 : 31|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN02 : 23|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN01 : 15|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ VIN00 : 7|8@0+ (1,0) [0|255] "" Vector__XXX + +BO_ 261 Vehicle_Mode_Command: 8 ACU + SG_ Heartbeat_105 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ CheckSum_105 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ Turn_Light_CTRL : 17|2@0+ (1,0) [0|7] "" Vector__XXX + SG_ VIN_Req : 24|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ Drive_Mode_CTRL : 10|3@0+ (1,0) [0|7] "" Vector__XXX + SG_ Steer_Mode_CTRL : 2|3@0+ (1,0) [0|7] "" Vector__XXX + +BO_ 1298 BMS_Report: 8 Vector__XXX + SG_ Battery_Flt_OverTemp : 49|1@0+ (1,0) [0|15] "" Vector__XXX + SG_ Battery_Flt_LowTemp : 48|1@0+ (1,0) [0|15] "" Vector__XXX + SG_ Battery_Inside_Temperature : 47|8@0+ (1,-40) [-40|215] "" Vector__XXX + SG_ Battery_Current : 23|16@0+ (0.1,-3200) [-3200|3353.5] "A" Vector__XXX + SG_ Battery_Voltage : 7|16@0+ (0.01,0) [0|300] "V" Vector__XXX + SG_ Battery_Soc : 39|8@0+ (1,0) [0|100] "%" Vector__XXX + +BO_ 1297 ULTR_Sensor_5: 8 Vector__XXX + SG_ uiUSS7_ToF_Direct : 55|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS6_ToF_Direct : 39|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS1_ToF_Direct : 23|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS0_ToF_Direct : 7|16@0+ (0.01724,0) [0|65535] "cm" ACU + +BO_ 1296 ULTR_Sensor_4: 8 Vector__XXX + SG_ uiUSS5_ToF_Indirect : 55|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS4_ToF_Indirect : 39|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS3_ToF_Indirect : 23|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS2_ToF_Indirect : 7|16@0+ (0.01724,0) [0|65535] "cm" ACU + +BO_ 1289 ULTR_Sensor_3: 8 Vector__XXX + SG_ uiUSS5_ToF_Direct : 55|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS4_ToF_Direct : 39|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS3_ToF_Direct : 23|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS2_ToF_Direct : 7|16@0+ (0.01724,0) [0|65535] "cm" ACU + +BO_ 1288 ULTR_Sensor_2: 8 Vector__XXX + SG_ uiUSS9_ToF_Indirect : 23|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS8_ToF_Indirect : 7|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS11_ToF_Indirect : 55|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS10_ToF_Indirect : 39|16@0+ (0.01724,0) [0|65535] "cm" ACU + +BO_ 1287 ULTR_Sensor_1: 8 Vector__XXX + SG_ uiUSS9_ToF_Direct : 23|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS8_ToF_Direct : 7|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS11_ToF_Direct : 55|16@0+ (0.01724,0) [0|65535] "cm" ACU + SG_ uiUSS10_ToF_Direct : 39|16@0+ (0.01724,0) [0|65535] "cm" ACU + +BO_ 260 Park_Command: 8 ACU + SG_ Heartbeat_104 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ CheckSum_104 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ Park_Target : 8|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ Park_EN_CTRL : 0|1@0+ (1,0) [0|1] "" Vector__XXX + +BO_ 1286 WheelSpeed_Report: 8 Vector__XXX + SG_ RR : 55|16@0+ (0.001,0) [0|65.535] "m/s" ACU + SG_ RL : 39|16@0+ (0.001,0) [0|65.535] "m/s" ACU + SG_ FR : 23|16@0+ (0.001,0) [0|65.535] "m/s" ACU + SG_ FL : 7|16@0+ (0.001,0) [0|65.535] "m/s" ACU + +BO_ 1285 VCU_Report: 8 Vector__XXX + SG_ Vehicle_Mode_State : 36|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ AEB_Mode : 58|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ Brake_Light_Actual : 11|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ Turn_Light_Actual : 57|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ Chassis_errcode : 47|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ Drive_Mode_STS : 39|3@0+ (1,0) [0|7] "" Vector__XXX + SG_ Steer_Mode_STS : 10|3@0+ (1,0) [0|7] "" Vector__XXX + SG_ FrontCrash_State : 33|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ BackCrash_State : 34|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ AEB_Brake_State : 32|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ ACC : 7|12@0- (0.01,0) [-10|10] "m/s^2" ACU + SG_ SPEED : 23|16@0- (0.001,0) [-32.768|32.767] "m/s" ACU + +BO_ 1284 Park_Report: 8 Vector__XXX + SG_ Parking_actual : 0|1@0+ (1,0) [0|1] "" ACU + SG_ Park_FLT : 15|8@0+ (1,0) [0|1] "" ACU + +BO_ 1283 Gear_Report: 8 Vector__XXX + SG_ Gear_FLT : 15|8@0+ (1,0) [0|1] "" ACU + SG_ Gear_Actual : 2|3@0+ (1,0) [0|4] "" ACU + +BO_ 1281 Brake_Report: 8 Vector__XXX + SG_ Brake_Pedal_Actual : 31|16@0+ (0.1,0) [0|100] "%" ACU + SG_ Brake_FLT2 : 23|8@0+ (1,0) [0|1] "" ACU + SG_ Brake_FLT1 : 15|8@0+ (1,0) [0|1] "" ACU + SG_ Brake_EN_state : 1|2@0+ (1,0) [0|2] "" ACU + +BO_ 1280 Throttle_Report: 8 Vector__XXX + SG_ Throttle_Pedal_Actual : 31|16@0+ (0.1,0) [0|100] "%" ACU + SG_ Throttle_FLT2 : 23|8@0+ (1,0) [0|1] "" ACU + SG_ Throttle_FLT1 : 15|8@0+ (1,0) [0|1] "" ACU + SG_ Throttle_EN_state : 1|2@0+ (1,0) [0|2] "" ACU + +BO_ 1282 Steering_Report: 8 Vector__XXX + SG_ Steer_ANGLE_Rear_Actual : 47|16@0+ (1,-500) [-500|500] "deg" Vector__XXX + SG_ Steer_ANGLE_SPD_Actual : 63|8@0+ (1,0) [0|0] "deg/s" ACU + SG_ Steer_FLT2 : 23|8@0+ (1,0) [0|255] "" ACU + SG_ Steer_FLT1 : 15|8@0+ (1,0) [0|255] "" ACU + SG_ Steer_EN_state : 1|2@0+ (1,0) [0|2] "" ACU + SG_ Steer_ANGLE_Actual : 31|16@0+ (1,-500) [-500|500] "deg" ACU + +BO_ 259 Gear_Command: 8 ACU + SG_ Heartbeat_103 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ Gear_Target : 10|3@0+ (1,0) [0|4] "" Vector__XXX + SG_ Gear_EN_CTRL : 0|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ CheckSum_103 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + +BO_ 258 Steering_Command: 8 ACU + SG_ Heartbeat_102 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ Steer_EN_CTRL : 0|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ Steer_ANGLE_Target : 31|16@0+ (1,-500) [-500|500] "deg" Vector__XXX + SG_ Steer_ANGLE_SPD_Target : 15|8@0+ (1,0) [0|250] "deg/s" Vector__XXX + SG_ CheckSum_102 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + +BO_ 257 Brake_Command: 8 ACU + SG_ Heartbeat_101 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ AEB_EN_CTRL : 1|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ Brake_Dec : 15|10@0+ (0.01,0) [0|10] "m/s^2" Vector__XXX + SG_ CheckSum_101 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ Brake_Pedal_Target : 31|16@0+ (0.1,0) [0|100] "%" Vector__XXX + SG_ Brake_EN_CTRL : 0|1@0+ (1,0) [0|1] "" Vector__XXX + +BO_ 256 Throttle_Command: 8 ACU + SG_ Heartbeat_100 : 7|4@0+ (1,0) [0|15] "" Vector__XXX + SG_ Speed_Target : 47|12@0+ (0.01,0) [0|40.95] "m/s" Vector__XXX + SG_ Throttle_Acc : 15|10@0+ (0.01,0) [0|10] "m/s^2" Vector__XXX + SG_ CheckSum_100 : 63|8@0+ (1,0) [0|255] "" Vector__XXX + SG_ Throttle_Pedal_Target : 31|16@0+ (0.1,0) [0|100] "%" Vector__XXX + SG_ Throttle_EN_CTRL : 0|1@0+ (1,0) [0|1] "" Vector__XXX + + + +CM_ SG_ 1298 Battery_Inside_Temperature "Battery Total Voltage"; +CM_ SG_ 1298 Battery_Current "Battery Total Current"; +CM_ SG_ 1298 Battery_Voltage "Battery Total Voltage"; +CM_ SG_ 1298 Battery_Soc "Battery Soc percentage"; +CM_ SG_ 260 Park_Target "command"; +CM_ SG_ 260 Park_EN_CTRL "enable"; +CM_ SG_ 1285 AEB_Mode "describle the vehicle AEB mode whether was set"; +CM_ SG_ 1285 AEB_Brake_State "describe the vehicle e-brake command whether was triggered by AEB"; +CM_ SG_ 1285 SPEED "speed"; +CM_ SG_ 1284 Parking_actual "command"; +CM_ SG_ 1284 Park_FLT "fault"; +CM_ SG_ 1283 Gear_FLT "fault"; +CM_ SG_ 1283 Gear_Actual "command"; +CM_ SG_ 1281 Brake_Pedal_Actual "command"; +CM_ SG_ 1281 Brake_FLT2 "Brake system communication fault"; +CM_ SG_ 1281 Brake_FLT1 "Brake system hardware fault"; +CM_ SG_ 1281 Brake_EN_state "enable"; +CM_ SG_ 1280 Throttle_Pedal_Actual "command"; +CM_ SG_ 1280 Throttle_FLT2 "Drive system communication fault"; +CM_ SG_ 1280 Throttle_FLT1 "Drive system hardware fault"; +CM_ SG_ 1280 Throttle_EN_state "enable"; +CM_ SG_ 1282 Steer_FLT2 "Steer system communication fault"; +CM_ SG_ 1282 Steer_FLT1 "Steer system hardware fault"; +CM_ SG_ 1282 Steer_EN_state "enable"; +CM_ SG_ 1282 Steer_ANGLE_Actual "command"; +CM_ SG_ 259 Gear_Target "command"; +CM_ SG_ 259 Gear_EN_CTRL "enable"; +CM_ SG_ 258 Steer_EN_CTRL "enable"; +CM_ SG_ 258 Steer_ANGLE_Target "command"; +CM_ SG_ 257 Brake_Pedal_Target "command"; +CM_ SG_ 257 Brake_EN_CTRL "enable"; +CM_ SG_ 256 Throttle_Pedal_Target "command"; +CM_ SG_ 256 Throttle_EN_CTRL "enable"; +BA_DEF_ "BusType" STRING ; +BA_DEF_ BU_ "NmStationAddress" HEX 0 0; +BA_DEF_ "GenNWMApBusSleep" STRING ; +BA_DEF_ "GenNWMApCanNormal" STRING ; +BA_DEF_ "GenNWMApCanOff" STRING ; +BA_DEF_ "GenNWMApCanOn" STRING ; +BA_DEF_ "GenNWMApCanSleep" STRING ; +BA_DEF_ "GenNWMApCanWakeUp" STRING ; +BA_DEF_ "GenNWMGotoMode_Awake" STRING ; +BA_DEF_ "GenNWMGotoMode_BusSleep" STRING ; +BA_DEF_ "GenNWMSleepTime" INT 0 1000000; +BA_DEF_ "GenNWMTalkNM" STRING ; +BA_DEF_ BU_ "GenNodSleepTime" INT 0 1000000; +BA_DEF_ BO_ "NmMessage" ENUM "No","Yes"; +BA_DEF_ BU_ "NmNode" ENUM "No","Yes"; +BA_DEF_ SG_ "NWM-WakeupAllowed" ENUM "No","Yes"; +BA_DEF_ BU_ "NodeLayerModules" STRING ; +BA_DEF_ BO_ "GenMsgCycleTime" INT 2 50000; +BA_DEF_ BO_ "GenMsgDelayTime" INT 1 1000; +BA_DEF_ BO_ "GenMsgNrOfRepetitions" INT 1 999999; +BA_DEF_ BO_ "GenMsgSendType" ENUM "cyclic","spontaneous","cyclicIfActive","spontaneousWithDelay","cyclicAndSpontaneous","cyclicAndSpontaneousWithDelay","spontaneousWithRepetition","cyclicIfActiveAndSpontaneousWD"; +BA_DEF_ SG_ "GenSigInactiveValue" INT 0 100000; +BA_DEF_ SG_ "GenSigSendType" ENUM "spontaneous"; +BA_DEF_ SG_ "GenSigStartValue" FLOAT 0 100000000000; +BA_DEF_ EV_ "GenEnvControlType" ENUM "NoControl","SliderHoriz","SliderVert","PushButton","Edit","BitmapSwitch"; +BA_DEF_ EV_ "GenEnvMsgName" STRING ; +BA_DEF_ EV_ "GenEnvMsgOffset" INT 0 999999999; +BA_DEF_ EV_ "GenEnvAutoGenCtrl" ENUM "No","Yes"; +BA_DEF_ "GenEnvVarEndingDsp" STRING ; +BA_DEF_ "GenEnvVarEndingSnd" STRING ; +BA_DEF_ "GenEnvVarPrefix" STRING ; +BA_DEF_ BO_ "GenMsgAltSetting" STRING ; +BA_DEF_ BO_ "GenMsgAutoGenDsp" ENUM "No","Yes"; +BA_DEF_ BO_ "GenMsgAutoGenSnd" ENUM "No","Yes"; +BA_DEF_ BO_ "GenMsgConditionalSend" STRING ; +BA_DEF_ BO_ "GenMsgEVName" STRING ; +BA_DEF_ BO_ "GenMsgPostIfSetting" STRING ; +BA_DEF_ BO_ "GenMsgPostSetting" STRING ; +BA_DEF_ BO_ "GenMsgPreIfSetting" STRING ; +BA_DEF_ BO_ "GenMsgPreSetting" STRING ; +BA_DEF_ BU_ "GenNodAutoGenSnd" ENUM "No","Yes"; +BA_DEF_ SG_ "GenSigAltSetting" STRING ; +BA_DEF_ SG_ "GenSigAssignSetting" STRING ; +BA_DEF_ SG_ "GenSigAutoGenDsp" ENUM "No","Yes"; +BA_DEF_ SG_ "GenSigAutoGenSnd" ENUM "No","Yes"; +BA_DEF_ SG_ "GenSigConditionalSend" STRING ; +BA_DEF_ SG_ "GenSigEnvVarType" ENUM "int","float","undef"; +BA_DEF_ SG_ "GenSigEVName" STRING ; +BA_DEF_ SG_ "GenSigPostIfSetting" STRING ; +BA_DEF_ SG_ "GenSigPostSetting" STRING ; +BA_DEF_ SG_ "GenSigPreIfSetting" STRING ; +BA_DEF_ SG_ "GenSigPreSetting" STRING ; +BA_DEF_ SG_ "GenSigReceiveSetting" STRING ; +BA_DEF_DEF_ "BusType" "CAN"; +BA_DEF_DEF_ "NmStationAddress" 0; +BA_DEF_DEF_ "GenNWMApBusSleep" "apBusSleep()"; +BA_DEF_DEF_ "GenNWMApCanNormal" "apCanNormal()"; +BA_DEF_DEF_ "GenNWMApCanOff" "apCanOff()"; +BA_DEF_DEF_ "GenNWMApCanOn" "apCanOn()"; +BA_DEF_DEF_ "GenNWMApCanSleep" "apCanSleep()"; +BA_DEF_DEF_ "GenNWMApCanWakeUp" "apCanWakeUp()"; +BA_DEF_DEF_ "GenNWMGotoMode_Awake" "GotoMode_Awake()"; +BA_DEF_DEF_ "GenNWMGotoMode_BusSleep" "GotoMode_BusSleep()"; +BA_DEF_DEF_ "GenNWMSleepTime" 0; +BA_DEF_DEF_ "GenNWMTalkNM" "TalkNM()"; +BA_DEF_DEF_ "GenNodSleepTime" 0; +BA_DEF_DEF_ "NmMessage" "No"; +BA_DEF_DEF_ "NmNode" "No"; +BA_DEF_DEF_ "NWM-WakeupAllowed" ""; +BA_DEF_DEF_ "NodeLayerModules" "CANoeILNLVector.dll"; +BA_DEF_DEF_ "GenMsgCycleTime" 100; +BA_DEF_DEF_ "GenMsgDelayTime" 1; +BA_DEF_DEF_ "GenMsgNrOfRepetitions" 1; +BA_DEF_DEF_ "GenMsgSendType" ""; +BA_DEF_DEF_ "GenSigInactiveValue" 0; +BA_DEF_DEF_ "GenSigSendType" ""; +BA_DEF_DEF_ "GenSigStartValue" 0; +BA_DEF_DEF_ "GenEnvControlType" ""; +BA_DEF_DEF_ "GenEnvMsgName" ""; +BA_DEF_DEF_ "GenEnvMsgOffset" 0; +BA_DEF_DEF_ "GenEnvAutoGenCtrl" "No"; +BA_DEF_DEF_ "GenEnvVarEndingDsp" "Dsp_"; +BA_DEF_DEF_ "GenEnvVarEndingSnd" "_"; +BA_DEF_DEF_ "GenEnvVarPrefix" "Env"; +BA_DEF_DEF_ "GenMsgAltSetting" ""; +BA_DEF_DEF_ "GenMsgAutoGenDsp" ""; +BA_DEF_DEF_ "GenMsgAutoGenSnd" ""; +BA_DEF_DEF_ "GenMsgConditionalSend" ""; +BA_DEF_DEF_ "GenMsgEVName" ""; +BA_DEF_DEF_ "GenMsgPostIfSetting" ""; +BA_DEF_DEF_ "GenMsgPostSetting" ""; +BA_DEF_DEF_ "GenMsgPreIfSetting" ""; +BA_DEF_DEF_ "GenMsgPreSetting" ""; +BA_DEF_DEF_ "GenNodAutoGenSnd" ""; +BA_DEF_DEF_ "GenSigAltSetting" ""; +BA_DEF_DEF_ "GenSigAssignSetting" ""; +BA_DEF_DEF_ "GenSigAutoGenDsp" ""; +BA_DEF_DEF_ "GenSigAutoGenSnd" ""; +BA_DEF_DEF_ "GenSigConditionalSend" ""; +BA_DEF_DEF_ "GenSigEnvVarType" "undef"; +BA_DEF_DEF_ "GenSigEVName" ""; +BA_DEF_DEF_ "GenSigPostIfSetting" ""; +BA_DEF_DEF_ "GenSigPostSetting" ""; +BA_DEF_DEF_ "GenSigPreIfSetting" ""; +BA_DEF_DEF_ "GenSigPreSetting" ""; +BA_DEF_DEF_ "GenSigReceiveSetting" ""; +BA_ "GenMsgCycleTime" BO_ 1302 500; +BA_ "GenMsgCycleTime" BO_ 1301 500; +BA_ "GenMsgCycleTime" BO_ 1300 500; +BA_ "GenMsgCycleTime" BO_ 1298 20; +BA_ "GenMsgCycleTime" BO_ 1297 130; +BA_ "GenMsgCycleTime" BO_ 1296 130; +BA_ "GenMsgCycleTime" BO_ 1289 130; +BA_ "GenMsgCycleTime" BO_ 1288 130; +BA_ "GenMsgCycleTime" BO_ 1287 130; +BA_ "GenMsgCycleTime" BO_ 260 20; +BA_ "GenMsgCycleTime" BO_ 1286 20; +BA_ "GenMsgCycleTime" BO_ 1285 20; +BA_ "GenMsgCycleTime" BO_ 1284 50; +BA_ "GenMsgCycleTime" BO_ 1283 50; +BA_ "GenMsgCycleTime" BO_ 1281 20; +BA_ "GenMsgCycleTime" BO_ 1280 20; +BA_ "GenMsgCycleTime" BO_ 1282 20; +BA_ "GenMsgCycleTime" BO_ 259 50; +BA_ "GenMsgCycleTime" BO_ 258 20; +BA_ "GenMsgCycleTime" BO_ 257 20; +BA_ "GenMsgCycleTime" BO_ 256 20; +BA_ "GenSigStartValue" SG_ 1298 Battery_Current 32000; +BA_ "GenSigStartValue" SG_ 1285 ACC 0; +BA_ "GenSigStartValue" SG_ 1282 Steer_ANGLE_Rear_Actual 500; +BA_ "GenSigStartValue" SG_ 1282 Steer_ANGLE_SPD_Actual 0; +BA_ "GenSigStartValue" SG_ 1282 Steer_ANGLE_Actual 500; +BA_ "GenSigStartValue" SG_ 258 Steer_ANGLE_Target 500; +BA_ "GenSigStartValue" SG_ 257 Brake_Dec 0; +BA_ "GenSigStartValue" SG_ 256 Throttle_Acc 0; +VAL_ 261 Turn_Light_CTRL 3 "Hazard_Warning_Lampsts_ON" 2 "Right_Turnlamp_ON" 1 "Left_Turnlamp_ON" 0 "Turnlamp_OFF" ; +VAL_ 261 VIN_Req 1 "Vin_req_enable" 0 "Vin_req_disable" ; +VAL_ 261 Drive_Mode_CTRL 1 "Speed Drive" 0 "Throttle Paddle Drive" ; +VAL_ 261 Steer_Mode_CTRL 2 "Sync Direction Steer" 1 "Non Direction Steer" 0 "Standard Steer" ; +VAL_ 1298 Battery_Flt_OverTemp 1 "Fault" 0 "No" ; +VAL_ 1298 Battery_Flt_LowTemp 1 "Fault" 0 "No" ; +VAL_ 260 Park_Target 1 "Parking_trigger" 0 "Release" ; +VAL_ 260 Park_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_ 1285 Vehicle_Mode_State 3 "Standby Mode" 2 "Emergency Mode" 1 "Auto Mode" 0 "Manual Remote Mode" ; +VAL_ 1285 AEB_Mode 1 "Enable" 0 "Disable" ; +VAL_ 1285 Brake_Light_Actual 1 "BrakeLight_ON" 0 "BrakeLight_OFF" ; +VAL_ 1285 Turn_Light_Actual 3 "Hazard_Warning_Lampsts_ON" 2 "Right_Turnlampsts_ON" 1 "Left_Turnlampsts_ON" 0 "Turnlampsts_OFF" ; +VAL_ 1285 Drive_Mode_STS 1 "Speed Drive Mode" 0 "Throttle Paddle Drive Mode" ; +VAL_ 1285 Steer_Mode_STS 2 "Sync Direction Steer Mode" 1 "Non Direction Steer Mode" 0 "Standard Steer Mode" ; +VAL_ 1285 FrontCrash_State 1 "Crash Event" 0 "No Event" ; +VAL_ 1285 BackCrash_State 1 "Crash Event" 0 "No Event" ; +VAL_ 1285 AEB_Brake_State 1 "Active" 0 "Inactive" ; +VAL_ 1284 Parking_actual 1 "Parking_trigger" 0 "Release" ; +VAL_ 1284 Park_FLT 1 "Fault" 0 "No Fault" ; +VAL_ 1283 Gear_FLT 1 "Fault" 0 "No Fault" ; +VAL_ 1283 Gear_Actual 4 "DRIVE" 3 "NEUTRAL" 2 "REVERSE" 1 "PARK" 0 "INVALID" ; +VAL_ 1281 Brake_FLT2 1 "Brake system comunication fault" 0 "No fault " ; +VAL_ 1281 Brake_FLT1 1 "Brake system hardware fault" 0 "No fault" ; +VAL_ 1281 Brake_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_ 1280 Throttle_FLT2 1 "Drive system comunication fault" 0 "No fault" ; +VAL_ 1280 Throttle_FLT1 1 "Drive system hardware fault" 0 "No fault" ; +VAL_ 1280 Throttle_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_ 1282 Steer_FLT2 1 "Steer system comunication fault" 0 "No fault" ; +VAL_ 1282 Steer_FLT1 1 "Steer system hardware fault" 0 "No fault" ; +VAL_ 1282 Steer_EN_state 3 "Standby" 2 "Takeover" 1 "Auto" 0 "Manual" ; +VAL_ 259 Gear_Target 4 "DRIVE" 3 "NEUTRAL" 2 "REVERSE" 1 "PARK" 0 "INVALID" ; +VAL_ 259 Gear_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_ 258 Steer_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_ 257 AEB_EN_CTRL 1 "Enable AEB" 0 "Disable AEB" ; +VAL_ 257 Brake_EN_CTRL 1 "Enable" 0 "Disable" ; +VAL_ 256 Throttle_EN_CTRL 1 "Enable" 0 "Disable" ; + diff --git a/modules/tools/gen_vehicle_protocol/template/canbus_aem.conf.tpl b/modules/tools/gen_vehicle_protocol/template/canbus_aem.conf.tpl index a61b24f4257..5398e66f307 100644 --- a/modules/tools/gen_vehicle_protocol/template/canbus_aem.conf.tpl +++ b/modules/tools/gen_vehicle_protocol/template/canbus_aem.conf.tpl @@ -3,4 +3,9 @@ --load_vehicle_library=/opt/apollo/neo/lib/modules/canbus_vehicle/%(car_type_lower)s/lib%(car_type_lower)s_vehicle_factory_lib.so --load_vehicle_class_name=%(car_type_cap)sVehicleFactory --enable_chassis_detail_pub ---noreceive_guardian \ No newline at end of file +--enable_chassis_detail_sender_pub +--chassis_debug_mode=false +--use_control_cmd_check=false +--use_guardian_cmd_check=false +--noreceive_guardian +--estop_brake=30.0 diff --git a/modules/tools/gen_vehicle_protocol/template/canbus_conf.pb.tpl b/modules/tools/gen_vehicle_protocol/template/canbus_conf.pb.tpl index 6399f165118..1748e6d2117 100644 --- a/modules/tools/gen_vehicle_protocol/template/canbus_conf.pb.tpl +++ b/modules/tools/gen_vehicle_protocol/template/canbus_conf.pb.tpl @@ -12,4 +12,4 @@ can_card_parameter { enable_debug_mode: false enable_receiver_log: false -enable_sender_log: true \ No newline at end of file +enable_sender_log: false diff --git a/modules/tools/gen_vehicle_protocol/template/canbus_source.conf.tpl b/modules/tools/gen_vehicle_protocol/template/canbus_source.conf.tpl index 7b43cd92ed2..5fbed4ca302 100644 --- a/modules/tools/gen_vehicle_protocol/template/canbus_source.conf.tpl +++ b/modules/tools/gen_vehicle_protocol/template/canbus_source.conf.tpl @@ -3,4 +3,9 @@ --load_vehicle_library=/apollo/bazel-bin/modules/canbus_vehicle/%(car_type_lower)s/lib%(car_type_lower)s_vehicle_factory_lib.so --load_vehicle_class_name=%(car_type_cap)sVehicleFactory --enable_chassis_detail_pub ---noreceive_guardian \ No newline at end of file +--enable_chassis_detail_sender_pub +--chassis_debug_mode=false +--use_control_cmd_check=false +--use_guardian_cmd_check=false +--noreceive_guardian +--estop_brake=30.0 diff --git a/modules/tools/gen_vehicle_protocol/template/control_protocol.cc.tpl b/modules/tools/gen_vehicle_protocol/template/control_protocol.cc.tpl index 191ef7a2618..4e02b00c19a 100644 --- a/modules/tools/gen_vehicle_protocol/template/control_protocol.cc.tpl +++ b/modules/tools/gen_vehicle_protocol/template/control_protocol.cc.tpl @@ -30,7 +30,7 @@ const int32_t %(classname)s::ID = 0x%(id_upper)s; %(classname)s::%(classname)s() { Reset(); } uint32_t %(classname)s::GetPeriod() const { - // TODO(All) : modify every protocol's period manually + // TODO(All) : modify every protocol's period manually static const uint32_t PERIOD = 20 * 1000; return PERIOD; } @@ -41,7 +41,7 @@ void %(classname)s::Parse(const std::uint8_t* bytes, int32_t length, } void %(classname)s::UpdateData_Heartbeat(uint8_t* data) { - // TODO(All) : you should add the heartbeat manually + // TODO(All) : you should add the heartbeat manually } void %(classname)s::UpdateData(uint8_t* data) { diff --git a/modules/tools/gen_vehicle_protocol/template/controller.cc.tpl b/modules/tools/gen_vehicle_protocol/template/controller.cc.tpl index beee9032d1c..482bd7aa482 100644 --- a/modules/tools/gen_vehicle_protocol/template/controller.cc.tpl +++ b/modules/tools/gen_vehicle_protocol/template/controller.cc.tpl @@ -42,10 +42,13 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; } // namespace +void %(car_type_cap)sController::AddSendMessage() { +%(protocol_add_list)s +} + ErrorCode %(car_type_cap)sController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::%(car_type_cap)s> *const can_sender, - CanReceiver<::apollo::canbus::%(car_type_cap)s>* const can_receiver, MessageManager<::apollo::canbus::%(car_type_cap)s> *const message_manager) { if (is_initialized_) { AINFO << "%(car_type_cap)sController has already been initiated."; @@ -66,12 +69,6 @@ ErrorCode %(car_type_cap)sController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - AERROR << "Canbus receiver is null."; - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; @@ -80,9 +77,8 @@ ErrorCode %(car_type_cap)sController::Init( // sender part %(protocol_ptr_get_list)s -%(protocol_add_list)s + AddSendMessage(); - // need sleep to ensure all messages received AINFO << "%(car_type_cap)sController is initialized."; is_initialized_ = true; @@ -117,19 +113,19 @@ void %(car_type_cap)sController::Stop() { Chassis %(car_type_cap)sController::chassis() { chassis_.Clear(); + %(car_type_cap)s chassis_detail = GetNewRecvChassisDetail();; - %(car_type_cap)s chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - - // 21, 22, previously 1, 2 + // 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); + // check chassis error if (CheckChassisError()) { chassis_.mutable_engage_advice()->set_advice( @@ -137,6 +133,7 @@ Chassis %(car_type_cap)sController::chassis() { chassis_.mutable_engage_advice()->set_reason( "Chassis has some fault, please check the chassis_detail."); } + // check the chassis detail lost if (is_chassis_communication_error_) { chassis_.mutable_engage_advice()->set_advice( @@ -447,66 +444,11 @@ void %(car_type_cap)sController::ResetProtocol() { message_manager_->ResetSendMessages(); } -bool %(car_type_cap)sController::CheckChassisCommunicationError() { - %(car_type_cap)s chassis_detail_receiver; - ADEBUG << "Can receiver finished recv once: " - << can_receiver_->IsFinishRecvOnce(); - if (message_manager_->GetSensorRecvData(&chassis_detail_receiver) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_receiver is " - << chassis_detail_receiver.ShortDebugString(); - size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); - ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; - // check receiver data is null - if (receiver_data_size < 2) { - if (is_need_count_) { - lost_chassis_reveive_detail_count_++; - } - } else { - lost_chassis_reveive_detail_count_ = 0; - is_need_count_ = true; - } - ADEBUG << "lost_chassis_reveive_detail_count_ is " - << lost_chassis_reveive_detail_count_; - // check receive data lost threshold is (100 * 10)ms - if (lost_chassis_reveive_detail_count_ > 100) { - is_need_count_ = false; - is_chassis_communication_error_ = true; - AERROR << "neolix chassis detail is lost, please check the communication " - "error."; - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorData(); - return true; - } else { - is_chassis_communication_error_ = false; - } - - %(car_type_cap)s chassis_detail_sender; - if (message_manager_->GetSensorSenderData(&chassis_detail_sender) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_sender is " - << chassis_detail_sender.ShortDebugString(); - size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); - ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; - - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorSenderData(); - return false; -} - bool %(car_type_cap)sController::CheckChassisError() { if (is_chassis_communication_error_) { AERROR_EVERY(100) << "ChassisDetail has no %(car_type_lower)s vehicle info."; return false; } - %(car_type_cap)s chassis_detail; - if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis detail failed."; - } /* ADD YOUR OWN CAR CHASSIS OPERATION // steer fault @@ -631,9 +573,14 @@ bool %(car_type_cap)sController::CheckResponse(const int32_t flags, bool need_wa } } while (need_wait && retry_num); - AERROR << "check_response fail: is_eps_online:" << is_eps_online - << ", is_vcu_online:" << is_vcu_online - << ", is_esp_online:" << is_esp_online; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } return false; } diff --git a/modules/tools/gen_vehicle_protocol/template/controller.h.tpl b/modules/tools/gen_vehicle_protocol/template/controller.h.tpl index 039d42a2cf6..37cfb072390 100644 --- a/modules/tools/gen_vehicle_protocol/template/controller.h.tpl +++ b/modules/tools/gen_vehicle_protocol/template/controller.h.tpl @@ -24,8 +24,8 @@ #include "modules/common_msgs/basic_msgs/error_code.pb.h" #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/common_msgs/control_msgs/control_cmd.pb.h" -#include "modules/canbus/vehicle/vehicle_controller.h" +#include "modules/canbus/vehicle/vehicle_controller.h" %(control_protocol_include_list)s namespace apollo { @@ -42,7 +42,6 @@ class %(car_type_cap)sController final : public VehicleController<::apollo::canb ::apollo::common::ErrorCode Init( const VehicleParameter& params, CanSender<::apollo::canbus::%(car_type_cap)s> *const can_sender, - CanReceiver<::apollo::canbus::%(car_type_cap)s>* const can_receiver, MessageManager<::apollo::canbus::%(car_type_cap)s> *const message_manager) override; bool Start() override; @@ -59,9 +58,9 @@ class %(car_type_cap)sController final : public VehicleController<::apollo::canb Chassis chassis() override; /** - * @brief check the chassis detail received or lost. + * @brief add the sender message. */ - bool CheckChassisCommunicationError(); + void AddSendMessage() override; private: // main logical function for operation the car enter or exit the auto driving @@ -139,9 +138,6 @@ class %(car_type_cap)sController final : public VehicleController<::apollo::canb std::mutex chassis_mask_mutex_; int32_t chassis_error_mask_ = 0; - uint32_t lost_chassis_reveive_detail_count_ = 0; - bool is_need_count_ = true; - bool is_chassis_communication_error_ = false; }; } // namespace %(car_type_lower)s diff --git a/modules/tools/gen_vehicle_protocol/template/controllerdemo.cc.tpl b/modules/tools/gen_vehicle_protocol/template/controllerdemo.cc.tpl index acdeb686f48..8d50de5b9ca 100644 --- a/modules/tools/gen_vehicle_protocol/template/controllerdemo.cc.tpl +++ b/modules/tools/gen_vehicle_protocol/template/controllerdemo.cc.tpl @@ -42,10 +42,13 @@ const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2; } // namespace +void %(car_type_cap)sController::AddSendMessage() { +%(protocol_add_list)s +} + ErrorCode %(car_type_cap)sController::Init( const VehicleParameter& params, CanSender<::apollo::canbus::%(car_type_cap)s> *const can_sender, - CanReceiver<::apollo::canbus::%(car_type_cap)s>* const can_receiver, MessageManager<::apollo::canbus::%(car_type_cap)s> *const message_manager) { if (is_initialized_) { AINFO << "%(car_type_cap)sController has already been initiated."; @@ -66,11 +69,6 @@ ErrorCode %(car_type_cap)sController::Init( } can_sender_ = can_sender; - if (can_receiver == nullptr) { - return ErrorCode::CANBUS_ERROR; - } - can_receiver_ = can_receiver; - if (message_manager == nullptr) { AERROR << "protocol manager is null."; return ErrorCode::CANBUS_ERROR; @@ -79,9 +77,8 @@ ErrorCode %(car_type_cap)sController::Init( // sender part %(protocol_ptr_get_list)s -%(protocol_add_list)s + AddSendMessage(); - // need sleep to ensure all messages received AINFO << "%(car_type_cap)sController is initialized."; is_initialized_ = true; @@ -116,20 +113,21 @@ void %(car_type_cap)sController::Stop() { Chassis %(car_type_cap)sController::chassis() { chassis_.Clear(); + %(car_type_cap)s chassis_detail = GetNewRecvChassisDetail();; - %(car_type_cap)s chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - - // 21, 22, previously 1, 2 + // 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); + %(protocol_chassis_get_list)s + // check chassis error if (CheckChassisError()) { chassis_.mutable_engage_advice()->set_advice( @@ -137,6 +135,7 @@ Chassis %(car_type_cap)sController::chassis() { chassis_.mutable_engage_advice()->set_reason( "Chassis has some fault, please check the chassis_detail."); } + // check the chassis detail lost if (is_chassis_communication_error_) { chassis_.mutable_engage_advice()->set_advice( @@ -364,11 +363,11 @@ void %(car_type_cap)sController::SetTurningSignal(const VehicleSignal& vehicle_s /* ADD YOUR OWN CAR CHASSIS OPERATION auto signal = vehicle_signal.turn_signal(); if (signal == common::VehicleSignal::TURN_LEFT) { - turnsignal_68_->set_turn_left(); + } else if (signal == common::VehicleSignal::TURN_RIGHT) { - turnsignal_68_->set_turn_right(); + } else { - turnsignal_68_->set_turn_none(); + } */ } @@ -428,66 +427,11 @@ void %(car_type_cap)sController::ResetProtocol() { message_manager_->ResetSendMessages(); } -bool %(car_type_cap)sController::CheckChassisCommunicationError() { - %(car_type_cap)s chassis_detail_receiver; - ADEBUG << "Can receiver finished recv once: " - << can_receiver_->IsFinishRecvOnce(); - if (message_manager_->GetSensorRecvData(&chassis_detail_receiver) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_receiver is " - << chassis_detail_receiver.ShortDebugString(); - size_t receiver_data_size = chassis_detail_receiver.ByteSizeLong(); - ADEBUG << "check chassis detail receiver_data_size is " << receiver_data_size; - // check receiver data is null - if (receiver_data_size < 2) { - if (is_need_count_) { - lost_chassis_reveive_detail_count_++; - } - } else { - lost_chassis_reveive_detail_count_ = 0; - is_need_count_ = true; - } - ADEBUG << "lost_chassis_reveive_detail_count_ is " - << lost_chassis_reveive_detail_count_; - // check receive data lost threshold is (100 * 10)ms - if (lost_chassis_reveive_detail_count_ > 100) { - is_need_count_ = false; - is_chassis_communication_error_ = true; - AERROR << "neolix chassis detail is lost, please check the communication " - "error."; - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorData(); - return true; - } else { - is_chassis_communication_error_ = false; - } - - %(car_type_cap)s chassis_detail_sender; - if (message_manager_->GetSensorSenderData(&chassis_detail_sender) != - ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis receive detail failed."; - } - ADEBUG << "chassis_detail_sender is " - << chassis_detail_sender.ShortDebugString(); - size_t sender_data_size = chassis_detail_sender.ByteSizeLong(); - ADEBUG << "check chassis detail sender_data_size is " << sender_data_size; - - message_manager_->ClearSensorRecvData(); - message_manager_->ClearSensorSenderData(); - return false; -} - bool %(car_type_cap)sController::CheckChassisError() { if (is_chassis_communication_error_) { AERROR_EVERY(100) << "ChassisDetail has no %(car_type_lower)s vehicle info."; return false; } - %(car_type_cap)s chassis_detail; - if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) { - AERROR_EVERY(100) << "Get chassis detail failed."; - } /* ADD YOUR OWN CAR CHASSIS OPERATION // steer fault @@ -581,24 +525,23 @@ bool %(car_type_cap)sController::CheckResponse(const int32_t flags, bool need_wa bool is_eps_online = false; bool is_vcu_online = false; bool is_esp_online = false; - Chassis chassis = Chassis(); do { bool check_ok = true; if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { - is_eps_online = chassis.has_check_response() && - chassis.check_response().has_is_eps_online() && - chassis.check_response().is_eps_online(); + is_eps_online = chassis_.has_check_response() && + chassis_.check_response().has_is_eps_online() && + chassis_.check_response().is_eps_online(); check_ok = check_ok && is_eps_online; } if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { - is_vcu_online = chassis.has_check_response() && - chassis.check_response().has_is_vcu_online() && - chassis.check_response().is_vcu_online(); - is_esp_online = chassis.has_check_response() && - chassis.check_response().has_is_esp_online() && - chassis.check_response().is_esp_online(); + is_vcu_online = chassis_.has_check_response() && + chassis_.check_response().has_is_vcu_online() && + chassis_.check_response().is_vcu_online(); + is_esp_online = chassis_.has_check_response() && + chassis_.check_response().has_is_esp_online() && + chassis_.check_response().is_esp_online(); check_ok = check_ok && is_vcu_online && is_esp_online; } if (check_ok) { @@ -613,9 +556,14 @@ bool %(car_type_cap)sController::CheckResponse(const int32_t flags, bool need_wa } } while (need_wait && retry_num); - AERROR << "check_response fail: is_eps_online:" << is_eps_online - << ", is_vcu_online:" << is_vcu_online - << ", is_esp_online:" << is_esp_online; + if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) { + AERROR << "steer check_response fail: is_eps_online:" << is_eps_online; + } + + if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) { + AERROR << "speed check_response fail: " << "is_vcu_online:" << is_vcu_online + << ", is_esp_online:" << is_esp_online; + } return false; } diff --git a/modules/tools/gen_vehicle_protocol/template/vehicle_factory.cc.tpl b/modules/tools/gen_vehicle_protocol/template/vehicle_factory.cc.tpl index 3cecec44aaa..bd621b70255 100644 --- a/modules/tools/gen_vehicle_protocol/template/vehicle_factory.cc.tpl +++ b/modules/tools/gen_vehicle_protocol/template/vehicle_factory.cc.tpl @@ -69,7 +69,6 @@ bool %(car_type_cap)sVehicleFactory::Init(const CanbusConf *canbus_conf) { AINFO << "The vehicle controller is successfully created."; if (vehicle_controller_->Init(canbus_conf->vehicle_parameter(), &can_sender_, - &can_receiver_, message_manager_.get()) != ErrorCode::OK) { AERROR << "Failed to init vehicle controller."; return false; @@ -84,6 +83,9 @@ bool %(car_type_cap)sVehicleFactory::Init(const CanbusConf *canbus_conf) { chassis_detail_writer_ = node_->CreateWriter<::apollo::canbus::%(car_type_cap)s>(FLAGS_chassis_detail_topic); + chassis_detail_sender_writer_ = node_->CreateWriter<::apollo::canbus::%(car_type_cap)s>( + FLAGS_chassis_detail_sender_topic); + return true; } @@ -152,12 +154,18 @@ Chassis %(car_type_cap)sVehicleFactory::publish_chassis() { } void %(car_type_cap)sVehicleFactory::PublishChassisDetail() { - %(car_type_cap)s chassis_detail; - message_manager_->GetSensorData(&chassis_detail); - ADEBUG << chassis_detail.ShortDebugString(); + %(car_type_cap)s chassis_detail = vehicle_controller_->GetNewRecvChassisDetail(); + ADEBUG << "latest chassis_detail is " << chassis_detail.ShortDebugString(); chassis_detail_writer_->Write(chassis_detail); } +void %(car_type_cap)sVehicleFactory::PublishChassisDetailSender() { + %(car_type_cap)s sender_chassis_detail = vehicle_controller_->GetNewSenderChassisDetail(); + ADEBUG << "latest sender_chassis_detail is " + << sender_chassis_detail.ShortDebugString(); + chassis_detail_sender_writer_->Write(sender_chassis_detail); +} + void %(car_type_cap)sVehicleFactory::UpdateHeartbeat() { can_sender_.Update_Heartbeat(); } @@ -169,9 +177,28 @@ bool %(car_type_cap)sVehicleFactory::CheckChassisCommunicationFault() { return false; } -std::unique_ptr<%(car_type_lower)s::%(car_type_cap)sController> +void %(car_type_cap)sVehicleFactory::AddSendProtocol() { + vehicle_controller_->AddSendMessage(); +} + +void %(car_type_cap)sVehicleFactory::ClearSendProtocol() { + can_sender_.ClearMessage(); +} + +bool %(car_type_cap)sVehicleFactory::IsSendProtocolClear() { + if (can_sender_.IsMessageClear()) { + return true; + } + return false; +} + +Chassis::DrivingMode %(car_type_cap)sVehicleFactory::Driving_Mode() { + return vehicle_controller_->driving_mode(); +} + +std::unique_ptr> %(car_type_cap)sVehicleFactory::CreateVehicleController() { - return std::unique_ptr<%(car_type_lower)s::%(car_type_cap)sController>( + return std::unique_ptr>( new %(car_type_lower)s::%(car_type_cap)sController()); } diff --git a/modules/tools/gen_vehicle_protocol/template/vehicle_factory.h.tpl b/modules/tools/gen_vehicle_protocol/template/vehicle_factory.h.tpl index 398afd13e80..804c8a737b2 100644 --- a/modules/tools/gen_vehicle_protocol/template/vehicle_factory.h.tpl +++ b/modules/tools/gen_vehicle_protocol/template/vehicle_factory.h.tpl @@ -89,27 +89,53 @@ class %(car_type_cap)sVehicleFactory : public AbstractVehicleFactory { */ Chassis publish_chassis() override; + /** + * @brief create cansender heartbeat + */ + void UpdateHeartbeat() override; + /** * @brief publish chassis for vehicle messages */ void PublishChassisDetail() override; /** - * @brief create cansender heartbeat + * @brief publish chassis for apollo sender messages + */ + void PublishChassisDetailSender() override; + + /** + * @brief check chassis can receiver lost + */ + bool CheckChassisCommunicationFault() override; + + /** + * @brief add the can sender messages + */ + void AddSendProtocol() override; + + /** + * @brief clear the can sender messages */ - void UpdateHeartbeat(); + void ClearSendProtocol() override; /** - * @brief check the chassis detail can receive lost + * @brief check the sender message clear or not */ - bool CheckChassisCommunicationFault(); + bool IsSendProtocolClear() override; + + /** + * @brief get the latest chassis driving mode + */ + Chassis::DrivingMode Driving_Mode() override; private: /** * @brief create %(car_type_lower)s vehicle controller * @returns a unique_ptr that points to the created controller */ - std::unique_ptr<%(car_type_lower)s::%(car_type_cap)sController> CreateVehicleController(); + std::unique_ptr> + CreateVehicleController(); /** * @brief create %(car_type_lower)s message manager @@ -122,10 +148,12 @@ class %(car_type_cap)sVehicleFactory : public AbstractVehicleFactory { CanSender<::apollo::canbus::%(car_type_cap)s> can_sender_; apollo::drivers::canbus::CanReceiver<::apollo::canbus::%(car_type_cap)s> can_receiver_; std::unique_ptr> message_manager_; - std::unique_ptr<%(car_type_lower)s::%(car_type_cap)sController> vehicle_controller_; + std::unique_ptr> vehicle_controller_; std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::%(car_type_cap)s>> chassis_detail_writer_; + std::shared_ptr<::apollo::cyber::Writer<::apollo::canbus::%(car_type_cap)s>> + chassis_detail_sender_writer_; }; CYBER_REGISTER_VEHICLEFACTORY(%(car_type_cap)sVehicleFactory)