diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 4045655208..49f5142a55 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -4,7 +4,7 @@ from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.ford import fordcan -from opendbc.car.ford.values import CarControllerParams, FordFlags +from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX # This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md # (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md. @@ -64,7 +64,9 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c return apply_curvature -def apply_creep_compensation(accel: float, v_ego: float) -> float: +def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float: + if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping): + return accel creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.]) creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.]) accel -= creep_accel @@ -181,12 +183,11 @@ class CarController(CarControllerBase): if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: accel = actuators.accel gas = accel + stopping = actuators.longControlState == LongCtrlState.stopping if CC.longActive: - # Compensate for engine creep at low speed. - # Either the ABS does not account for engine creep, or the correction is very slow - # TODO: verify this applies to EV/hybrid - accel = apply_creep_compensation(accel, CS.out.vEgo) + accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint, + standstill=CS.out.standstill, stopping=stopping) # The stock system has been seen rate limiting the brake accel to 5 m/s^3, # however even 3.5 m/s^3 causes some overshoot with a step response. @@ -210,7 +211,6 @@ class CarController(CarControllerBase): elif accel_pitch_compensated < 0.0: self.brake_request = True - stopping = CC.actuators.longControlState == LongCtrlState.stopping # TODO: look into using the actuators packet to send the desired speed can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX)) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 3db803bb2b..2396d1218c 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_ford.py @@ -9,7 +9,7 @@ import pytest from opendbc.car import Bus, gen_empty_fingerprint from opendbc.can import CANPacker from opendbc.car.ford import fordcan -from opendbc.car.ford.carcontroller import FordStockCruiseButton +from opendbc.car.ford.carcontroller import FordStockCruiseButton, apply_creep_compensation from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict @@ -38,6 +38,17 @@ def test_stock_cruise_button_ignores_press_with_cruise_master_off(): assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False) +def test_mach_e_does_not_apply_engine_creep_compensation(): + for accel in (-1.0, -0.1, 0.0, 0.1): + assert apply_creep_compensation(accel, 0.5, CAR.FORD_MUSTANG_MACH_E_MK1, + standstill=False, stopping=False) == accel + + assert apply_creep_compensation(0.0, 0.0, CAR.FORD_MUSTANG_MACH_E_MK1, + standstill=True, stopping=True) == -0.6 + assert apply_creep_compensation(0.0, 0.5, CAR.FORD_F_150_MK14, + standstill=False, stopping=False) == -0.6 + + ECU_ADDRESSES = { Ecu.eps: 0x730, # Power Steering Control Module (PSCM) Ecu.abs: 0x760, # Anti-Lock Brake System (ABS) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index ba4ee8fb5a..a0ed46c676 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -770,7 +770,8 @@ class CarController(CarControllerBase): lead_data = self._can_lead_data.update(lead_distance, lead_rel_speed, lead_visible) # HUD messages - sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, + stinger_hud_enabled = CC.enabled or (self.CP.carFingerprint == CAR.KIA_STINGER_2022 and CC.latActive) + sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(stinger_hud_enabled, self.car_fingerprint, hud_control) if blended_hda2: diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index c29a6f0a47..fe3df9c322 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -12,7 +12,7 @@ _adrv_0x51_templates: dict[CAR, bytes] = {} def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None: - if car_fingerprint != CAR.KIA_EV6: + if car_fingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN): return if dat is None: @@ -26,7 +26,6 @@ def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None if template is None: return packer.make_can_msg("ADRV_0x51", CAN.ACAN, {}) - # EV6 MRR30 tracks stop when the ADAS takeover replaces this platform payload with zeros. dat = bytearray(template) dat[2] = (template[2] + frame + 1) & 0xFF dat[3] = (dat[3] & ~0x1) | int(drive_gear) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index a1dc76ba09..a878037250 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -395,7 +395,7 @@ class CarInterface(CarInterfaceBase): if not skip_disable_ecu: disable_can_recv = can_recv - if CP.carFingerprint == CAR.KIA_EV6 and can_recv is not None: + if CP.carFingerprint in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and can_recv is not None: hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None) base_can_recv = can_recv adrv_bus = CanBus(CP).ACAN diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ff05eaddbf..d8046f5544 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1316,6 +1316,103 @@ class TestHyundaiFingerprint: assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable + def test_sportage_hev_hda2_redneck_uses_stock_scc(self, monkeypatch): + class FakeParams: + def __init__(self, *args, **kwargs): + pass + + @staticmethod + def get_bool(key): + return key == "RedneckCruise" + + monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams) + toggles = get_test_toggles() + fingerprint = gen_empty_fingerprint() + can_bus = CanBus(None, fingerprint, True) + fingerprint[can_bus.CAM][0x110] = 32 + fingerprint[can_bus.ECAN][0x1CF] = 8 + + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles) + + assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING + assert FPCP.redneckCruiseAvailable + assert not FPCP.pcmCruiseSpeed + assert CP.pcmCruise + assert not CP.openpilotLongitudinalControl + assert not CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG + controller = CarInterface(CP, FPCP).CC + assert not controller.long_active_ecu + + controller.frame = 30 + CS = SimpleNamespace(redneck_send_button=1, buttons_counter=5) + msgs = controller._create_canfd_redneck_button_messages(CS) + assert len(msgs) == 20 + assert all(msg[0] == 0x1CF and msg[2] == can_bus.ECAN for msg in msgs) + assert all(msg[1][2] & 0x7 == Buttons.RES_ACCEL for msg in msgs) + + controller.frame = 60 + CS.redneck_send_button = 2 + msgs = controller._create_canfd_redneck_button_messages(CS) + assert len(msgs) == 20 + assert all(msg[1][2] & 0x7 == Buttons.SET_DECEL for msg in msgs) + + monkeypatch.setattr(FakeParams, "get_bool", staticmethod(lambda key: False)) + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles) + assert FPCP.redneckCruiseAvailable + assert FPCP.pcmCruiseSpeed + assert CP.pcmCruise + assert not CP.openpilotLongitudinalControl + + def test_sportage_redneck_rejects_unverified_button_layouts(self, monkeypatch): + class FakeParams: + def __init__(self, *args, **kwargs): + pass + + @staticmethod + def get_bool(key): + return key == "RedneckCruise" + + monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams) + toggles = get_test_toggles() + for button_address, button_bus, button_length, lka_steering in ( + (0x1AA, 1, 16, True), + (0x1CF, 0, 8, True), + (0x1CF, 0, 8, False), + (0x1CF, 1, 16, True), + ): + fingerprint = gen_empty_fingerprint() + can_bus = CanBus(None, fingerprint, lka_steering) + if lka_steering: + fingerprint[can_bus.CAM][0x110] = 32 + fingerprint[button_bus][button_address] = button_length + + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles) + assert not FPCP.redneckCruiseAvailable + assert FPCP.pcmCruiseSpeed + assert CP.pcmCruise + assert not CP.openpilotLongitudinalControl + + fingerprint = gen_empty_fingerprint() + can_bus = CanBus(None, fingerprint, True) + fingerprint[can_bus.CAM][0x110] = 32 + fingerprint[can_bus.ECAN][0x1CF] = 8 + fingerprint[can_bus.ECAN][0x1AA] = 16 + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles) + assert not FPCP.redneckCruiseAvailable + + fingerprint = gen_empty_fingerprint() + can_bus = CanBus(None, fingerprint, True) + fingerprint[can_bus.CAM][0x110] = 32 + fingerprint[can_bus.ECAN][0x1CF] = 8 + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles) + CP.openpilotLongitudinalControl = True + FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles) + assert not FPCP.redneckCruiseAvailable + def test_hyundai_non_scc_without_redneck_keeps_stock_longitudinal_mode(self, monkeypatch): class FakeParams: def __init__(self, *args, **kwargs): diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index b8f585ff49..54393d7dff 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -248,12 +248,24 @@ class CarInterfaceBase(ABC): (candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6): fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value - fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and - not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and - not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) + sportage_stock_scc_buttons = ( + candidate == HYUNDAI.KIA_SPORTAGE_HEV_2026 and + not CP.openpilotLongitudinalControl and + bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and + not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and + fingerprint[CAN.ECAN].get(0x1CF) == 8 and + 0x1AA not in fingerprint[CAN.ECAN] + ) + fp_ret.redneckCruiseAvailable = ( + (bool(CP.flags & HyundaiFlags.NON_SCC) and + not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and + not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) or + sportage_stock_scc_buttons + ) if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"): fp_ret.pcmCruiseSpeed = False - CP.openpilotLongitudinalControl = True + if CP.flags & HyundaiFlags.NON_SCC: + CP.openpilotLongitudinalControl = True if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl: fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index d55cea7c4a..400dae7656 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -992,6 +992,22 @@ class TestSportageNoStockLka(unittest.TestCase): "Damping_Gain": 100, }) + def test_stock_scc_buttons_require_engagement(self): + resume = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.RESUME}) + set_button = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.SET}) + self.assertFalse(self.safety.safety_tx_hook(resume)) + self.assertFalse(self.safety.safety_tx_hook(set_button)) + + self.safety.safety_rx_hook(set_button) + self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 1})) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.safety_tx_hook(resume)) + self.assertTrue(self.safety.safety_tx_hook(set_button)) + + self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 0})) + self.assertFalse(self.safety.safety_tx_hook(resume)) + self.assertFalse(self.safety.safety_tx_hook(set_button)) + def test_aol_toggle_keeps_stock_blocked_and_inactive_status_allowed(self): self._speed(30) for expected_aol in (False, True, False, True, False): diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index afa1b5ef65..4b7038f452 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -197,7 +197,9 @@ class RedneckCruise: def _update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None: update_manual_button_timers(CS, self.cruise_button_timers) button_pressed = any(0 < timer <= int(MANUAL_BUTTON_INACTIVE_TIMER / DT_CTRL) for timer in self.cruise_button_timers.values()) - self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed + stock_cruise_ready = (not self.CP.pcmCruise or self.CP.openpilotLongitudinalControl or CS.cruiseState.enabled) + self.is_ready = (CC.enabled and stock_cruise_ready and not CC.cruiseControl.override and + not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed) def _desired_state(self) -> str: if self.v_target > self.v_cruise_cluster: diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index c9662319f9..ae201ab4b2 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -26,13 +26,13 @@ ButtonType = car.CarState.ButtonEvent.Type class TestRedneckCruise(unittest.TestCase): def setUp(self): - self.CP = SimpleNamespace() + self.CP = SimpleNamespace(pcmCruise=False, openpilotLongitudinalControl=False) self.FPCP = SimpleNamespace(pcmCruiseSpeed=False, redneckCruiseAvailable=True) self.redneck = RedneckCruise(self.CP, self.FPCP) - def _new_state(self, speed_cluster_mph=20.0, button_events=None): + def _new_state(self, speed_cluster_mph=20.0, button_events=None, cruise_enabled=True): return SimpleNamespace( - cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS), + cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS, enabled=cruise_enabled), buttonEvents=button_events or [], ) @@ -143,6 +143,21 @@ class TestRedneckCruise(unittest.TestCase): send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, **kwargs) self.assertEqual(SEND_BUTTON_NONE, send_button) + def test_stock_scc_only_sends_buttons_while_engaged(self): + self.CP.pcmCruise = True + frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4 + for _ in range(frames): + send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(), + 25.0 * CV.MPH_TO_MS, is_metric=False) + self.assertEqual(SEND_BUTTON_NONE, send_button) + + send_button, _ = self._run_until_active(target_mph=25.0) + self.assertEqual(SEND_BUTTON_INCREASE, send_button) + + send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(), + 25.0 * CV.MPH_TO_MS, is_metric=False) + self.assertEqual(SEND_BUTTON_NONE, send_button) + def test_resets_when_pcm_cruise_speed_is_enabled(self): self.FPCP.pcmCruiseSpeed = True send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 88c696a4a3..321c34ba4f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -35,6 +35,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( is_gm_silverado_early_follow_lead, is_toyota_rav4_tss2_post_departure_tune, get_toyota_rav4_tss2_early_lead_cap, + get_toyota_corolla_braking_lead_cap, is_toyota_rav4_tss2_radar_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, @@ -2580,6 +2581,16 @@ class LongitudinalPlanner: vision_low_speed_stop_active = False vision_brake_cap_active = False if lead_control_active: + if (not experimental_mode and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False))): + corolla_cap = get_toyota_corolla_braking_lead_cap( + self.CP, self.lead_one, v_ego, + desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow), output_accel_min, + ) + if corolla_cap is not None: + close_lead_caps.append(corolla_cap) for lead in (self.lead_one, self.lead_two): rav4_early_lead_cap = get_toyota_rav4_tss2_early_lead_cap( self.CP, lead, v_ego, output_accel_min, diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 4430ee46e5..84299cb1a7 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -24,6 +24,8 @@ HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75 KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE = 1.5 +KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 +KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER = 1.5 GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER = 2.0 GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER = 5.0 @@ -66,6 +68,7 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB = 0.95 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET = 1.75 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32 +TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL = 1.5 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2 @@ -168,6 +171,31 @@ def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego): return float(min(bias, max(distance - 0.5, 0.0))) +def get_toyota_corolla_braking_lead_cap(CP, lead, v_ego, desired_gap, accel_min): + if ( + getattr(CP, "brand", "") != "toyota" or + str(getattr(CP, "carFingerprint", "")) != "TOYOTA_COROLLA_TSS2" or + lead is None or not bool(getattr(lead, "status", False)) or + not bool(getattr(lead, "radar", False)) or + float(v_ego) < 10.0 or + float(getattr(lead, "vLead", 0.0)) < 2.0 or + abs(float(getattr(lead, "yRel", 0.0))) > 1.2 + ): + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + distance = float(getattr(lead, "dRel", float("inf"))) + if ( + lead_brake < 0.6 or + float(v_ego) - float(lead.vLead) < 0.75 or + distance <= 0.0 or distance > min(60.0, 3.0 * float(v_ego)) or + distance > float(desired_gap) + 6.0 + ): + return None + + return max(float(accel_min), -min(TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL, 0.65 * lead_brake)) + + def is_honda_crv_5g(CP): return ( getattr(CP, "brand", "") == "honda" and @@ -532,6 +560,11 @@ def allow_radar_standstill_gap_settle(CP): def get_far_follow_output_slew_rates(CP): + if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV": + return ( + KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE, + KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE, + ) if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": return ( HONDA_ACCORD_FAR_FOLLOW_BRAKE_SLEW_RATE, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 1650d2c63e..3133c26827 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -376,6 +376,30 @@ def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow(): assert smoothed == pytest.approx(-0.5) +def test_niro_ev_far_follow_slew_is_vehicle_specific_and_preserves_urgent_braking(): + CP = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV) + next_gen = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV_2ND_GEN) + planner = LongitudinalPlanner(CP, init_v=16.0) + planner.lead_one = make_lead(status=True, d_rel=45.0, v_lead=15.0, model_prob=0.99, y_rel=0.0) + planner.lead_two = make_lead(status=False) + + assert get_far_follow_output_slew_rates(CP) == pytest.approx((2.5, 1.75)) + assert get_far_follow_output_slew_rates(next_gen) == (0.0, 0.0) + first = planner.get_vehicle_far_follow_slew_target(16.0, 0.0, -0.4, False, False) + release = planner.get_vehicle_far_follow_slew_target(16.0, first, 0.3, False, False) + assert first == pytest.approx(-0.4) + assert release == pytest.approx(first + 1.75 * planner.dt) + + planner.lead_one.dRel = 18.0 + assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5) + assert not planner.far_follow_output_slew_active + + planner.lead_one.dRel = 45.0 + planner.lead_one.vLead = 10.0 + assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5) + assert not planner.far_follow_output_slew_active + + def test_crv_far_follow_output_slew_damps_nonurgent_lead_transition(): v_ego = 24.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index e03c48cfce..2af1a29de8 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -85,6 +85,8 @@ MACH_E_PATH_ANGLE_MAX = 0.16 MACH_E_PATH_ANGLE_STEP = 0.055 MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0 MACH_E_PATH_ANGLE_MAX_SPEED = 8.8 +MACH_E_PATH_ANGLE_TRACKING_FACTOR = 0.75 +MACH_E_PATH_ANGLE_DRIVER_COOLDOWN = 0.75 FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } @@ -171,6 +173,7 @@ class FordLateralController: self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT))) self.curvature_last = 0.0 self.path_angle_last = 0.0 + self.path_angle_driver_cooldown = 0.0 self.desired_curvature_last = 0.0 self._frame = 0 self._update_params() @@ -232,18 +235,28 @@ class FordLateralController: deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0])) return base + (MACH_E_UNDERSTEER_ERROR_MAX - base) * speed_weight * deficit_weight - def _path_angle_assist(self, requested: float, desired: float, applied: float, v_ego: float, + def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float, steering_pressed: bool, lane_change: bool) -> float: + if steering_pressed: + self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN + else: + self.path_angle_driver_cooldown = max(0.0, self.path_angle_driver_cooldown - STEER_DT) target = 0.0 if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and - not steering_pressed and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and + not steering_pressed and self.path_angle_driver_cooldown == 0.0 and not lane_change and + 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and requested * desired > 0.0 and requested * applied > 0.0 and - abs(requested) > 0.021 and abs(desired) > 0.021 and abs(applied) >= 0.0195): + abs(requested) > 0.0198 and abs(desired) > 0.016 and abs(applied) >= 0.0195 and + np.sign(desired) * (desired - current) > 0.002): max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2 - residual = max(0.0, min(abs(requested), abs(desired), max_curvature) - abs(applied)) + residual = max(0.0, min(max(abs(requested), abs(desired)), max_curvature) - abs(applied)) + tracking_deficit = max(0.0, np.sign(applied) * (applied - current)) + acceleration_headroom = max(0.0, (max_curvature - abs(applied)) * v_ego) speed_weight = float(np.interp( v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0])) - target = float(np.sign(applied) * min(residual * v_ego * speed_weight, MACH_E_PATH_ANGLE_MAX)) + target = float(np.sign(applied) * min( + (residual + MACH_E_PATH_ANGLE_TRACKING_FACTOR * tracking_deficit) * v_ego, + acceleration_headroom, MACH_E_PATH_ANGLE_MAX) * speed_weight) if target == 0.0 or target * self.path_angle_last < 0.0: self.path_angle_last = 0.0 else: @@ -477,11 +490,14 @@ class FordLateralController: self.curvature_samples.clear() self.curvature_last = 0.0 self.path_angle_last = 0.0 + self.path_angle_driver_cooldown = 0.0 self.desired_curvature_last = 0.0 return FordLateralResult() manual_turn = self._manual_turn(CC, CS, float(actuators.curvature)) if manual_turn or CS.out.vEgoRaw < 0.1: + if CS.out.steeringPressed: + self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN self.curvature_samples.clear() self.curvature_last = 0.0 self.path_angle_last = 0.0 @@ -557,7 +573,7 @@ class FordLateralController: max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2 applied = float(np.clip(applied, -max_curvature, max_curvature)) path_angle = self._path_angle_assist( - requested, desired, applied, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) + requested, desired, applied, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) self.curvature_samples.append(predicted) curvature_rate = 0.0 diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 5323331a72..fa31ca9a55 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -120,17 +120,31 @@ def test_understeer_error_preserves_other_fords(controller): def test_mach_e_path_angle_assist_at_curvature_limit(controller): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.CP.flags = FordFlags.CANFD - outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) for _ in range(3)] + outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False) for _ in range(3)] assert outputs == pytest.approx([0.055, 0.110, 0.150]) - assert controller._path_angle_assist(0.018, 0.018, 0.018, 7.5, False, False) == 0.0 - assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, False, False) == pytest.approx(-0.055) - assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, True, False) == 0.0 + assert controller._path_angle_assist(0.018, 0.018, 0.018, 0.018, 7.5, False, False) == 0.0 + assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, False, False) == pytest.approx(-0.055) + assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, True, False) == 0.0 + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_path_angle_assist_starts_at_saturation_and_releases_after_driver(controller, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + request = (sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.007, 7.0) + assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055) + assert controller._path_angle_assist(*request, True, False) == 0.0 + for _ in range(round(0.75 / STEER_DT) - 1): + assert controller._path_angle_assist(*request, False, False) == 0.0 + assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055) + assert controller._path_angle_assist( + sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.021, 7.0, False, False) == 0.0 @pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", ( (9.0, 0.04, 0.04, 0.02, False, False), - (7.5, 0.020, 0.04, 0.02, False, False), - (7.5, 0.04, 0.018, 0.02, False, False), + (7.5, 0.0197, 0.04, 0.02, False, False), + (7.5, 0.04, 0.015, 0.02, False, False), (7.5, 0.04, 0.04, 0.018, False, False), (7.5, 0.04, 0.04, 0.02, True, False), (7.5, 0.04, 0.04, 0.02, False, True), @@ -138,18 +152,18 @@ def test_mach_e_path_angle_assist_at_curvature_limit(controller): def test_mach_e_path_angle_assist_is_scoped(controller, speed, requested, desired, applied, driver, lane_change): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.CP.flags = FordFlags.CANFD - assert controller._path_angle_assist(requested, desired, applied, speed, driver, lane_change) == 0.0 + assert controller._path_angle_assist(requested, desired, applied, 0.01, speed, driver, lane_change) == 0.0 def test_path_angle_assist_preserves_other_fords(controller): controller.CP.flags = FordFlags.CANFD - assert controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) == 0.0 + assert controller._path_angle_assist(0.04, 0.04, 0.02, 0.01, 7.5, False, False) == 0.0 def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.CP.flags = FordFlags.CANFD - assist = controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) + assist = controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False) packer = CANPacker("ford_lincoln_base_pt") can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()])) _, data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, -0.02, 0.0, 0, -assist)