From 36f281ac9fe482c4de1cffc95be068e7a51768a9 Mon Sep 17 00:00:00 2001 From: carrot <43668841+ajouatom@users.noreply.github.com> Date: Thu, 8 May 2025 19:16:10 +0900 Subject: [PATCH] fix Lanemode. (#162) * test socketmaster... * remove LongVelocityControl * fix.. doorOpen, seatBelt... * fix.. steerTemp alert * torqued liveDelay * sync cruise_state, fix.. angle_steer * tr7 * Revert "tr7" This reverts commit ebbabdbfc8917efa0bae12a150c6841965d59675. * fix.... * fix.. * fix.. * Hyundai: Sonata 2024 Car Port (#161) * fix.. * fix... angle curve scale * fix.. sonata * add.. j_lead clip * fix.. jlead clip * fix.. angle torque... * fix.. mdps.. * fix.. * fix lanemode * fix.. * fix.. lanemode * ff * fix.. * fix.. * fix.. --------- Co-authored-by: Kirito3481 <47619491+Kirito3481@users.noreply.github.com> --- opendbc_repo/opendbc/car/fingerprints.py | 1 + opendbc_repo/opendbc/car/hyundai/carcontroller.py | 9 +++++---- opendbc_repo/opendbc/car/hyundai/fingerprints.py | 9 +++++++++ opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py | 13 +++---------- opendbc_repo/opendbc/car/hyundai/values.py | 11 ++++++++--- .../opendbc/car/torque_data/substitute.toml | 1 + opendbc_repo/opendbc/dbc/hyundai_canfd.dbc | 4 ++++ scripts/add/events_ko.py | 2 +- selfdrive/carrot/carrot_functions.py | 2 +- selfdrive/carrot_settings.json | 8 ++++---- selfdrive/controls/controlsd.py | 6 +++--- selfdrive/locationd/torqued.py | 6 ++++-- selfdrive/ui/qt/offroad/settings.cc | 2 +- system/updated/updated.py | 10 +++++++++- 14 files changed, 54 insertions(+), 30 deletions(-) diff --git a/opendbc_repo/opendbc/car/fingerprints.py b/opendbc_repo/opendbc/car/fingerprints.py index f3b7bf8..7517537 100644 --- a/opendbc_repo/opendbc/car/fingerprints.py +++ b/opendbc_repo/opendbc/car/fingerprints.py @@ -209,6 +209,7 @@ MIGRATION = { "HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE, "HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER, "HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID, + "HYUNDAI SONATA 2024": HYUNDAI.HYUNDAI_SONATA_2024, "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, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index a8d0f5e..4fcac18 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -94,6 +94,7 @@ class CarController(CarControllerBase): self.LFA_trigger = 0 self.activeCarrot = 0 + self.camera_scc_params = Params().get_int("HyundaiCameraSCC") def update(self, CC, CS, now_nanos): @@ -120,7 +121,7 @@ class CarController(CarControllerBase): self.speed_from_pcm = params.get_int("SpeedFromPCM") self.canfd_debug = params.get_int("CanfdDebug") - + self.camera_scc_params = params.get_int("HyundaiCameraSCC") actuators = CC.actuators hud_control = CC.hudControl @@ -151,7 +152,7 @@ class CarController(CarControllerBase): else: curv = abs(actuators.curvature) y_std = actuators.yStd - curvature_threshold = np.interp(y_std, [0.0, 0.25], [0.5, 0.006]) + curvature_threshold = np.interp(y_std, [0.0, 0.2], [0.5, 0.006]) curve_scale = np.clip(curv / curvature_threshold, 0.0, 1.0) torque_pts = [ @@ -159,7 +160,7 @@ class CarController(CarControllerBase): (1 - curve_scale) * self.angle_max_torque + curve_scale * 50, self.angle_max_torque ] - base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], torque_pts) + base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 30, 60], torque_pts) target_torque = np.interp(abs(actuators.curvature), [0.0, 0.003, 0.006], [0.5 * base_max_torque, 0.75 * base_max_torque, base_max_torque]) @@ -264,7 +265,7 @@ class CarController(CarControllerBase): self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control) if True: #not camera_scc: - if camera_scc: + if self.camera_scc_params == 2: self.canfd_toggle_adas(CC, CS) can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.canfd_debug, self.MainMode_ACC_trigger, self.LFA_trigger)) if hda2: diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 4d8edab..db63da6 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -225,6 +225,15 @@ FW_VERSIONS = { b'\xf1\x00LFF LKAS AT USA LHD 1.01 1.02 95740-C1000 E52', ], }, + CAR.HYUNDAI_SONATA_2024: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DN8_ RDR ----- 1.00 1.00 99110-L1800 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.01 99211-L1800 230512', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L1800 230512', + ], + }, CAR.HYUNDAI_TUCSON: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TL__ FCA F-CUP 1.00 1.01 99110-D3500 ', diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index be3fab0..a1ab4f0 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -97,7 +97,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active, values["LKA_ACTIVE"] = 1 if CS.lfa_info["STEER_REQ"] == 1 else 0 if frame % 1000 < 40: - values["STEERING_COL_TORQUE"] += 100 + values["STEERING_COL_TORQUE"] += 120 ret.append(packer.make_can_msg("MDPS", CAN.CAM, values)) if frame % 10 == 0: @@ -422,20 +422,13 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["DATA102"] = 0 # steer_temp관련없음 ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) - """ if CS.cruise_buttons_msg is not None: values = CS.cruise_buttons_msg if MainMode_ACC_trigger > 0: - #values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1 - pass + values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1 elif LFA_trigger > 0: - pass - #values["LFA_BTN"] = 1 - #values["COUNTER"] = (values["COUNTER"] + 1) % 256 - #ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.ECAN, values)) - #ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.ECAN, values)) + values["LFA_BTN"] = 1 ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values)) - """ if frame % 5 == 0: diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 9d9cd50..705b131 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -26,7 +26,7 @@ class CarControllerParams: #([0, 9, 16, 25], [2.0, 1.2, 0.8, 0.28]), # sunny's #([0, 5, 25], [0.6, 0.4, 0.15]), - #([0, 5, 25], [1.0, 0.5, 0.26]), + #([0, 5, 25], [1.0, 0.5, 0.26]), ) # Stock LFA system is seen sending 250 max, but for LKAS events it's 175 max. # 250 can at least achieve 4 m/s^2, 80 corresponds to ~2.5 m/s^2 @@ -34,7 +34,7 @@ class CarControllerParams: ANGLE_MIN_TORQUE = 25 # equivalent to ~0.8 m/s^2 of torque (based on ANGLE_MAX_TORQUE) when overriding ANGLE_TORQUE_UP_RATE = 8 #2 # Indicates how fast the torque ramps up after user intervention. ANGLE_TORQUE_DOWN_RATE = 12 #4 Indicates how fast the torque ramps down during user intervention (handing off). - + def __init__(self, CP): self.STEER_DELTA_UP = 3 self.STEER_DELTA_DOWN = 7 @@ -197,7 +197,7 @@ class CAR(Platforms): HYUNDAI_AZERA_7TH_GEN = HyundaiCanFDPlatformConfig( [HyundaiCarDocs("Hyundai Azera 2023-2024", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], CarSpecs(mass=1700, wheelbase=2.895, steerRatio=16.5), - ) + ) HYUNDAI_AZERA_6TH_GEN = HyundaiPlatformConfig( [HyundaiCarDocs("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], CarSpecs(mass=1600, wheelbase=2.885, steerRatio=14.5), @@ -348,6 +348,11 @@ class CAR(Platforms): flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, ) + HYUNDAI_SONATA_2024 = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Hyundai Sonata 2024-25", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=1556, wheelbase=2.84, steerRatio=12.81), + flags=HyundaiFlags.CAMERA_SCC, + ) HYUNDAI_STARIA_4TH_GEN = HyundaiCanFDPlatformConfig( [HyundaiCarDocs("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], CarSpecs(mass=2205, wheelbase=3.273, steerRatio=11.94), # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index ddeda5b..ad4dddf 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -45,6 +45,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "GENESIS_G90" = "GENESIS_G70" "GENESIS_G80" = "GENESIS_G70" "GENESIS_G70_2020" = "HYUNDAI_SONATA" +"HYUNDAI_SONATA_2024" = "HYUNDAI_SONATA" "HONDA_FREED" = "HONDA_ODYSSEY" "HONDA_CRV_EU" = "HONDA_CRV" diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc index 622c129..3f31864 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc @@ -137,6 +137,9 @@ BO_ 234 MDPS: 24 XXX SG_ STEERING_ANGLE_2 : 128|16@1- (-0.1,0) [0|65535] "deg" XXX SG_ LFA2_ACTIVE : 145|2@0+ (1,0) [0|3] "" XXX SG_ LFA2_FAULT : 149|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 159|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_5 : 167|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_6 : 175|8@0+ (1,0) [0|255] "" XXX BO_ 256 ACCELERATOR_BRAKE_ALT: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -480,6 +483,7 @@ BO_ 463 CRUISE_BUTTONS: 8 XXX SG_ CRUISE_BUTTONS : 16|3@1+ (1,0) [0|3] "" XXX SG_ RIGHT_PADDLE : 25|1@1+ (1,0) [0|1] "" XXX SG_ LEFT_PADDLE : 27|1@1+ (1,0) [0|1] "" XXX + SG_ SET_ME_1_ : 40|1@0+ (1,0) [0|1] "" XXX BO_ 474 ADRV_0x1da: 32 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/scripts/add/events_ko.py b/scripts/add/events_ko.py index d7d8468..88c7afd 100644 --- a/scripts/add/events_ko.py +++ b/scripts/add/events_ko.py @@ -314,7 +314,7 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: text = "크루즈 버튼으로 활성화됩니다" - if CP.carName == "honda": + if CP.brand == "honda": text = "메인 스위치로 활성화됩니다" return NoEntryAlert(text) diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index 5df473e..a699be1 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -212,7 +212,7 @@ class CarrotPlanner: self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC # 차선변경시 jerk factor를 줄여 aggresive하게 elif lead.status: if self.dynamicTFollow > 0.0: - gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) + gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1 t_follow += gap_dist_adjust if gap_dist_adjust < 0: self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게. diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 3c76d9b..2329bde 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -1098,12 +1098,12 @@ "group": "시작", "name": "HyundaiCameraSCC", "title": "HYUNDAI: CAMERA SCC", - "descr": "SCC배선 개조시 1로 설정", + "descr": "SCC가 카메라쪽에 있거나, 배선 개조시 1,2로 설정, 2:Sync cruise state(canfd)", "egroup": "START", "etitle": "HYUNDAI: CAMERA SCC(0)", - "edescr": "", + "edescr": "SCC's CAN line connected to CAM, 2:Sync ruise state(canfd)", "min": 0, - "max": 1, + "max": 2, "default": 0, "unit": 1 }, @@ -1248,7 +1248,7 @@ "min": 0, "max": 100, "default": 0, - "unit": 5 + "unit": 1 }, { "group": "차량간격", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 350fea7..6a4a493 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -140,7 +140,7 @@ class Controls: if len(model_v2.position.yStd) > 0: yStd = np.interp(steer_actuator_delay + lat_smooth_seconds, ModelConstants.T_IDXS, model_v2.position.yStd) - self.yStd = yStd * 0.1 + self.yStd * 0.9 + self.yStd = yStd * 0.02 + self.yStd * 0.98 else: self.yStd = 0.0 @@ -154,8 +154,8 @@ class Controls: alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1 return alpha * val + (1 - alpha) * prev_val - t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL - curvature = np.interp(steer_actuator_delay + lat_smooth_seconds + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures) + curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds) + new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds) else: new_desired_curvature = model_v2.action.desiredCurvature diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 0d45a25..6f356ea 100644 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -52,7 +52,7 @@ class TorqueBuckets(PointBuckets): class TorqueEstimator(ParameterEstimator): def __init__(self, CP, decimated=False, track_all_points=False): self.hist_len = int(HISTORY / DT_MDL) - self.lag = Params().get_float("SteerActuatorDelay") * 0.01 #CP.steerActuatorDelay + .2 # from controlsd + self.lag = 0.0 self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters if decimated: self.min_bucket_points = MIN_BUCKET_POINTS / 10 @@ -171,6 +171,8 @@ class TorqueEstimator(ParameterEstimator): # TODO: check if high aEgo affects resulting lateral accel self.raw_points["vego"].append(msg.vEgo) self.raw_points["steer_override"].append(msg.steeringPressed) + elif which == "liveDelay": + self.lag = msg.lateralDelay elif which == "liveLocationKalman": if len(self.raw_points['steer_torque']) == self.hist_len: yaw_rate = msg.angularVelocityCalibrated.value[2] @@ -231,7 +233,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveDelay', 'liveLocationKalman'], poll='liveLocationKalman') params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index dd2ed2f..42f54c8 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -785,7 +785,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 CValueControl("HyundaiCameraSCC", "HYUNDAI: CAMERA SCC", "1:Connect the SCC's CAN line to CAM, 2:Sync Cruise state", "../assets/offroad/icon_shell.png", 0, 2, 1)); startToggles->addItem(new CValueControl("EnableRadarTracks", "Enable Radar Track", "1:Enable RadarTrack, -1,2:Disable use HKG SCC radar at all times", "../assets/offroad/icon_shell.png", -1, 2, 1)); 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)); diff --git a/system/updated/updated.py b/system/updated/updated.py index 0759c0a..8e82da4 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -9,6 +9,7 @@ import signal import fcntl import time import threading +import gc from collections import defaultdict from pathlib import Path @@ -71,9 +72,13 @@ def read_time_from_param(params, param) -> datetime.datetime | None: pass return None -def run(cmd: list[str], cwd: str = None) -> str: +def run_org(cmd: list[str], cwd: str = None) -> str: return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') +def run(cmd: list[str], cwd: str = None) -> str: + proc = subprocess.run(cmd, cwd=cwd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, encoding='utf8', check=True) + return proc.stdout + def set_consistent_flag(consistent: bool) -> None: os.sync() @@ -492,6 +497,9 @@ def main() -> None: cloudlog.exception("uncaught updated exception, shouldn't happen") exception = str(e) OVERLAY_INIT.unlink(missing_ok=True) + finally: + gc.collect() + try: params.put("UpdaterState", "idle")