From 4483bb7a7309e495c46922c567667594c6a851dc Mon Sep 17 00:00:00 2001 From: ajouatom Date: Fri, 28 Feb 2025 19:28:52 +0900 Subject: [PATCH] aLeadK&vLeadK, EV6BSM, CanParserDisplay --- common/params.cc | 1 + opendbc_repo/opendbc/can/parser.cc | 25 ++++++++++---- opendbc_repo/opendbc/car/hyundai/interface.py | 4 +-- scripts/add/events_ko.py | 32 ++++++++++++------ selfdrive/carrot_settings.json | 4 +-- .../lib/longitudinal_mpc_lib/long_mpc.py | 4 +-- selfdrive/controls/radard.py | 2 ++ selfdrive/selfdrived/events.py | 33 ++++++++++++------- selfdrive/ui/qt/offroad/settings.cc | 2 +- 9 files changed, 72 insertions(+), 35 deletions(-) diff --git a/common/params.cc b/common/params.cc index 403b81f..6d556fb 100644 --- a/common/params.cc +++ b/common/params.cc @@ -290,6 +290,7 @@ std::unordered_map keys = { {"RadarReactionFactor", PERSISTENT}, {"EnableRadarTracks", PERSISTENT}, {"EnableRadarTracksResult", PERSISTENT | CLEAR_ON_MANAGER_START}, + {"CanParserResult", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION }, {"HotspotOnBoot", PERSISTENT}, {"HyundaiCameraSCC", PERSISTENT}, {"CanfdHDA2", PERSISTENT}, diff --git a/opendbc_repo/opendbc/can/parser.cc b/opendbc_repo/opendbc/can/parser.cc index 0cfc88e..88e1169 100644 --- a/opendbc_repo/opendbc/can/parser.cc +++ b/opendbc_repo/opendbc/can/parser.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include "opendbc/can/common.h" @@ -210,8 +211,10 @@ void CANParser::UpdateCans(const CanData &can, std::set &updated_addre bus_timeout = (can.nanos - last_nonempty_nanos) > bus_timeout_threshold; } -int error_print_count = 0; void CANParser::UpdateValid(uint64_t nanos) { + static uint64_t last_log_time = 0; + const uint64_t LOG_INTERVAL = 2e9; + const bool show_missing = (nanos - first_nanos) > 8e9; bool _valid = true; @@ -227,14 +230,22 @@ void CANParser::UpdateValid(uint64_t nanos) { const bool timed_out = (nanos - state.last_seen_nanos) > state.check_threshold; if (state.check_threshold > 0 && (missing || timed_out)) { if (show_missing && !bus_timeout) { + char str[256]; + const char* parser_result_params = "/data/params/d/CanParserResult"; if (missing) { - if (error_print_count++ < 20) { - LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str()); - } + if ((nanos - last_log_time) > LOG_INTERVAL) { + last_log_time = nanos; + LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str()); + sprintf(str, "echo -n \"0x%X '%s' NOT SEEN\" > %s", state.address, state.name.c_str(), parser_result_params); + system(str); + } } else if (timed_out) { - if (error_print_count++ < 20) { - LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str()); - } + if ((nanos - last_log_time) > LOG_INTERVAL) { + last_log_time = nanos; + LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str()); + sprintf(str, "echo -n \"0x%X '%s' TIMED OUT\" > %s", state.address, state.name.c_str(), parser_result_params); + system(str); + } } } _valid = false; diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index efa833b..3b92c58 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase): cam_can = CanBus(None, fingerprint).CAM if camera_scc == 0 else 1 hda2 = False #0x50 in fingerprint[cam_can] or 0x110 in fingerprint[cam_can] - hda2 = hda2 or params.get_bool("CanfdHDA2") + hda2 = hda2 or params.get_int("CanfdHDA2") > 0 CAN = CanBus(None, fingerprint, hda2) if params.get_int("CanfdDebug") == -1: @@ -187,7 +187,7 @@ class CarInterface(CarInterfaceBase): # *** feature detection *** if ret.flags & HyundaiFlags.CANFD: #if candidate in (CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.HYUNDAI_IONIQ_5_N, CAR.KIA_EV9) and hda2: ##카니발4th & hda2 인경우에만 BSM이 ADAS에서 나옴. - if 0x161 in fingerprint[CAN.ECAN] and hda2: + if (0x161 in fingerprint[CAN.ECAN] and hda2) or params.get_int("CanfdHDA2") == 2: # EV6일부모델은 BSM이 ADAS에서 나옴. ret.extFlags |= HyundaiExtFlags.BSM_IN_ADAS.value print(f"$$$$$ CanFD ECAN = {CAN.ECAN}") if 0x1fa in fingerprint[CAN.ECAN]: diff --git a/scripts/add/events_ko.py b/scripts/add/events_ko.py index fc7e72d..ceb5f4d 100644 --- a/scripts/add/events_ko.py +++ b/scripts/add/events_ko.py @@ -355,6 +355,16 @@ def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging personality = str(personality).title() return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) +def car_parser_result(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + results = Params().get("CanParserResult") + if results is None: + results = "" + return Alert( + "CAN 오류: Check Connections", + results, + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.) + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { @@ -898,22 +908,24 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # - CAN data is received, but some message are not received at the right frequency # If you're not writing a new car port, this is usually cause by faulty wiring EventName.canError: { + ET.PERMANENT: car_parser_result, ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN 오류 : 장치를 점검하세요"), - ET.PERMANENT: Alert( - "CAN 오류 : 장치를 점검하세요", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + #ET.PERMANENT: Alert( + # "CAN 오류 : 장치를 점검하세요", + # "", + # AlertStatus.normal, AlertSize.small, + # Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), ET.NO_ENTRY: NoEntryAlert("CAN 오류 : 장치를 점검하세요"), }, EventName.canBusMissing: { + ET.PERMANENT: car_parser_result, ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus 연결 끈김"), - ET.PERMANENT: Alert( - "CAN Bus 오류 : 케이블을 점검하세요", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + #ET.PERMANENT: Alert( + # "CAN Bus 오류 : 케이블을 점검하세요", + # "", + # AlertStatus.normal, AlertSize.small, + # Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), ET.NO_ENTRY: NoEntryAlert("CAN Bus 오류 : 장치를 점검하세요"), }, diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index f98b096..bf72af4 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -1150,12 +1150,12 @@ "group": "시작", "name": "CanfdHDA2", "title": "CANFD HDA2지원(0)", - "descr": "HDA2 강제인식, 일부CANFD차량 오류발생시 켜야함.", + "descr": "1: HDA2 강제인식, 2: HDA2+BSM", "egroup": "START", "etitle": "CANFD HDA2(0)", "edescr": "", "min": 0, - "max": 1, + "max": 2, "default": 0, "unit": 1 }, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index ac8f321..d301e60 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -321,8 +321,8 @@ class LongitudinalMpc: v_ego = self.x0[1] if lead is not None and lead.status: x_lead = lead.dRel - v_lead = lead.vLead - a_lead = lead.aLead + v_lead = lead.vLeadK + a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau else: # Fake a fast lead car, so mpc can keep running in the same mode diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 5871aff..dd91216 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -408,6 +408,7 @@ class RadarD: self.kalman_params = KalmanParams(DT_MDL) self.v_ego = 0.0 + print("###RadarD.. : delay = ", delay, int(round(delay / DT_MDL))+1) self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) self.last_v_ego_frame = -1 @@ -423,6 +424,7 @@ class RadarD: self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) + leads_v3 = sm['modelV2'].leadsV3 if sm.recv_frame['carState'] != self.last_v_ego_frame: self.v_ego = sm['carState'].vEgo diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index f044d0f..d1bd6e1 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -355,7 +355,16 @@ def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging personality = str(personality).title() return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) - +def car_parser_result(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + results = Params().get("CanParserResult") + if results is None: + results = "" + return Alert( + "CAN Error: Check Connections!!", + results, + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.) + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -891,22 +900,24 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # - CAN data is received, but some message are not received at the right frequency # If you're not writing a new car port, this is usually cause by faulty wiring EventName.canError: { + ET.PERMANENT: car_parser_result, ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error"), - ET.PERMANENT: Alert( - "CAN Error: Check Connections", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + #ET.PERMANENT: Alert( + # "CAN Error: Check Connections", + # "", + # AlertStatus.normal, AlertSize.small, + # Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), ET.NO_ENTRY: NoEntryAlert("CAN Error: Check Connections"), }, EventName.canBusMissing: { + ET.PERMANENT: car_parser_result, ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"), - ET.PERMANENT: Alert( - "CAN Bus Disconnected: Likely Faulty Cable", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + #ET.PERMANENT: Alert( + # "CAN Bus Disconnected: Likely Faulty Cable", + # "", + # AlertStatus.normal, AlertSize.small, + # Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"), }, diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 3a79d84..73c07bf 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -775,7 +775,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { startToggles->addItem(selectCarBtn); startToggles->addItem(new ParamControl("HyundaiCameraSCC", "HYUNDAI: CAMERA SCC", "Connect the SCC's CAN line to CAM", "../assets/offroad/icon_shell.png", this)); startToggles->addItem(new ParamControl("EnableRadarTracks", "Enable RadarTrack", "", "../assets/offroad/icon_shell.png", this)); - startToggles->addItem(new ParamControl("CanfdHDA2", "CANFD: HDA2 mode", "", "../assets/offroad/icon_shell.png", this)); + startToggles->addItem(new CValueControl("CanfdHDA2", "CANFD: HDA2 mode", "1:HDA2,2:HDA2+BSM", "../assets/offroad/icon_shell.png", 0, 2, 1)); startToggles->addItem(new CValueControl("AutoCruiseControl", "Auto Cruise control", "Softhold, Auto Cruise ON/OFF control", "../assets/offroad/icon_road.png", 0, 3, 1)); startToggles->addItem(new CValueControl("CruiseOnDist", "CRUISE: Auto ON distance(0cm)", "When GAS/Brake is OFF, Cruise ON when the lead car gets closer.", "../assets/offroad/icon_road.png", 0, 2500, 50)); startToggles->addItem(new CValueControl("AutoEngage", "Auto Engage control on start", "1:SteerEnable, 2:Steer/Cruise Engage", "../assets/offroad/icon_road.png", 0, 2, 1));