From 1bc2b23ec12c0ed520c9e6d9e6bcbf74628361a8 Mon Sep 17 00:00:00 2001 From: ajouatom Date: Sun, 3 Aug 2025 19:19:18 +0900 Subject: [PATCH] IONIQ9, fix StockSCC, fix Cpu usage --- opendbc_repo/opendbc/can/parser.py | 18 ++++++---- opendbc_repo/opendbc/car/fingerprints.py | 1 + .../opendbc/car/hyundai/carcontroller.py | 3 +- opendbc_repo/opendbc/car/hyundai/carstate.py | 33 ++++++++++++++----- .../opendbc/car/hyundai/fingerprints.py | 8 +++++ .../opendbc/car/hyundai/hyundaicanfd.py | 14 +++++--- .../opendbc/car/hyundai/radar_interface.py | 2 +- opendbc_repo/opendbc/car/hyundai/values.py | 7 ++++ .../opendbc/car/torque_data/override.toml | 1 + release/build_carrot.sh | 2 +- selfdrive/car/card.py | 3 +- selfdrive/controls/radard.py | 2 +- selfdrive/frogpilot/fleetmanager/helpers.py | 1 + 13 files changed, 69 insertions(+), 26 deletions(-) diff --git a/opendbc_repo/opendbc/can/parser.py b/opendbc_repo/opendbc/can/parser.py index 1a3affc6..1402c80b 100644 --- a/opendbc_repo/opendbc/can/parser.py +++ b/opendbc_repo/opendbc/can/parser.py @@ -1,3 +1,4 @@ +import time import math import numbers from collections import defaultdict, deque @@ -103,8 +104,12 @@ class MessageState: if self.ignore_alive: return True if not self.timestamps: + if self.first_seen_nanos != 0 and (current_nanos - self.first_seen_nanos) < 2e9: # 2초 유예 + return True + #print(f"Not Seen {self.name} on bus {self.address} has no timestamps yet, first seen at {self.first_seen_nanos} ns") return False if (current_nanos - self.timestamps[-1]) > self.timeout_threshold: + #print(f"Timeout {self.name} on bus {self.address} timed out: {current_nanos - self.timestamps[-1]} ns since last update") return False return True @@ -130,8 +135,8 @@ class CANParser: self.ts_nanos: dict[int | str, dict[str, int]] = {} self.addresses: set[int] = set() self.message_states: dict[int, MessageState] = {} - self.frame = 0 self.seen_addresses: set[int] = set() + self.controls_ready = False for name_or_addr, freq in messages: if isinstance(name_or_addr, numbers.Number): @@ -179,6 +184,7 @@ class CANParser: signals=list(msg.sigs.values()), ignore_alive=freq is not None and math.isnan(freq), ) + state.first_seen_nanos = time.monotonic_ns() # 등록시 즉시 타임스탬프 설정 if freq is not None and freq > 0: state.frequency = freq state.timeout_threshold = (1_000_000_000 / freq) * 10 @@ -195,13 +201,12 @@ class CANParser: for state in self.message_states.values(): if state.counter_fail >= MAX_BAD_COUNTER: counters_valid = False - #print("counters_valid=", state.name) if not state.valid(nanos, self.bus_timeout): valid = False self.invalid_time_counter += 1 - if self.invalid_name is None or state.name != self.invalid_name or self.invalid_time_counter > 100: - if self.invalid_print_counter < 200: - print("CAN_INVALID = ", state.name) + if self.controls_ready and self.invalid_name is None or state.name != self.invalid_name or self.invalid_time_counter > 100: + if self.invalid_print_counter < 100: + print(f"CAN_INVALID = {state.name}, bus = {self.bus}") self.invalid_print_counter += 1 self.invalid_name = state.name self.invalid_time_counter = 0 @@ -210,7 +215,6 @@ class CANParser: self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT and counters_valid def update(self, strings, sendcan: bool = False): - self.frame += 1 if strings and not isinstance(strings[0], list | tuple): strings = [strings] @@ -226,7 +230,7 @@ class CANParser: for address, dat, src in frames: if src != self.bus: continue - if self.frame > 800: + if self.controls_ready: self.seen_addresses.add(address) bus_empty = False state = self.message_states.get(address) diff --git a/opendbc_repo/opendbc/car/fingerprints.py b/opendbc_repo/opendbc/car/fingerprints.py index 0ab46ff9..db140d1a 100644 --- a/opendbc_repo/opendbc/car/fingerprints.py +++ b/opendbc_repo/opendbc/car/fingerprints.py @@ -214,6 +214,7 @@ MIGRATION = { "HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5, "HYUNDAI IONIQ 5 PE (NE1)": HYUNDAI.HYUNDAI_IONIQ_5_PE, "HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6, + "HYUNDAI IONIQ 9 2025": HYUNDAI.HYUNDAI_IONIQ_9, "HYUNDAI TUCSON 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, "HYUNDAI SANTA CRUZ 1ST GEN": HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, "HYUNDAI CUSTIN 1ST GEN": HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index aa29310b..77ecf6f5 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -283,8 +283,7 @@ class CarController(CarControllerBase): # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and hda2 and not camera_scc: - can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg, - self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING)) + can_sends.extend(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS)) # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 20544f68..34c26668 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -75,7 +75,8 @@ class CarState(CarStateBase): self.steer_touch_info = {} self.cruise_buttons_msg = None - self.hda2_lfa_block_msg = None + self.msg_0x362 = None + self.msg_0x2a4 = None # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz self.cluster_speed = 0 @@ -133,9 +134,25 @@ class CarState(CarStateBase): self.cp_bsm = None + self.controls_ready_count = 0 + def update(self, can_parsers) -> structs.CarState: + + if self.controls_ready_count <= 200: + if Params().get_bool("ControlsReady"): + self.controls_ready_count += 1 cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] + cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None + if self.controls_ready_count == 50: + cp.controls_ready = cp_cam.controls_ready = True + if cp_alt is not None: + cp_alt.controls_ready = True + elif self.controls_ready_count == 100: + print("cp_cam.seen_addresses =", cp_cam.seen_addresses) + print("cp.seen_addresses =", cp.seen_addresses) + if cp_alt is not None: + print("cp_alt.seen_addresses =", cp_alt.seen_addresses) if self.CP.flags & HyundaiFlags.CANFD: return self.update_canfd(can_parsers) @@ -419,13 +436,9 @@ class CarState(CarStateBase): if 442 in cp.seen_addresses: self.cp_bsm = cp print("######## BSM in ECAN") - print("######## BSM in CAM, cp_cam.seen_addresses =", cp_cam.seen_addresses) - print("######## BSM in CAM, cp.seen_addresses =", cp.seen_addresses) elif 442 in cp_cam.seen_addresses: self.cp_bsm = cp_cam print("######## BSM in CAM") - print("######## BSM in CAM, cp_cam.seen_addresses =", cp_cam.seen_addresses) - print("######## BSM in CAM, cp.seen_addresses =", cp.seen_addresses) else: bsm_info = self.cp_bsm.vl["BLINDSPOTS_REAR_CORNERS"] ret.leftBlindspot = (bsm_info["FL_INDICATOR"] + bsm_info["INDICATOR_LEFT_TWO"] + bsm_info["INDICATOR_LEFT_FOUR"]) > 0 @@ -501,7 +514,7 @@ class CarState(CarStateBase): ret.gearStep = cp.vl["GEAR"]["GEAR_STEP"] if self.GEAR else 0 ret.gearStep = cp.vl["GEAR_ALT"]["GEAR_STEP"] if self.GEAR_ALT else ret.gearStep - if cp_alt: + if cp_alt and self.CP.flags & HyundaiFlags.CAMERA_SCC: lane_info = None lane_info = cp_alt.vl["CAM_0x362"] if self.CAM_0x362 else None lane_info = cp_alt.vl["CAM_0x2a4"] if self.CAM_0x2a4 else lane_info @@ -557,9 +570,11 @@ class CarState(CarStateBase): self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED - if self.CP.flags & HyundaiFlags.CANFD_HDA2 and not (self.CP.flags & HyundaiFlags.CAMERA_SCC): - self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING - else cp_cam.vl["CAM_0x2a4"]) + if not (self.CP.flags & HyundaiFlags.CAMERA_SCC): + if self.msg_0x362 is not None or 0x362 in cp_cam.seen_addresses: + self.msg_0x362 = cp_cam.vl["CAM_0x362"] + elif self.msg_0x2a4 is not None or 0x2a4 in cp_cam.seen_addresses: + self.msg_0x2a4 = cp_cam.vl["CAM_0x2a4"] speed_conv = CV.KPH_TO_MS # if self.is_metric else CV.MPH_TO_MS cluSpeed = cp.vl["CRUISE_BUTTONS_ALT"]["CLU_SPEED"] diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 5401150e..14bc3b31 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -1099,6 +1099,14 @@ FW_VERSIONS = { b'\xf1\x00CE MFC AT USA LHD 1.00 1.06 99211-KL000 230915', ], }, + CAR.HYUNDAI_IONIQ_9: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00MEev RDR ----- 1.00 1.00 99110-GO000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00ME MFC AT KOR LHD 1.00 1.00 99211-GO000 241007', + ], + }, CAR.HYUNDAI_TUCSON_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.00 99211-N9260 14Y', diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index d274a085..c106f5f8 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -207,9 +207,15 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, return ret -def create_suppress_lfa(packer, CAN, lfa_block_msg, lka_steering_alt): - suppress_msg = "CAM_0x362" if lka_steering_alt else "CAM_0x2a4" - msg_bytes = 32 if lka_steering_alt else 24 +def create_suppress_lfa(packer, CAN, CS): + if CS.msg_0x362 is not None: + suppress_msg = "CAM_0x362" + lfa_block_msg = CS.msg_0x362 + elif CS.msg_0x2a4 is not None: + suppress_msg = "CAM_0x2a4" + lfa_block_msg = CS.msg_0x2a4 + else: + return [] #values = {f"BYTE{i}": lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7} values = copy.copy(lfa_block_msg) @@ -218,7 +224,7 @@ def create_suppress_lfa(packer, CAN, lfa_block_msg, lka_steering_alt): values["SET_ME_0_2"] = 0 values["LEFT_LANE_LINE"] = 0 values["RIGHT_LANE_LINE"] = 0 - return packer.make_can_msg(suppress_msg, CAN.ACAN, values) + return [packer.make_can_msg(suppress_msg, CAN.ACAN, values)] def create_buttons(packer, CP, CAN, cnt, btn): values = { diff --git a/opendbc_repo/opendbc/car/hyundai/radar_interface.py b/opendbc_repo/opendbc/car/hyundai/radar_interface.py index 548cc38a..6dd21023 100644 --- a/opendbc_repo/opendbc/car/hyundai/radar_interface.py +++ b/opendbc_repo/opendbc/car/hyundai/radar_interface.py @@ -37,7 +37,7 @@ def get_radar_can_parser_scc(CP): CAN = CanBus(CP) if CP.flags & HyundaiFlags.CANFD: messages = [("SCC_CONTROL", 50)] - bus = CAN.ACAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.ECAN + bus = CAN.ECAN else: messages = [("SCC11", 50)] bus = CAN.ECAN diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 5ae45e47..04cb48dd 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -406,6 +406,13 @@ class CAR(Platforms): HYUNDAI_IONIQ_5.specs, flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, ) + HYUNDAI_IONIQ_9 = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Hyundai Ioniq 9", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), + ], + CarSpecs(mass=2505, wheelbase=3.13, steerRatio=16.02), + flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL, + ) HYUNDAI_TUCSON_4TH_GEN = HyundaiCanFDPlatformConfig( [ HyundaiCarDocs("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 5bccd825..33868bca 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -69,6 +69,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HYUNDAI_KONA_EV_2ND_GEN" = [2.5, 2.5, 0.1] "HYUNDAI_KONA_HEV_2ND_GEN" = [2.5, 2.5, 0.1] "HYUNDAI_IONIQ_6" = [2.5, 2.5, 0.005] +"HYUNDAI_IONIQ_9" = [1.75, 1.75, 0.15] "HYUNDAI_AZERA_7TH_GEN" = [1.8, 1.8, 0.1] "HYUNDAI_AZERA_6TH_GEN" = [1.8, 1.8, 0.1] "HYUNDAI_AZERA_HEV_6TH_GEN" = [1.8, 1.8, 0.1] diff --git a/release/build_carrot.sh b/release/build_carrot.sh index 9cc589a2..2b8155dd 100755 --- a/release/build_carrot.sh +++ b/release/build_carrot.sh @@ -14,7 +14,7 @@ find . -name '*.pyc' -delete find . -name 'moc_*' -delete find . -name '__pycache__' -delete rm -rf .sconsign.dblite Jenkinsfile release/ -rm selfdrive/modeld/models/supercombo.onnx +rm selfdrive/modeld/models/*.onnx touch prebuilt # Add built files to git diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 74051b1c..8c3ece0e 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -310,7 +310,8 @@ class Car: t.join() def main(): - config_realtime_process(4, Priority.CTRL_HIGH) + #config_realtime_process(4, Priority.CTRL_HIGH) + config_realtime_process(6, Priority.CTRL_HIGH) car = Car() car.card_thread() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8733c289..ac410218 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -100,7 +100,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA #vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0 max_offset_vision_dist = max(offset_vision_dist * 0.35, 5.0) - max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.7]), 5.0) # 확률이 낮으면 속도오차를 줄임. + max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # 확률이 낮으면 속도오차를 줄임. def prob(c): #if abs(c.dRel - offset_vision_dist) > max_offset_vision_dist: diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index fdd3bb47..26c57939 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -164,6 +164,7 @@ def ffmpeg_mp4_concat_wrap_process_builder(file_list, cameratype, chunk_size=102 command_line += ["-r", "20"] command_line += ["-i", "concat:" + file_list] command_line += ["-c", "copy"] + command_line += ["-bsf:a", "aac_adtstoasc"] command_line += ["-map", "0"] if not cameratype == "qcamera": command_line += ["-vtag", "hvc1"]