mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-27 09:53:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Carbon Rods

Browse Source
This commit is contained in:
firestar5683
2026-08-07 23:35:12 -05:00
parent cd1d13e456
commit 06f6ff66a8
20 changed files with 281 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/gm/carcontroller.py
+4 -3
View File
@@ -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
opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py
+2 -2
View File
@@ -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():
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+8 -1
View File
@@ -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,
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+5 -2
View File
@@ -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:
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+37
View File
@@ -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
opendbc_repo/opendbc/car/subaru/fingerprints.py
+17
View File
@@ -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',
],
},
}
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+27
View File
@@ -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)
opendbc_repo/opendbc/car/subaru/values.py
+5
View File
@@ -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,
opendbc_repo/opendbc/car/torque_data/override.toml
+1
View File
@@ -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]
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+8 -1
View File
@@ -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 {
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+8
View File
@@ -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()
panda/board/boards/board_declarations.h
+1
View File
@@ -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
panda/python/__init__.py
+2 -1
View File
@@ -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("<BIBBBBBBBBIIIIIIIHHBBBIIII")
F4_DEVICES = [HW_TYPE_WHITE, HW_TYPE_BLACK, HW_TYPE_DOS]
H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_TRES, HW_TYPE_CUATRO, HW_TYPE_BODY]
H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES, HW_TYPE_CUATRO, HW_TYPE_BODY]
SUPPORTED_DEVICES = H7_DEVICES + F4_DEVICES
INTERNAL_DEVICES = (HW_TYPE_DOS, HW_TYPE_TRES, HW_TYPE_CUATRO)
scripts/model_compiler.py
+9 -3
View File
@@ -73,7 +73,7 @@ def wait_for_external_gpu(compile_env: dict[str, str]) -> 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:
selfdrive/controls/lib/latcontrol_torque.py
+2
View File
@@ -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
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+18 -1
View File
@@ -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]))
selfdrive/controls/lib/longitudinal_planner.py
+21 -2
View File
@@ -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)
selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+18
View File
@@ -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."""
selfdrive/controls/tests/test_latcontrol.py
+7
View File
@@ -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(
selfdrive/controls/tests/test_longitudinal_planner.py
+81 -1
View File
@@ -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)