diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index a02a39e03..84520892c 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -26,6 +26,12 @@ HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0 HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 IONIQ_6_CANFD_SCC_ACCEL_STEP = 6.0 / 50.0 IONIQ_6_CANFD_SCC_DECEL_STEP = 15.0 / 50.0 +GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5] +GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.30, -0.55] +GENESIS_G90_RELEASE_SPEED_BP = [0.0, 0.3, 0.6] +GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.24, 0.20, 0.14] +GENESIS_G90_RELEASE_DECEL_STEP_V = [0.30, 0.24, 0.18] +GENESIS_G90_RELEASE_MAX_SPEED = 0.8 IONIQ_6_LONG_MIN_JERK = 0.5 IONIQ_6_LONG_JERK_LIMIT = 4.8 IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0] @@ -51,6 +57,13 @@ class Ioniq6LongitudinalTuningState: long_control_state_last: LongCtrlState = LongCtrlState.off +@dataclass +class GenesisG90LongitudinalTuningState: + actual_accel: float = 0.0 + release_active: bool = False + long_control_state_last: LongCtrlState = LongCtrlState.off + + def _jerk_limited_integrator(desired_accel: float, last_accel: float, jerk_upper: float, jerk_lower: float) -> float: step = (jerk_upper if desired_accel >= last_accel else jerk_lower) * DT_CTRL * 5.0 return float(np.clip(desired_accel, last_accel - step, last_accel + step)) @@ -126,6 +139,36 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc return state +def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningState, accel_cmd: float, v_ego: float, + long_control_state: LongCtrlState, long_active: bool) -> GenesisG90LongitudinalTuningState: + if not long_active: + state.actual_accel = 0.0 + state.release_active = False + state.long_control_state_last = long_control_state + return state + + stopping = long_control_state == LongCtrlState.stopping + if stopping and v_ego <= GENESIS_G90_STOP_HOLD_SPEED_BP[-1]: + state.release_active = False + state.actual_accel = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V)) + else: + if state.long_control_state_last == LongCtrlState.stopping and long_control_state == LongCtrlState.pid and \ + accel_cmd > 0.0 and v_ego < GENESIS_G90_RELEASE_MAX_SPEED: + state.release_active = True + + if state.release_active: + accel_step = float(np.interp(v_ego, GENESIS_G90_RELEASE_SPEED_BP, GENESIS_G90_RELEASE_ACCEL_STEP_V)) + decel_step = float(np.interp(v_ego, GENESIS_G90_RELEASE_SPEED_BP, GENESIS_G90_RELEASE_DECEL_STEP_V)) + state.actual_accel = float(np.clip(accel_cmd, state.actual_accel - decel_step, state.actual_accel + accel_step)) + if v_ego >= GENESIS_G90_RELEASE_MAX_SPEED or accel_cmd <= 0.0 or state.actual_accel >= accel_cmd - 1e-3: + state.release_active = False + else: + state.actual_accel = accel_cmd + + state.long_control_state_last = long_control_state + return state + + def process_hud_alert(enabled, fingerprint, hud_control): sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) @@ -171,6 +214,7 @@ class CarController(CarControllerBase): self._ioniq_6_lane_change_ui_side = None self._ioniq_6_lane_change_ui_trigger_frames = 0 self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState() + self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState() def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators @@ -260,6 +304,12 @@ class CarController(CarControllerBase): self._ioniq_6_long_tuning.stopping = stopping self._ioniq_6_long_tuning.long_control_state_last = actuators.longControlState + if self.CP.carFingerprint == CAR.GENESIS_G90 and self.long_active_ecu: + self._genesis_g90_long_tuning = update_genesis_g90_longitudinal_tuning(self._genesis_g90_long_tuning, accel_cmd, + CS.out.vEgo, actuators.longControlState, + self.long_active_ecu) + accel = self._genesis_g90_long_tuning.actual_accel + # *** common hyundai stuff *** # tester present - w/ no response (keeps relevant ECU disabled) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index b35a60b2c..0b9dfd0d5 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -7,7 +7,8 @@ from opendbc.can import CANPacker, CANParser from opendbc.car import Bus, ButtonType, gen_empty_fingerprint from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car -from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, update_ioniq_6_longitudinal_tuning +from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \ + update_ioniq_6_longitudinal_tuning, update_genesis_g90_longitudinal_tuning from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai import hyundaican, hyundaicanfd @@ -291,6 +292,26 @@ class TestHyundaiFingerprint: assert state.desired_accel > 0.3 assert state.actual_accel > 0.24 + def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self): + state = GenesisG90LongitudinalTuningState() + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.02, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.actual_accel == pytest.approx(-0.12) + assert not state.release_active + + def test_genesis_g90_longitudinal_tuning_ramps_out_of_stop_hold(self): + state = GenesisG90LongitudinalTuningState(actual_accel=-0.12, long_control_state_last=LongCtrlState.stopping) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=0.5, v_ego=0.02, + long_control_state=LongCtrlState.pid, long_active=True) + assert state.release_active + assert state.actual_accel == pytest.approx(0.12) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=0.5, v_ego=0.2, + long_control_state=LongCtrlState.pid, long_active=True) + assert state.actual_accel > 0.12 + def test_canfd_acc_control_uses_direct_accel(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV6