From 08a11c445c3dd0d92c64333a1bea4f7b40b12411 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 10 Sep 2026 17:39:45 -0500 Subject: [PATCH] the bell --- opendbc_repo/opendbc/car/hyundai/interface.py | 3 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 7 +++- selfdrive/controls/lib/latcontrol_torque.py | 4 ++ .../controls/lib/latcontrol_vehicle_tunes.py | 37 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 33 +++++++++++++++++ 5 files changed, 81 insertions(+), 3 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 4e8a13b40b..470bb339e8 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -7,7 +7,6 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \ CANFD_SECURITYACCESS_CAR, \ CANFD_ANGLE_LONGITUDINAL_CAR, \ - CANFD_RADAR_LIVE_LONGITUDINAL_CAR, \ CANFD_RADAR_ECU_KEEPALIVE_CAR, \ RADAR_LIVE_LONGITUDINAL_CAR, \ UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \ @@ -30,7 +29,7 @@ Ecu = structs.CarParams.Ecu def get_communication_control_request(car_fingerprint): - if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR: + if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR and car_fingerprint != CAR.HYUNDAI_IONIQ_5: return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL]) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 69dec2e980..aa075c6546 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -130,13 +130,18 @@ def get_test_toggles() -> SimpleNamespace: class TestHyundaiFingerprint: - def test_ev6_uses_stock_hda2_communication_control_path(self): + def test_egmp_communication_control_paths(self): stock_request = bytes([0x28, 0x83, 0x01]) radar_keepalive_request = bytes([0x28, 0x01, 0x01]) assert CAR.KIA_EV6 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR assert CAR.KIA_EV6 not in CANFD_RADAR_ECU_KEEPALIVE_CAR assert get_communication_control_request(CAR.KIA_EV6) == stock_request + + assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR + assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_ECU_KEEPALIVE_CAR + assert get_communication_control_request(CAR.HYUNDAI_IONIQ_5) == stock_request + assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request def test_carnival_hev_low_speed_torque_rate_limits(self): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 9094e12ae1..8a98b7b7d9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -652,6 +652,10 @@ class LatControlTorque(LatControl): output_torque *= get_genesis_g70_angle_output_scale(CS.steeringAngleDeg, output_torque) low_speed_output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo) output_torque = float(np.clip(output_torque, -low_speed_output_limit, low_speed_output_limit)) + if not CS.steeringPressed: + output_torque = get_genesis_g70_stabilized_output( + output_torque, self.prev_output_torque, setpoint, desired_lateral_jerk, CS.vEgo, self.dt, + ) elif self.is_genesis_gv70: output_torque *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo) output_torque *= get_genesis_gv70_low_speed_center_overshoot_scale( diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index e7ebd21539..8029dfe2af 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -335,6 +335,17 @@ GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10 +GENESIS_G70_OUTPUT_SMOOTHING_SPEED = 40.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT = 0.42 +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.14 +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.30 +GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10 +GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC = 0.10 +GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 +GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 +GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.45 +GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.055 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 @@ -3380,6 +3391,32 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc return 1.0 - reduction +def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float, + desired_lateral_accel: float, desired_lateral_jerk: float, + v_ego: float, dt: float) -> float: + speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_SPEED) / + GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH) + curve_weight = 1.0 - center_weight + response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight + + GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC * center_weight) + + unwind_phase = -desired_lateral_accel * desired_lateral_jerk + unwind_weight = _sigmoid((unwind_phase - GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE) / + GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH) + response_time += GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight + + changing_direction = (abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and + prev_output_torque * desired_lateral_accel <= 0.0) + if changing_direction: + response_time = min(response_time, GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC) + + output_alpha = dt / (max(response_time, 0.0) + dt) + smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque) + return float(output_torque + speed_weight * (smoothed_output - output_torque)) + + def _ioniq_5_sigmoid(x: float) -> float: return _sigmoid(x) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 358757eeb6..9372470fc6 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -55,6 +55,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_subaru_impreza_pid_output_scale, get_genesis_gv70_low_speed_center_overshoot_scale, get_genesis_g70_high_speed_transition_scale, + get_genesis_g70_stabilized_output, normalize_flm_overrides, set_flm_runtime_overrides, ) @@ -1736,6 +1737,38 @@ class TestLatControl: assert lac_log.active assert 0.0 < abs(output) <= get_genesis_g70_low_speed_output_limit(0.0, CS.vEgo) + def test_genesis_g70_output_stabilizer_is_speed_and_phase_aware(self): + low_speed = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 5.0, DT_CTRL) + high_speed_center = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 30.0, DT_CTRL) + high_speed_wind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL) + high_speed_unwind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL) + high_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL) + + assert low_speed == pytest.approx(-0.2, abs=0.005) + assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2) + assert high_speed_unwind > high_speed_wind > 0.1 + assert abs(high_speed_direction_change - 0.3) > abs(high_speed_center - 0.2) + + def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch): + calls = [] + + def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel, + desired_lateral_jerk, v_ego, dt): + calls.append((output_torque, prev_output_torque, desired_lateral_accel, + desired_lateral_jerk, v_ego, dt)) + return 0.123 + + monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_stabilized_output", stabilized_output) + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020) + CS.vEgo = 25.0 + output, _, lac_log = controller.update( + True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles, + ) + + assert calls + assert lac_log.active + assert output == pytest.approx(-0.123) + def test_ioniq_5_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5) CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5]