活动介绍
file-type

C语言实现十进制转二进制及十六进制示例详解

PDF文件

下载需积分: 50 | 13.06MB | 更新于2024-08-05 | 102 浏览量 | 39 下载量 举报 收藏
download 立即下载
本资源主要讲解了如何在C语言中进行十进制与其他进制之间的转换,重点涉及了从十进制转换到十六进制和二进制的方法。C语言提供了内置的格式控制符,如`%d`用于十进制输出,`%x`用于十六进制输出,这在`printf()`和`scanf()`函数中起到关键作用。 在实例001中,用户被引导从键盘输入一个十进制整数,通过`scanf()`获取输入,然后使用`printf()`输出转换后的十六进制值。关键步骤包括: 1. 创建C文件并引用`stdio.h`头文件。 2. 使用`scanf()`函数接收用户输入,并使用`&`符号连接变量名和地址。 3. 使用`printf()`函数输出,格式化字符串中包含`%x`,表示输出十六进制值。 实例002则展示了十进制转换为二进制的过程,涉及到将十进制数转换为二进制数的具体算法,这通常通过循环和取余操作完成。使用数组存储每次对2取余的结果,并确保循环的边界设置正确,以适应数组的索引和实际转换过程。此外,使用`%`和`/`运算符进行模运算和除法操作,确保在循环体内正确使用花括号。 通过这两个实例,读者可以掌握基本的进制转换技巧,并能够扩展到其他进制间的转换,如二进制转十进制、八进制转十进制等。同时,这些实例有助于提升编程实践能力,将纸上的运算逻辑转化为计算机程序。整个章节还包括了条件判断、循环应用、数组操作、字符和字符串处理以及函数的使用等内容,全面覆盖了C语言的基础知识,适合学习者逐步深入理解和掌握C语言编程技巧。

相关推荐

filetype

#include "master.h" #include <log_custom.h> #include <cstddef> #include <cstdint> #include <map> #include <string> #include <base/params_manager/params_manager.h> #include <perception_and_ld_map_fusion/data_fusion/geometry_match_info.h> #include "fmt/core.h" #include "lib/sd_navigation/SDMapElementExtract.h" #include "message/env_model/routing_map/routing_map.h" #include "message/env_model/occ/occ.h" #include "message/sensor/camera/bev_lane/bev_lane.h" #include "cyber/event/trace.h" #include "cyber/time/time.h" #include "modules/perception/env/src/lib/base/localmap_conf.h" #include "modules/simulator/deterministic_scheduler/deterministic_scheduler.h" using byd::simulator::deterministic_scheduler::SchedulerUnitManager; using byd::simulator::deterministic_scheduler::TaskType; using namespace cem::fusion; using namespace cem::fusion::extendLane; namespace cem { namespace fusion { std::shared_ptr<cem::fusion::Master> MasterPtr = nullptr; std::shared_ptr<MasterInterface> GetMasterInstance() { if (!MasterPtr) { MasterPtr = std::shared_ptr<Master>(new Master("modules/perception/env", "local_map")); } return MasterPtr; } Master::Master(const std::string &confdir, const std::string &name) { conf_abs_path_ = confdir + std::string("/"); VLOG(1) << "conf_abs_path_ = " << conf_abs_path_; Init(); } void Master::Init() { LoadConfig(); InitMember(); } void Master::LoadConfig() { ParamsManager::Instance()->ReadJsonParams(conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_TrafficRoadFusion.json", conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_MeasurementEvaluation.json"); } void Master::InitMember() { routing_map_.reset(new RoutingMap()); pre_processor_.reset(new PreProcessor()); post_processor_.reset(new PostProcessor()); messenger_.reset(new Messenger()); stopline_mapping_.reset(new StoplineMapping); traffic_light_mapping_.reset(new TrafficLightMapping); bev_map_processor_.reset(new BevMapPreProcessor()); fusion_manager_.reset(new FusionManager()); fusion_manager_->Init(); map_fusion_.reset(new MapFusion); lane_guidance_.reset(new LaneGuidance); lane_guidance_city_.reset(new LaneGuidanceCity); env_info_processor_.reset(new navigation::EnvInfoProcessor()); traffic_flow_mapping_.reset(new TrafficFlowMapping); toggle_to_bev_info_.push(MapType::BEV_MAP); cross_road_construction_.reset(new CrossRoadConstruction); cross_road_construction_->Init(); extend_lane_.reset(new ExtendLane); debug_infos_.reset(new DebugInfos); sd_navigation_base_ = std::make_shared<navigation::SdNavigationBase>(); sd_navigation_highway_ = std::make_shared<navigation::SdNavigationHighway>(); sd_navigation_city_ = std::make_shared<navigation::SdNavigationCity>(); if (FLAGS_init_map_source == "ld" || FLAGS_init_map_source == "LD") { toggle_to_bev_info_.push(MapType::HD_MAP); } else { toggle_to_bev_info_.push(MapType::BEV_MAP); } } #define MapLessFlag void Master::Proc() { env_status_ = std::make_shared<byd::msg::orin::routing_map::EnvStatus>(); //get previous cross_road status and cross road distance pre_processor_->is_cross_road_status = cross_road_construction_->IsCrossRoad(); pre_processor_->cross_road_distance = cross_road_construction_->CrossRoadDis(); pre_processor_->Process(); //get previous cross_road status pre_processor_->is_cross_road_status = cross_road_construction_->IsCrossRoad(); RoutingMapPtr routing_map_raw{nullptr}; SensorDataManager::Instance()->GetLatestSensorFrame(routing_map_raw); auto raw_bev_map_ptr = INTERNAL_PARAMS.raw_bev_data.GetRawBevMapPtr(); ///高速和城区的切换 bool IS_ON_HIGHWAY_FLAG = false; { IS_ON_HIGHWAY_FLAG = GetIsOnHighwayFlag(routing_map_raw); if (IS_ON_HIGHWAY_FLAG) { sd_navigation_base_ = sd_navigation_highway_; } else { sd_navigation_base_ = sd_navigation_city_; } } env_status_->set_is_on_freeway(IS_ON_HIGHWAY_FLAG == true); DetectBevMap bev_map = pre_processor_->GetGlobalBevMapPtr(); ///停止线处理 stopline_mapping_->Process(); const auto &bev_stopline_objs = stopline_mapping_->GetStopLines(); ///导航推荐 std::vector<std::pair<uint64_t, std::pair<int, int>>> guide_lane_result = {}; std::vector<uint64_t> sd_guide_lane_ids_ref = {}; sd_navigation_base_->Proc(routing_map_raw, raw_bev_map_ptr, bev_map.bev_map_ptr, guide_lane_result, sd_guide_lane_ids_ref); auto junctions_ptr = INTERNAL_PARAMS.navigation_info_data.GetJunctionInfoCityPtr(); if ((!IS_ON_HIGHWAY_FLAG) && (routing_map_raw != nullptr)) { auto trajectory = sd_navigation_city_->GetNextRightPoints(routing_map_raw); pre_processor_->SetRightTurnTrajectory(routing_map_raw->header.timestamp, trajectory); } ///env_info env_info_processor_->SetIsOnHighway(IS_ON_HIGHWAY_FLAG); env_info_processor_->Process(bev_stopline_objs); const EnvInfo *env_info = env_info_processor_->GetEnvInfo(); if (bev_map.bev_map_ptr != nullptr) { bev_map.bev_map_ptr->env_info = *env_info; } bev_map_processor_->Process(bev_map, sd_guide_lane_ids_ref); fusion_manager_->Process(); const auto routing_map = pre_processor_->routing_map_pre_processor_.GetRoutingMap(); if (routing_map != nullptr) { routing_map->env_info = *env_info; } extend_lane_->Process(bev_map); bev_ld_map_geometry_match_info_.CalculateMapGeometryMatchInfo(fusion_manager_->GetBevMapInfo(), fusion_manager_->GetLdMapInfo()); // cross road part cross_road_construction_->SetGeometryMatchInfo(&bev_ld_map_geometry_match_info_); cross_road_construction_->Process(bev_map.bev_map_ptr, bev_map.Twb, raw_bev_map_ptr, junctions_ptr); // Post-processing of the bev map and intergration the guide lane, virtual // lane, traffic light status, etc. lane_guidance_city_->Process(routing_map_raw, bev_map, bev_stopline_objs, guide_lane_result); traffic_light_mapping_->Process(routing_map_raw, routing_map, lane_guidance_city_->GetMapInfo(), sd_navigation_city_); GeometryMatchResult match_detail; auto map_convert_type = MapToggleMode(match_detail); messenger_->PulishDiagno(); messenger_->PulishTrafficLightE2E(traffic_light_mapping_->GetTrafficLightsE2E()); bool used_ld_map = toggle_to_bev_info_.back() == MapType::HD_MAP; uint32_t map_counter = 0; // std::string map_type_str = used_ld_map ? "ld" : "bev"; map_counter = used_ld_map ? (routing_map ? routing_map->header.cycle_counter : 2) : (lane_guidance_city_->GetMapInfo() ? lane_guidance_city_->GetMapInfo()->header.cycle_counter : 1); const auto &map_convert_params = ParamsManager::Instance()->GetMapConvertParams(); if (IS_ON_HIGHWAY_FLAG) { used_ld_map = false; env_status_->clear_hd_map_suppression_reason(); env_status_->clear_perception_map_suppression_reason(); if (env_info->is_switched_to_LD_ && match_detail.fail_reason != kLostHdMapLane) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_THE_RAMP); used_ld_map = true; } else { if (match_detail.fail_reason == kLostHdMapLane) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_OUTSIDE_MAP); } else { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_NOT_IN_THE_RAMP); } used_ld_map = false; } env_status_->clear_distance_to_next_junction(); env_status_->clear_distance_to_passed_junction(); env_status_->clear_left_laneline_match_info(); env_status_->clear_right_laneline_match_info(); // todo: add env status here } else { if (map_convert_params.map_stay_ld) { used_ld_map = true; } else if (map_convert_params.map_stay_bev) { used_ld_map = false; } } std::string map_type_str = used_ld_map ? "ld" : "bev"; if (!map_convert_params.map_stay_ld && used_ld_map) { const bool ld_has{false}; // const bool ld_has = bev_ld_map_geometry_match_info_.LDHasEnoughPoints(); // const bool bev_has = bev_ld_map_geometry_match_info_.BEVHasEnoughPoints(); if (!routing_map || routing_map->route.navi_start.section_id == 0U) { used_ld_map = false; if (!toggle_to_bev_info_.empty()) { toggle_to_bev_info_.pop(); } toggle_to_bev_info_.emplace(MapType::BEV_MAP); if (!routing_map) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_NO_HD_MAP_FRAME); } else { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_NO_NAVIGATION_START_SECTION); } env_status_->clear_perception_map_suppression_reason(); map_type_str = fmt::format("bev ld_less_points.[{:d}-{}-{:d}]", static_cast<bool>(routing_map), routing_map ? routing_map->route.navi_start.section_id : 0, ld_has); } } RoutingMapPtr routing_map_copy_ptr = nullptr; if (nullptr != routing_map) { bool use_ld_tmp = used_ld_map; if (true == used_ld_map) { routing_map->sensor_status_info.sensor_status = 2; } post_processor_->run(routing_map, used_ld_map); if (use_ld_tmp && !used_ld_map) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_CLOSE_TO_HIGH_LEVEL_ROAD); env_status_->clear_perception_map_suppression_reason(); env_status_->clear_distance_to_next_junction(); env_status_->clear_distance_to_passed_junction(); env_status_->clear_left_laneline_match_info(); env_status_->clear_right_laneline_match_info(); } map_fusion_->SetGeometryMatchInfo(&bev_ld_map_geometry_match_info_); routing_map_copy_ptr = std::make_shared<RoutingMap>(*routing_map); map_fusion_->Process(bev_map, routing_map_copy_ptr); } if (used_ld_map) { env_status_->set_map_source(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_HD_MAP); } else { env_status_->set_map_source(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_PERCEPTION_MAP); } #ifdef MapLessFlag used_ld_map = false; #endif debug_infos_->SetDebugInfo(used_ld_map, lane_guidance_city_->GetMapInfo(), lane_guidance_city_->GetTwb(), routing_map_copy_ptr); messenger_->PulishFusionMap(!used_ld_map, lane_guidance_city_->GetMapInfo(), routing_map_copy_ptr, bev_stopline_objs, IS_ON_HIGHWAY_FLAG, sd_navigation_city_->GetInfo()); if (auto map_output = messenger_->GetFusionMapInfo(); map_output) { double adc_to_junction = traffic_light_mapping_->DistanceToJunction(); uint64_t ld_counter = routing_map == nullptr ? 0 : routing_map->header.cycle_counter; uint64_t bev_counter = lane_guidance_city_->GetMapInfo() == nullptr ? 0 : lane_guidance_city_->GetMapInfo()->header.cycle_counter; map_output->mutable_map_info()->mutable_env_status()->CopyFrom(*env_status_); map_output->mutable_map_info()->set_map_version( fmt::format("map_type:{} ld_counter:{} bev_counter:{} vision_counter:{} " "adc_to_junction:{:.2f} map_change_type:{} " "stay_ld:{:d} stay_bev:{:d} IS_ON_HIGHWAY_FLAG:{:d} " "used_ld_map:{:d} is_switched:{:d}", map_type_str, ld_counter, bev_counter, traffic_light_mapping_->GetPerceptionCyberCounter(), adc_to_junction, to_string(map_convert_type), map_convert_params.map_stay_ld, map_convert_params.map_stay_bev, IS_ON_HIGHWAY_FLAG, used_ld_map, env_info->is_switched_to_LD_) + sd_navigation_city_->GetInfo()); map_output->mutable_map_info()->set_engine_version( bev_ld_map_geometry_match_info_.GetDebugInfo() + fmt::format(" is_keep_lane:{:d} is_right:{:d} ", is_lane_keeping_, sd_navigation_city_->IsEgoInDedicatedRightLane()) + traffic_light_mapping_->TrafficLightLaneDebug()); GEOMETRY_LOG << map_output->map_info().map_version(); TRAFFIC_LOG << map_output->map_info().engine_version(); } } bool Master::GetIsOnHighwayFlag(const RoutingMapPtr &routing_map_raw) { auto is_on_highway_flag = [](const SDSectionInfo &sd_section_info) { auto ¤t_road_class = sd_section_info.road_class; uint32_t current_section_type = sd_section_info.link_type; uint64_t sd_section_id = sd_section_info.id / 10; if ((sd_section_id == 1678534592) || (sd_section_id == 1699361336) || (sd_section_id == 1699374097) || (sd_section_id == 1691246158) || (sd_section_id == 1607693456) || (sd_section_id == 1607693601) || (sd_section_id == 1648672436) || (sd_section_id == 1648672824) || (sd_section_id == 1678890528) || (sd_section_id == 1705771743) || (sd_section_id == 1705771555) || (sd_section_id == 1699402002) || (sd_section_id == 1699402004) || (sd_section_id == 1613352349) || (sd_section_id == 1678533134) || (sd_section_id == 1699372997) || (sd_section_id == 1699374094) || (sd_section_id == 1607690036) || (sd_section_id == 1607690332) || (sd_section_id == 1664789284) || (sd_section_id == 1664789271) || (sd_section_id == 1648672814) || (sd_section_id == 1678890452) || (sd_section_id == 1699443498) || (sd_section_id == 1705771744) || (sd_section_id == 1705771880) || (sd_section_id == 1705771278) || (sd_section_id == 1705771745) || (sd_section_id == 1699402006) || (sd_section_id == 1699402007) || (sd_section_id == 1705771745) || (sd_section_id == 1663998523) || (sd_section_id == 1582251492)) { return false; } if (current_road_class == SDRoadClass::SD_HIGHWAY || current_road_class == SDRoadClass::SD_CITY_FAST_WAY || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_JCT) == (uint32_t)SDLinkTypeMask::SDLT_JCT || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_SERVICE) == (uint32_t)SDLinkTypeMask::SDLT_SERVICE) { return true; } return false; }; auto is_highway_ramp = [](const SDSectionInfo &sd_section_info) { uint32_t section_type = sd_section_info.link_type; if ((section_type & (uint32_t)SDLinkTypeMask::SDLT_JCT) == (uint32_t)SDLinkTypeMask::SDLT_JCT || (section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC || (section_type & (uint32_t)SDLinkTypeMask::SDLT_SERVICE) == (uint32_t)SDLinkTypeMask::SDLT_SERVICE || (section_type & (uint32_t)SDLinkTypeMask::SDLT_PARK) == (uint32_t)SDLinkTypeMask::SDLT_PARK || (section_type & (uint32_t)SDLinkTypeMask::SDLT_RAMP) == (uint32_t)SDLinkTypeMask::SDLT_RAMP || (sd_section_info.road_class == SDRoadClass::SD_OTHER_ROAD && (section_type & (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION) == (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION)) { return true; } else { return false; } }; auto is_highway_city_connection = [is_on_highway_flag, is_highway_ramp]( const SDSectionInfo &pre_sd_section_info, const SDSectionInfo &cur_sd_section_info, const SDSectionInfo &suc_sd_section_info) { uint32_t cur_section_type = cur_sd_section_info.link_type; if ((pre_sd_section_info.successor_section_id_list.size() == 1) && (is_on_highway_flag(pre_sd_section_info)) && ((cur_section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC) && (!is_on_highway_flag(suc_sd_section_info) && (!is_highway_ramp(suc_sd_section_info)))) { return true; } else { return false; } }; if (routing_map_raw == nullptr) { return false; } auto &sd_mpp_sections = routing_map_raw->sd_route.mpp_sections; uint64_t sd_ego_sectionid = routing_map_raw->sd_route.navi_start.section_id; int ego_section_index = -1; for (unsigned int i = 0; i < sd_mpp_sections.size(); i++) { if (sd_ego_sectionid == sd_mpp_sections[i].id) { ego_section_index = i; break; } } if (ego_section_index == -1) { return routing_map_raw->is_on_highway; } auto ¤t_section = sd_mpp_sections[ego_section_index]; bool IS_ON_HIGHWAY_FLAG = routing_map_raw->is_on_highway; if (IS_ON_HIGHWAY_FLAG) { if (!is_on_highway_flag(current_section)) { return false; } if (ego_section_index >= 1 && ego_section_index < sd_mpp_sections.size() - 1) { if (is_highway_city_connection(sd_mpp_sections[ego_section_index - 1], current_section, sd_mpp_sections[ego_section_index + 1])) { return false; } } if (is_highway_ramp(current_section)) { return true; } double length_tmp = fabs(current_section.length - routing_map_raw->sd_route.navi_start.s_offset); double max_range = 1500; for (unsigned int i = ego_section_index + 1; i < sd_mpp_sections.size(); i++) { if (max_range < length_tmp) { break; } if (i >= 1 && i < sd_mpp_sections.size() - 1) { if (is_highway_city_connection(sd_mpp_sections[i - 1], sd_mpp_sections[i], sd_mpp_sections[i + 1])) { IS_ON_HIGHWAY_FLAG = false; break; } } if (is_on_highway_flag(sd_mpp_sections[i]) && is_highway_ramp(sd_mpp_sections[i])) { break; } if (!is_on_highway_flag(sd_mpp_sections[i])) { IS_ON_HIGHWAY_FLAG = false; break; } length_tmp += sd_mpp_sections[i].length; } return IS_ON_HIGHWAY_FLAG; } else { return false; } } MapConvertType Master::MapToggleMode(GeometryMatchResult &match_detail) { static constexpr size_t queue_max_size = 30; if (bev_ld_matched_info_.size() > queue_max_size) { bev_ld_matched_info_.pop(); } if (toggle_to_bev_info_.size() > queue_max_size) { toggle_to_bev_info_.pop(); } constexpr double distance_bev_to_ld_max = 150; constexpr double distance_bev_to_ld_min = 0.5; constexpr double distance_ld_to_bev_max = 150; constexpr double distance_threshold = 100; const bool bev_ld_boundary_matched = bev_ld_map_geometry_match_info_.IsLaneBoundaryMatch(match_detail); double adc_to_junction = traffic_light_mapping_->DistanceToJunction(); bool is_drive_into_unreliable_road = traffic_light_mapping_->IsUnreliableRoadCrossJunction(); is_drive_into_unreliable_road = false; bool distance_bev_to_ld = adc_to_junction < distance_bev_to_ld_max && adc_to_junction > distance_bev_to_ld_min; bool bev_ld_matched_current_period = distance_bev_to_ld && bev_ld_boundary_matched; double dis_ego_to_passed_junction = traffic_light_mapping_->GetPreviousJunctionEgoPassed(); LaneInfo ego_lane_ld = bev_ld_map_geometry_match_info_.GetLdBevLane(); bool ego_in_junction = ego_lane_ld.id != 0 && ego_lane_ld.type == cem::message::env_model::LaneType::LANE_VIRTUAL_JUNCTION; PLanningResultPtr planning_result{nullptr}; SensorDataManager::Instance()->GetLatestSensorFrame(planning_result); if (planning_result) { if (planning_result->lc_state == PLanningResultData::Keeping) { is_lane_keeping_ = true; } else { is_lane_keeping_ = false; } } if (!env_status_->is_on_freeway() && ((match_detail.fail_reason == MatchFailReason::kNotFailed) || (match_detail.fail_reason == MatchFailReason::kMatchFailed))) { env_status_->set_distance_to_next_junction(adc_to_junction); env_status_->set_distance_to_passed_junction(dis_ego_to_passed_junction); env_status_->mutable_left_laneline_match_info()->set_average_offset(match_detail.left_offset_average); env_status_->mutable_left_laneline_match_info()->set_max_offset(match_detail.left_offset_max); env_status_->mutable_right_laneline_match_info()->set_average_offset(match_detail.right_offset_average); env_status_->mutable_right_laneline_match_info()->set_max_offset(match_detail.right_offset_max); } if (env_status_->is_on_freeway() && match_detail.fail_reason == MatchFailReason::kLostHdMapLane && (!is_lane_keeping_)) { match_detail.fail_reason = MatchFailReason::kNotFailed; } GEOMETRY_LOG << fmt::format( " bev_ld_boundary_matched:{:d} distance_to_junction:{:.2f} ego_in_junction:{:d} vector_type:{:d} size:{:d} " "ego_dis_to_last_junction:{:.2f} is_lane_keeping:{:d} bev_ld_matched_current_period:{:d}", bev_ld_boundary_matched, adc_to_junction, ego_in_junction, (toggle_to_bev_info_.empty() ? MapType::UNKOWN_MAP : toggle_to_bev_info_.back()), toggle_to_bev_info_.size(), dis_ego_to_passed_junction, is_lane_keeping_, bev_ld_matched_current_period); switch (toggle_to_bev_info_.back()) { case MapType::HD_MAP: { if (adc_to_junction > distance_ld_to_bev_max && bev_ld_boundary_matched && dis_ego_to_passed_junction > distance_threshold && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::BEV_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_PASSED_JUNCTION); return MapConvertType::LD_2_BEV; } if (!is_lane_keeping_) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IS_CHANGING_LANE); } else if (adc_to_junction <= distance_ld_to_bev_max || dis_ego_to_passed_junction <= distance_threshold) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); } else { env_status_->set_perception_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP); } toggle_to_bev_info_.emplace(MapType::HD_MAP); return MapConvertType::LD_2_LD; } case MapType::BEV_MAP: { if (bev_ld_matched_current_period && is_lane_keeping_ && (!is_drive_into_unreliable_road)) { toggle_to_bev_info_.emplace(MapType::HD_MAP); env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); return MapConvertType::BEV_2_LD; } if (adc_to_junction < distance_bev_to_ld_min) { toggle_to_bev_info_.emplace(MapType::UNKOWN_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); return MapConvertType::BEV_2_UNKNOWN; } toggle_to_bev_info_.emplace(MapType::BEV_MAP); if (!is_lane_keeping_) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IS_CHANGING_LANE); } else if (is_drive_into_unreliable_road) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_CLOSE_TO_HIGH_LEVEL_ROAD); } else { if (adc_to_junction > distance_bev_to_ld_max) { env_status_->set_hd_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_EGO_NOT_CLOSE_ENOUGH_TO_JUNCTION); } else { env_status_->set_hd_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP); } } return MapConvertType::BEV_2_BEV; } case MapType::UNKOWN_MAP: { if (bev_ld_matched_current_period && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::HD_MAP); env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); return MapConvertType::UNKNOWN_2_LD; } if (adc_to_junction > distance_bev_to_ld_min * 2 && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::BEV_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); return MapConvertType::UNKNOWN_2_BEV; } env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); toggle_to_bev_info_.emplace(MapType::UNKOWN_MAP); return MapConvertType::UNKNOWN_2_UNKNOWN; } default: { toggle_to_bev_info_.emplace(MapType::BEV_MAP); return MapConvertType::DEFAULT_BEV; } } toggle_to_bev_info_.emplace(MapType::BEV_MAP); return MapConvertType::DEFAULT_BEV; } /**********************************发送消息的数据结构接口************************************/ std::shared_ptr<byd::msg::basic::ModuleStatus> Master::GetModuleStatusPtr() { return this->messenger_->GetDiagnoInfo(); } std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> Master::GetENVRoutingMapPtr() { return this->messenger_->GetRoutingMapInfo(); } std::shared_ptr<byd::msg::orin::routing_map::TrafficLightsE2EInfo> Master::GetTrafficLightsE2EPtr() { return this->messenger_->GetTrafficLightsE2EInfo(); } /***************************接收消息callback函数 * 在此处实现************************************/ void Master::OnVehInfoCallback(const std::shared_ptr<byd::msg::drivers::VehInfo> &msg) { this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::VehicleSignal>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Bcm50msPdu02>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu08>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Imcu20msPdu05>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Eps010msPdu00>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu15>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Sas10msPdu00>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Mpd100msPdu04>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Icm100msPdu29>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::AdasIpd100msPdu12>(msg); } void Master::OnEnvModelLocalMapInfoCallback(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg) { this->messenger_->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::BevMapInfo>(msg); } void Master::OnRoutingMapCallBack(const std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> &msg) { this->messenger_->Callback<byd::msg::orin::routing_map::RoutingMap, cem::message::env_model::RoutingMap>(msg); } void Master::OnMsgMapEventCallback(const std::shared_ptr<byd::msg::orin::routing_map::MapEvent> &msg) { this->messenger_->Callback<byd::msg::orin::routing_map::MapEvent, cem::message::env_model::MapEvent>(msg); } /* OnMsgMapEventCallback */ void Master::OnPercepTrfInfoCallback(const std::shared_ptr<byd::msg::perception::PerceptionTrafficInfo> &msg) { this->messenger_->Callback<byd::msg::perception::PerceptionTrafficInfo, cem::message::sensor::PercepTrfInfo>(msg); } void Master::OnPerceptionObstaclesInfoCallback(const std::shared_ptr<byd::msg::perception::PerceptionObstacles> &msg) { this->messenger_->Callback<byd::msg::perception::PerceptionObstacles, cem::message::sensor::FusObjInfo>(msg); } void Master::OnMsgDriverInsCallback(const std::shared_ptr<byd::msg::drivers::Ins> &msg) { this->messenger_->Callback<byd::msg::drivers::Ins, cem::message::sensor::LocalizationData>(msg); } void Master::OnMsgLocationDrCallback(const std::shared_ptr<byd::msg::localization::LocalizationEstimate> &msg) { this->messenger_->Callback<byd::msg::localization::LocalizationEstimate, cem::message::sensor::LocalizationData>(msg); } // void Master::OncanoutCallback( // const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg) // { // this->messenger_->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output, // cem::message::sensor::CAN1>(msg); // } void Master::OnEnvModelLidaRoadedgeCallback(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg) { this->messenger_->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::LidarRoadEdgeInfo>(msg); } void Master::OnPlanResultCallback(const std::shared_ptr<byd::msg::planning::PLanningResultProto> &msg) { this->messenger_->Callback<byd::msg::planning::PLanningResultProto, cem::message::env_model::PLanningResultData>(msg); } void Master::OnPlanFuncStateCallback(const std::shared_ptr<byd::msg::pnc::PlanFuncState> &msg) { this->messenger_->Callback<byd::msg::pnc::PlanFuncState, cem::message::env_model::PLanFuncState>(msg); } void Master::OncanoutCallback(const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg) { this->messenger_->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output, cem::message::env_model::CAN1>(msg); } void Master::OnOccCallback(const std::shared_ptr<byd::msg::perception::OCCInfo>& msg){ this->messenger_->Callback<byd::msg::perception::OCCInfo,cem::env_model::occ::OCCInfo>(msg); return; } /***************************************************************************************************/ } // namespace fusion } // namespace cem 解读下代码

filetype

``` void PositionControl::_velocityControl(const float dt) { // Constrain vertical velocity integral _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G); // PID velocity control Vector3f vel_error = _vel_sp - _vel; Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); _accelerationControl(); // Integrator anti-windup in vertical direction if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) { vel_error(2) = 0.f; } // Prioritize vertical control while keeping a horizontal margin const Vector2f thrust_sp_xy(_thr_sp); const float thrust_sp_xy_norm = thrust_sp_xy.norm(); const float thrust_max_squared = math::sq(_lim_thr_max); // Determine how much vertical thrust is left keeping horizontal margin const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin); const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust); // Saturate maximal vertical thrust _thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared)); // Determine how much horizontal thrust is left after prioritizing vertical control const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); float thrust_max_xy = 0; if (thrust_max_xy_squared > 0) { thrust_max_xy = sqrtf(thrust_max_xy_squared); } // Saturate thrust in horizontal direction if (thrust_sp_xy_norm > thrust_max_xy) { _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; } // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); const float arw_gain = 2.f / _gain_vel_p(0); // The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently). // The ARW loop needs to run if the signal is saturated only. const Vector2f acc_sp_xy = _acc_sp.xy(); const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared()) ? acc_sp_xy_produced : acc_sp_xy; vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy); // Make sure integral doesn't get NAN ControlMath::setZeroIfNanVector3f(vel_error); // Update integral part of velocity control _vel_int += vel_error.emult(_gain_vel_i) * dt; }```将这段代码改成积分分离PID

淡墨1913
  • 粉丝: 33
上传资源 快速赚钱