diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 9dd3724aa..b8152bd92 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -69,7 +69,8 @@ TRUCK_LONG_SMOOTH_CARS = { TRUCK_FRICTION_BRAKE_ENGAGE = 40 TRUCK_FRICTION_BRAKE_RELEASE = 8 TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL = -0.85 -TRUCK_FOLLOW_MICRO_ACCEL_MAX = 0.30 +TRUCK_FOLLOW_MICRO_ACCEL_MAX = 0.55 +TRUCK_FOLLOW_MICRO_ACCEL_MIN = -0.10 TRUCK_FOLLOW_MICRO_ACCEL_SLEW = 1.5 ACC_DASHBOARD_ZERO_RESERVED_CARS = { CAR.CHEVROLET_BLAZER, @@ -238,8 +239,8 @@ def smooth_truck_follow_accel(accel: float, previous_accel: float, v_ego: float, enabled: bool, lead_visible: bool, stopping: bool) -> float: if ( not enabled or not lead_visible or stopping or v_ego < 25.0 or - abs(accel) > TRUCK_FOLLOW_MICRO_ACCEL_MAX or accel <= TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL or - abs(previous_accel) > TRUCK_FOLLOW_MICRO_ACCEL_MAX or previous_accel <= TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL + accel > TRUCK_FOLLOW_MICRO_ACCEL_MAX or accel < TRUCK_FOLLOW_MICRO_ACCEL_MIN or + previous_accel > TRUCK_FOLLOW_MICRO_ACCEL_MAX or previous_accel < TRUCK_FOLLOW_MICRO_ACCEL_MIN ): return accel diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index aae318968..bef0b7241 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -871,9 +871,9 @@ def test_shape_truck_positive_accel_keeps_more_highway_follow_authority(): def test_smooth_truck_follow_accel_slews_small_highway_commands(): - shaped = smooth_truck_follow_accel(0.20, -0.20, 30.0, True, True, False) + shaped = smooth_truck_follow_accel(0.50, 0.0, 30.0, True, True, False) - assert shaped == pytest.approx(-0.14) + assert shaped == pytest.approx(0.06) def test_smooth_truck_follow_accel_does_not_delay_safety_requests(): diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index c71a73ee1..9550a65cc 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -792,7 +792,14 @@ class CarController(CarControllerBase): steering_msg_active, apply_torque, apply_angle, CS.stock_lfa_msg, CS.stock_lkas_msg if preserve_stock_lkas else None, - lka_icon=lka_icon)) + lka_icon=lka_icon, + send_lfa_status=self.ecu_disable_failed and + self.CP.carFingerprint == CAR.KIA_EV9)) + elif self.ecu_disable_failed and self.CP.carFingerprint == CAR.KIA_EV9: + can_sends.extend(hyundaicanfd.create_steering_messages( + self.packer, self.CP, self.CAN, CC.enabled, False, 0.0, 0.0, + CS.stock_lfa_msg, lka_icon=lka_icon, send_lfa_status=True, lfa_only=True, + )) direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault inactive_steering_angle = float(np.clip(CS.angle_steering_angle, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 09bfb0a2b..3294a2a4e 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -97,7 +97,8 @@ def create_angle_adas_cmd(packer, CAN, apply_angle: float, lat_active: bool, tor def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle, - lfa_base_values=None, lkas_base_values=None, lka_icon=None): + lfa_base_values=None, lkas_base_values=None, lka_icon=None, + send_lfa_status=False, lfa_only=False): if lka_icon is None: lka_icon = 2 if enabled else 1 angle_lkas_alt = CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT @@ -190,8 +191,10 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, ret = [] if CP.flags & HyundaiFlags.CANFD_LKA_STEERING: lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS" - if CP.openpilotLongitudinalControl: + if CP.openpilotLongitudinalControl or send_lfa_status: ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) + if lfa_only: + return ret ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, lkas_values)) else: if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 4d96e7dbd..f0651a9ee 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -2185,6 +2185,43 @@ class TestHyundaiFingerprint: ("LKAS", can_bus.ACAN), ] + def test_ev9_fallback_keeps_lfa_status_without_longitudinal_control(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV9 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CCNC | + HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.CANFD_LKA_STEERING | + HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = False + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + msgs = hyundaicanfd.create_steering_messages( + packer, CP, can_bus, True, True, 0.44, -31.5, send_lfa_status=True, + ) + + assert [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [ + ("LFA", can_bus.ECAN), + ("LKAS_ALT", can_bus.ACAN), + ] + + def test_ev9_fallback_lfa_only_does_not_send_lkas_at_standstill(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV9 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CCNC | + HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.CANFD_LKA_STEERING | + HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = False + + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + msgs = hyundaicanfd.create_steering_messages( + packer, CP, can_bus, True, False, 0.0, 0.0, send_lfa_status=True, lfa_only=True, + ) + + assert [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [ + ("LFA", can_bus.ECAN), + ] + def test_kia_ev6_lkas_helper_preserves_stock_camera_fields_with_stock_long(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV6 diff --git a/opendbc_repo/opendbc/car/subaru/fingerprints.py b/opendbc_repo/opendbc/car/subaru/fingerprints.py index 370ae734c..799b48140 100644 --- a/opendbc_repo/opendbc/car/subaru/fingerprints.py +++ b/opendbc_repo/opendbc/car/subaru/fingerprints.py @@ -594,4 +594,21 @@ FW_VERSIONS = { b'\xa9\x17w!r', ], }, + CAR.SUBARU_LEGACY_2025: { + (Ecu.abs, 0x7b0, None): [ + b'\xa1 $\x11\x00', + ], + (Ecu.eps, 0x746, None): [ + b'[\xc0\xd1\x10\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x1a!\x08\x00C\x0e!\x08\x018', + ], + (Ecu.engine, 0x7a2, None): [ + b'\x08,\xa0p\x07', + ], + (Ecu.transmission, 0x7a3, None): [ + b'\xeb\x17U!r', + ], + }, } diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 4bb99d44f..14750cb70 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -86,10 +86,24 @@ class TestSubaruFingerprint: assert exact assert matches == {CAR.SUBARU_OUTBACK_2023} + def test_legacy_2025_firmware(self): + legacy_fw = FW_VERSIONS[CAR.SUBARU_LEGACY_2025] + car_fw = [ + CarParams.CarFw(ecu=CarParams.Ecu.abs, fwVersion=b'\xa1 $\x11\x00', address=0x7b0, brand="subaru"), + CarParams.CarFw(ecu=CarParams.Ecu.eps, fwVersion=b'[\xc0\xd1\x10\x00', address=0x746, brand="subaru"), + CarParams.CarFw(ecu=CarParams.Ecu.fwdCamera, fwVersion=b'\x1a!\x08\x00C\x0e!\x08\x018', address=0x787, brand="subaru"), + CarParams.CarFw(ecu=CarParams.Ecu.engine, fwVersion=b'\x08,\xa0p\x07', address=0x7a2, brand="subaru"), + CarParams.CarFw(ecu=CarParams.Ecu.transmission, fwVersion=b'\xeb\x17U!r', address=0x7a3, brand="subaru"), + ] + exact, matches = match_fw_to_car(car_fw, "4S3BWGG67S3011945", allow_fuzzy=False, log=False) + assert exact + assert matches == {CAR.SUBARU_LEGACY_2025} + ANGLE_PLATFORMS = ( CAR.SUBARU_FORESTER_2022, CAR.SUBARU_OUTBACK_2023, + CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_CROSSTREK_2025, ) @@ -127,6 +141,19 @@ def test_outback_2023_uses_d_platform_bus_layout(): assert parsers[Bus.alt].bus == CanBus.alt +def test_legacy_2025_uses_d_platform_bus_layout(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) + parsers = CarState.get_can_parsers(CP) + + assert CP.flags & SubaruFlags.D_PLATFORM + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM + assert CanBus.main_for_cp(CP) == CanBus.alt + assert CanBus.angle_for_cp(CP) == CanBus.camera + assert parsers[Bus.pt].bus == CanBus.alt + assert parsers[Bus.cam].bus == CanBus.camera + assert parsers[Bus.alt].bus == CanBus.alt + + def test_other_angle_platforms_keep_existing_bus_layout(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025) parsers = CarState.get_can_parsers(CP) diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index ee1cdfabf..ff5c756ef 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -237,6 +237,11 @@ class CAR(Platforms): SUBARU_OUTBACK.specs, flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM, ) + SUBARU_LEGACY_2025 = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Legacy 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + SUBARU_OUTBACK.specs, + flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM, + ) SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig( [SubaruCarDocs("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], SUBARU_ASCENT.specs, diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index df375cdbf..8368bb0be 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -13,6 +13,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] # New subarus angle based controllers "SUBARU_FORESTER_2022" = [nan, 3.0, nan] "SUBARU_OUTBACK_2023" = [nan, 3.0, nan] +"SUBARU_LEGACY_2025" = [nan, 3.0, nan] "SUBARU_ASCENT_2023" = [nan, 3.0, nan] "SUBARU_CROSSTREK_2025" = [nan, 3.0, nan] diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index 58ca24537..d2be6f95b 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -427,6 +427,11 @@ static safety_config hyundai_canfd_init(uint16_t param) { {0x1DA, 1, 32, .check_relay = false}, // ADRV_0x1da }; + static const CanMsg HYUNDAI_CANFD_CCNC_ANGLE_FALLBACK_TX_MSGS[] = { + HYUNDAI_CANFD_LKA_STEERING_ALT_COMMON_TX_MSGS(0, 1) + {0x12A, 1, 16, .check_relay = false}, // LFA status + }; + static const CanMsg HYUNDAI_CANFD_LFA_STEERING_TX_MSGS[] = { HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2) HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0) @@ -550,7 +555,9 @@ static safety_config hyundai_canfd_init(uint16_t param) { } else { SET_RX_CHECKS(hyundai_canfd_lka_steering_rx_checks, ret); } - if (hyundai_canfd_lka_steering_alt) { + if (hyundai_ccnc && hyundai_canfd_angle_steering && hyundai_canfd_lka_steering_alt) { + SET_TX_MSGS(HYUNDAI_CANFD_CCNC_ANGLE_FALLBACK_TX_MSGS, ret); + } else if (hyundai_canfd_lka_steering_alt) { if (hyundai_canfd_alt_buttons) { SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEERING_ALT_ALT_BUTTONS_TX_MSGS, ret); } else { diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 247a17b50..1d3771758 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -793,6 +793,14 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu with self.subTest(address=address): self.assertFalse(self._tx(common.make_msg(1 if address != 0x51 else 0, address, length))) + def test_ccnc_angle_fallback_allows_lfa_status_without_longitudinal_control(self): + fallback_param = (self.SAFETY_PARAM & ~HyundaiSafetyFlags.LONG) | HyundaiSafetyFlags.CCNC + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, fallback_param) + self.safety.init_tests() + + self.assertTrue(self._tx(common.make_msg(1, 0x12A, 16))) + self.assertFalse(self._tx(common.make_msg(1, 0x1A0, 32))) + def test_ccnc_angle_long_uses_second_mdps_angle(self): self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM | HyundaiSafetyFlags.CCNC) self.safety.init_tests() diff --git a/panda/board/boards/board_declarations.h b/panda/board/boards/board_declarations.h index a9f0d7acc..cbe4eead2 100644 --- a/panda/board/boards/board_declarations.h +++ b/panda/board/boards/board_declarations.h @@ -52,6 +52,7 @@ struct board { #define HW_TYPE_UNKNOWN 0U #define HW_TYPE_DOS 6U #define HW_TYPE_RED_PANDA 7U +#define HW_TYPE_RED_PANDA_V2 8U #define HW_TYPE_TRES 9U #define HW_TYPE_CUATRO 10U diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 2e3cef759..3187092f7 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -116,6 +116,7 @@ class Panda: HW_TYPE_BLACK = b'\x03' HW_TYPE_DOS = b'\x06' HW_TYPE_RED_PANDA = b'\x07' + HW_TYPE_RED_PANDA_V2 = b'\x08' HW_TYPE_TRES = b'\x09' HW_TYPE_CUATRO = b'\x0a' HW_TYPE_BODY = b'\xb1' @@ -127,7 +128,7 @@ class Panda: CAN_HEALTH_STRUCT = struct.Struct(" None: doing the same here avoids making the model compiler lose its one chance at initialization while keeping all non-GPU builds unchanged. """ - probe = [sys.executable, "-c", "from tinygrad.device import Device; Device[Device.DEFAULT]"] + probe = [sys.executable, "-c", "from tinygrad.device import Device; Device[Device.DEFAULT]; import os; os._exit(0)"] probe_env = {**compile_env, "DEV": "USB+AMD"} diagnostics: list[str] = [] @@ -90,8 +90,14 @@ def wait_for_external_gpu(compile_env: dict[str, str]) -> None: timeout=USBGPU_PROBE_TIMEOUT, check=False, ) - except subprocess.TimeoutExpired: - diagnostics.append(f"probe timed out after {USBGPU_PROBE_TIMEOUT}s") + except subprocess.TimeoutExpired as exc: + partial = exc.stderr or exc.stdout or "" + if isinstance(partial, bytes): + partial = partial.decode(errors="replace") + partial = partial.strip() + diagnostics.append( + f"probe timed out after {USBGPU_PROBE_TIMEOUT}s" + (f": {partial[-2000:]}" if partial else "") + ) continue if result.returncode == 0: diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f20c1dbe0..8d7aadae1 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -280,6 +280,8 @@ class LatControlTorque(LatControl): ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale + if self.is_ram_1500: + ff *= get_ram_1500_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) trailer_load_kg = float(max(getattr(starpilot_toggles, "trailer_load_kg", 0.0) or 0.0, 0.0)) bolt_2022_2023_tuned_path_active = self.is_bolt_2022_2023 bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index b23433dbc..4a22faf29 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -262,7 +262,7 @@ BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 7.2 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.5 -BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.035 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.050 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7 @@ -895,6 +895,13 @@ RAM_1500_TRANSITION_JERK_ONSET = 0.35 RAM_1500_TRANSITION_JERK_FULL = 1.10 RAM_1500_TRANSITION_LAT_FADE_START = 0.65 RAM_1500_TRANSITION_LAT_FADE_END = 1.85 +RAM_1500_PHASE_SCALE = 0.12 +RAM_1500_PHASE_SPEED_ONSET = 8.0 +RAM_1500_PHASE_SPEED_FULL = 15.0 +RAM_1500_PHASE_LAT_ONSET = 0.25 +RAM_1500_PHASE_LAT_WIDTH = 0.12 +RAM_1500_TURN_IN_FF_BOOST = 0.06 +RAM_1500_UNWIND_FF_REDUCTION = 0.10 # The Kona route is exceptionally accurate below highway speed, but Pop V2 # reverses the requested lateral acceleration roughly once per second at @@ -1338,6 +1345,16 @@ def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_l return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight) +def get_ram_1500_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAM_1500_PHASE_SCALE) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + speed_weight = float(np.interp(v_ego, [RAM_1500_PHASE_SPEED_ONSET, RAM_1500_PHASE_SPEED_FULL], [0.0, 1.0])) + lat_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_PHASE_LAT_ONSET) / RAM_1500_PHASE_LAT_WIDTH) + return 1.0 + ((RAM_1500_TURN_IN_FF_BOOST * turn_in_weight - + RAM_1500_UNWIND_FF_REDUCTION * unwind_weight) * speed_weight * lat_weight) + + def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_TRANSITION_SPEED_ONSET, KONA_NON_SCC_TRANSITION_SPEED_FULL], [0.0, 1.0])) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 70ee17e6a..16f5c4f43 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.lead_follow_policy import apply as apply_f from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_duplicate_vision_follow from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, + is_gm_silverado_early_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, ) @@ -55,6 +56,7 @@ ALLOW_THROTTLE_ENABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYST ALLOW_THROTTLE_DISABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS ALLOW_THROTTLE_TRANSITION_CONFIRM_TIME = 0.25 MIN_ALLOW_THROTTLE_SPEED = 5.0 +FORCE_DECEL_MIN_ACCEL = -0.05 MODEL_LAUNCH_DISARM_SPEED = 2.0 MODEL_LAUNCH_COMMIT_TIME = 3.5 MODEL_LAUNCH_MOVING_SPEED = 1.2 @@ -62,6 +64,8 @@ MODEL_LAUNCH_MAX_ACCEL = 1.5 RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 +RAW_RADAR_STOPPED_LEAD_MAX_SPEED = 1.0 +RAW_RADAR_STOPPED_LEAD_MAX_DISTANCE = 120.0 RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED = 4.5 RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED = 3.5 RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE = 10.0 @@ -83,7 +87,7 @@ LEAD_DEPART_RELEASE_HOLD_TIME = 1.5 LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME = 0.15 STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED = 0.5 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED = 0.45 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.35 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.50 STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB = 0.95 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET = 1.75 STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE = 3.0 @@ -1811,6 +1815,11 @@ class LongitudinalPlanner: return False dynamic_distance = max(RAW_LEAD_SAFETY_DISTANCE, 3.0 * float(v_ego)) + if bool(getattr(lead, "radar", False)) and lead_speed <= RAW_RADAR_STOPPED_LEAD_MAX_SPEED: + dynamic_distance = max( + dynamic_distance, + min(RAW_RADAR_STOPPED_LEAD_MAX_DISTANCE, 5.0 * float(v_ego)), + ) ttc = d_rel / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking) @@ -1961,9 +1970,13 @@ class LongitudinalPlanner: self.lead_one = sm['radarState'].leadOne self.lead_two = sm['radarState'].leadTwo raw_close_lead_control = any(self.raw_close_lead_needs_control(lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) + early_truck_follow = ( + not experimental_mode and + any(is_gm_silverado_early_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) + ) # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead # safety path so ACC/chill does not ignore a visible lead during that debounce. - lead_control_active = tracking_lead or raw_close_lead_control + lead_control_active = tracking_lead or raw_close_lead_control or early_truck_follow lead_one_active = bool(self.lead_one.status and lead_control_active) effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) @@ -2820,6 +2833,12 @@ class LongitudinalPlanner: output_a_target = float(min(max(output_a_target, lc_merge_floor), output_accel_max)) self.a_desired = max(self.a_desired, min(lc_merge_floor, output_accel_max)) + # Force-decel is the driver-monitoring no-response path. Keep a small + # braking floor until the vehicle is actually stopped; normal MPC tapering + # can otherwise leave it creeping indefinitely at the maneuver-test cutoff. + if force_slow_decel and scene_v_ego > 0.1: + output_a_target = min(output_a_target, FORCE_DECEL_MIN_ACCEL) + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index d3424fe69..44d606391 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -4,6 +4,10 @@ import numpy as np HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35 +GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 +GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0 +GM_SILVERADO_EARLY_FOLLOW_MIN_MODEL_PROB = 0.85 +GM_SILVERADO_EARLY_FOLLOW_MAX_LATERAL_OFFSET = 1.2 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED = 2.0 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_SPEED = 0.45 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_DELTA = 0.35 @@ -29,6 +33,20 @@ def get_untracked_slow_lead_decel_scale(CP): return 1.0 +def is_gm_silverado_early_follow_lead(CP, lead, v_ego): + """Admit a credible centered vision lead before it becomes a close lead.""" + if ( + CP.brand != "gm" or str(CP.carFingerprint) not in ("CHEVROLET_SILVERADO", "CHEVROLET_SILVERADO_CC") or + lead is None or not bool(getattr(lead, "status", False)) or bool(getattr(lead, "radar", False)) or + float(v_ego) < GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED or + float(getattr(lead, "dRel", float("inf"))) > GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE or + float(getattr(lead, "modelProb", 0.0)) < GM_SILVERADO_EARLY_FOLLOW_MIN_MODEL_PROB or + abs(float(getattr(lead, "yRel", 0.0))) > GM_SILVERADO_EARLY_FOLLOW_MAX_LATERAL_OFFSET + ): + return False + return True + + def get_toyota_sienna_post_departure_restop_cap(CP, lead, v_ego, accel_min, stop_distance, now_t, departure_latch_until): """Re-arm a stop if a Sienna's lead twitches forward and stops again.""" diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 96d859213..c64c84254 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,6 +32,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT, RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT, get_ram_1500_transition_output_scale, + get_ram_1500_ff_scale, get_subaru_impreza_pid_output_scale, normalize_flm_overrides, set_flm_runtime_overrides, @@ -839,6 +840,12 @@ class TestLatControl: assert 0.6 < center_transition < medium_transition < 1.0 assert get_ram_1500_transition_output_scale(1.85, 2.5, 17.0) == pytest.approx(1.0) + def test_ram_1500_phase_feedforward_curve(self): + assert get_ram_1500_ff_scale(0.0, 1.0, 15.0) == pytest.approx(1.0) + assert get_ram_1500_ff_scale(1.2, 1.1, 17.0) > 1.0 + assert get_ram_1500_ff_scale(1.2, -1.1, 17.0) < 1.0 + assert get_ram_1500_ff_scale(1.2, 1.1, 6.0) < get_ram_1500_ff_scale(1.2, 1.1, 17.0) + def test_ram_1500_transition_taper_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(CHRYSLER.RAM_1500_5TH_GEN) base_output, _, lac_log = controller.update( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 906fd7a07..7e6e0f603 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -18,7 +18,10 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_toyota_sienna_post_departure_restop_cap +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( + get_toyota_sienna_post_departure_restop_cap, + is_gm_silverado_early_follow_lead, +) from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld import modeld @@ -464,6 +467,59 @@ def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_trac np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) +def test_gm_silverado_admits_credible_far_vision_lead_for_acc_follow(): + CP = SimpleNamespace(brand="gm", carFingerprint=GM_CAR.CHEVROLET_SILVERADO) + lead = make_lead(status=True, d_rel=80.0, v_lead=30.0, model_prob=0.90, y_rel=0.2) + + assert is_gm_silverado_early_follow_lead(CP, lead, 30.0) + + +@pytest.mark.parametrize("fingerprint", [GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, CAR.HONDA_CIVIC]) +def test_gm_silverado_early_follow_does_not_apply_to_other_vehicles(fingerprint): + brand = "gm" if fingerprint == GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL else "honda" + CP = SimpleNamespace(brand=brand, carFingerprint=fingerprint) + lead = make_lead(status=True, d_rel=80.0, v_lead=30.0, model_prob=0.99) + + assert not is_gm_silverado_early_follow_lead(CP, lead, 30.0) + + +@pytest.mark.parametrize("kwargs", [ + {"radar": True}, + {"model_prob": 0.80}, + {"y_rel": 1.3}, + {"d_rel": 131.0}, +]) +def test_gm_silverado_early_follow_requires_a_credible_centered_vision_lead(kwargs): + CP = SimpleNamespace(brand="gm", carFingerprint=GM_CAR.CHEVROLET_SILVERADO) + lead_kwargs = {"d_rel": 80.0, "v_lead": 30.0, "model_prob": 0.90} + lead_kwargs.update(kwargs) + lead = make_lead(status=True, **lead_kwargs) + + assert not is_gm_silverado_early_follow_lead(CP, lead, 30.0) + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_acc_mode_uses_far_near_stopped_radar_lead_before_tracking(model_version): + v_ego = 24.6 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.0, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=116.0, v_lead=0.0, a_lead=-0.2, radar=True, model_prob=0.85), + ) + sm["starpilotPlan"].vCruise = v_ego + 6.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.raw_close_lead_needs_control(sm["radarState"].leadOne, v_ego) + assert planner.output_a_target < -0.1 + + def test_cruise_accel_cap_does_not_manufacture_braking_after_set_speed_drop_with_lead(): v_ego = 20.115 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -2029,6 +2085,30 @@ def test_standstill_stopped_lead_guard_does_not_block_radar_depart_at_longer_gap assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_stopped_lead_guard_holds_marginal_creep_release(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.8, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=4.2, v_lead=0.38, a_lead=0.31, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_stopped_lead_guard_blocks_false_release_during_creep_frame(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)