mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-20 07:43:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Uptown Funk

Browse Source
This commit is contained in:
firestar5683
2026-08-10 10:52:01 -05:00
parent 7f6adc7a7f
commit 0315fe483a
22 changed files with 299 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
+1
View File
@@ -209,6 +209,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"CECurvesLead", {PERSISTENT, BOOL, "0", "0", 1}},
{"CELead", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
{"CEModelStopTime", {PERSISTENT, FLOAT, "7.7", "0.0", 2, SETTINGS_SIMPLE}},
{"CEOpenRoad", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"CESignalLaneDetection", {PERSISTENT, BOOL, "1", "0", 2}},
{"CESignalSpeed", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
{"CESlowerLead", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
opendbc_repo/opendbc/car/subaru/carcontroller.py
+1 -1
View File
@@ -35,7 +35,7 @@ class CarController(CarControllerBase):
self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
self.main_bus = CanBus.main_for_cp(CP)
self.angle_bus = CanBus.angle_for_cp(CP)
self.status_bus = CanBus.main
self.status_bus = CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM_CAMERA else CanBus.main
if CP.flags & SubaruFlags.LKAS_ANGLE:
self.VM = VehicleModel(get_safety_CP())
opendbc_repo/opendbc/car/subaru/fingerprints.py
+3
View File
@@ -46,6 +46,7 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x787, None): [
b'\x05!\x08\x1dK\x05!\x08\x01/',
b'\x17!\x08\x01A\x12!\x08\x00;',
b'\x20\x02\x0e',
],
(Ecu.engine, 0x7a2, None): [
b'\xe5,\xa0P\x07',
@@ -53,6 +54,7 @@ FW_VERSIONS = {
],
(Ecu.transmission, 0x7a3, None): [
b'\x04\xfe\xf3\x00\x00',
b'\x05\xfe\xe7\x00\x00',
],
},
CAR.SUBARU_LEGACY: {
@@ -607,6 +609,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x787, None): [
b'\x1a!\x08\x00C\x0e!\x08\x018',
b'\x20\x02\x0e',
],
(Ecu.engine, 0x7a2, None): [
b'\x08,\xa0p\x07',
opendbc_repo/opendbc/car/subaru/interface.py
+2
View File
@@ -38,6 +38,8 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value
if ret.flags & SubaruFlags.D_PLATFORM:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
if ret.flags & SubaruFlags.D_PLATFORM_CAMERA:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+17 -7
View File
@@ -67,7 +67,10 @@ class TestSubaruFingerprint:
for (ecu, _, _), fws in fws_per_ecu.items():
fw_size = len(fws[0])
for fw in fws:
assert len(fw) == fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}"
if platform in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_LEGACY_2025) and ecu == CarParams.Ecu.fwdCamera:
assert len(fw) > 0, f"{platform} {ecu}: empty firmware response"
else:
assert len(fw) == fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}"
def test_outback_2024_firmware(self):
outback_fw = FW_VERSIONS[CAR.SUBARU_OUTBACK_2023]
@@ -92,6 +95,7 @@ class TestSubaruFingerprint:
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.fwdCamera, fwVersion=b'\x20\x02\x0e', 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"),
]
@@ -104,7 +108,9 @@ class TestSubaruFingerprint:
CarParams.CarFw(ecu=CarParams.Ecu.abs, fwVersion=b'\xa5 %\x03\x01', address=0x7b0, brand="subaru"),
CarParams.CarFw(ecu=CarParams.Ecu.eps, fwVersion=b'\x55\xc0\xd0\x10', address=0x746, brand="subaru"),
CarParams.CarFw(ecu=CarParams.Ecu.fwdCamera, fwVersion=b'\x17!\x08\x01A\x12!\x08\x00;', address=0x787, brand="subaru"),
CarParams.CarFw(ecu=CarParams.Ecu.fwdCamera, fwVersion=b'\x20\x02\x0e', address=0x787, brand="subaru"),
CarParams.CarFw(ecu=CarParams.Ecu.engine, fwVersion=b'\x11,\xa00\x07', address=0x7a2, brand="subaru"),
CarParams.CarFw(ecu=CarParams.Ecu.transmission, fwVersion=b'\x05\xfe\xe7\x00\x00', address=0x7a3, brand="subaru"),
]
exact, matches = match_fw_to_car(car_fw, "4S4WMAAD9S3414980", allow_fuzzy=False, log=False)
assert exact
@@ -163,14 +169,16 @@ def test_legacy_2025_uses_d_platform_bus_layout():
assert CP.flags & SubaruFlags.D_PLATFORM
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
assert CP.flags & SubaruFlags.D_PLATFORM_CAMERA
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA
assert CanBus.main_for_cp(CP) == CanBus.alt
assert CanBus.angle_for_cp(CP) == CanBus.main
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
assert parsers[Bus.main].bus == CanBus.main
assert controller.angle_bus == CanBus.main
assert controller.status_bus == CanBus.main
assert controller.angle_bus == CanBus.camera
assert controller.status_bus == CanBus.camera
def test_ascent_2023_uses_d_platform_bus_layout():
@@ -180,14 +188,16 @@ def test_ascent_2023_uses_d_platform_bus_layout():
assert CP.flags & SubaruFlags.D_PLATFORM
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
assert CP.flags & SubaruFlags.D_PLATFORM_CAMERA
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA
assert CanBus.main_for_cp(CP) == CanBus.alt
assert CanBus.angle_for_cp(CP) == CanBus.main
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
assert parsers[Bus.main].bus == CanBus.main
assert controller.angle_bus == CanBus.main
assert controller.status_bus == CanBus.main
assert controller.angle_bus == CanBus.camera
assert controller.status_bus == CanBus.camera
def test_other_angle_platforms_keep_existing_bus_layout():
opendbc_repo/opendbc/car/subaru/values.py
+6 -5
View File
@@ -75,6 +75,7 @@ class SubaruSafetyFlags(IntFlag):
STOP_AND_GO = 8
LKAS_ANGLE = 16
D_PLATFORM = 32
D_PLATFORM_CAMERA = 64
class SubaruFlags(IntFlag):
@@ -92,6 +93,7 @@ class SubaruFlags(IntFlag):
HYBRID = 32
LKAS_ANGLE = 64
D_PLATFORM = 128
D_PLATFORM_CAMERA = 256
GLOBAL_ES_ADDR = 0x787
@@ -113,9 +115,8 @@ class CanBus:
@staticmethod
def angle_for_cp(CP):
# D-platform angle commands reach the EPS through the main bus. The camera
# bus still carries the stock angle message and remains receive-only.
return CanBus.main
# This Ascent variant receives angle commands through EyeSight's camera bus.
return CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM_CAMERA else CanBus.main
class Footnote(Enum):
@@ -242,12 +243,12 @@ class CAR(Platforms):
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,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM | SubaruFlags.D_PLATFORM_CAMERA,
)
SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Ascent 2023-25", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
SUBARU_ASCENT.specs,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM | SubaruFlags.D_PLATFORM_CAMERA,
)
SUBARU_CROSSTREK_2025 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Crosstrek 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
opendbc_repo/opendbc/safety/modes/subaru.h
+17 -8
View File
@@ -55,11 +55,11 @@
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
#define SUBARU_D_PLATFORM_ANGLE_TX_MSGS() \
{MSG_SUBARU_ES_LKAS_ANGLE, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
#define SUBARU_D_PLATFORM_ANGLE_TX_MSGS(bus) \
{MSG_SUBARU_ES_LKAS_ANGLE, bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_DashStatus, bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_LKAS_State, bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Infotainment, bus, 8, .check_relay = true}, \
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \
@@ -305,8 +305,13 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static const CanMsg SUBARU_D_PLATFORM_ANGLE_TX_MSGS[] = {
SUBARU_D_PLATFORM_ANGLE_TX_MSGS()
static const CanMsg SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS[] = {
SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static const CanMsg SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS[] = {
SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_CAM_BUS)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
@@ -343,6 +348,9 @@ static safety_config subaru_init(uint16_t param) {
const uint16_t SUBARU_PARAM_D_PLATFORM = 32;
subaru_d_platform = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM);
const uint16_t SUBARU_PARAM_D_PLATFORM_CAMERA = 64;
const bool subaru_d_platform_camera = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM_CAMERA);
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -350,7 +358,8 @@ static safety_config subaru_init(uint16_t param) {
safety_config ret;
if (subaru_lkas_angle) {
ret = subaru_d_platform ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_TX_MSGS) : \
ret = subaru_d_platform ? (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS)) : \
subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
} else if (subaru_gen2) {
opendbc_repo/opendbc/safety/tests/common.py
+1 -1
View File
@@ -1033,7 +1033,7 @@ class SafetyTest(SafetyTestBase):
if attr.startswith('TestHyundaiCanfdCCNC') and current_test.startswith('TestSubaruPreglobal'):
tx = list(filter(lambda m: m[0] not in [0x161], tx))
if current_test == 'TestSubaruDPlatformAngleSafety' and attr.startswith('TestRivian'):
if current_test.startswith('TestSubaruDPlatform') and attr.startswith('TestRivian'):
tx = list(filter(lambda m: not (m[1] == 2 and m[0] in [0x321, 0x322, 0x323]), tx))
if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong',
opendbc_repo/opendbc/safety/tests/test_subaru.py
+22
View File
@@ -372,6 +372,28 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test
return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})
class TestSubaruDPlatformCameraAngleSafety(TestSubaruDPlatformAngleSafety):
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | SubaruSafetyFlags.D_PLATFORM_CAMERA
TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS],
[SubaruMsg.ES_DashStatus, SUBARU_CAM_BUS],
[SubaruMsg.ES_LKAS_State, SUBARU_CAM_BUS],
[SubaruMsg.ES_Infotainment, SUBARU_CAM_BUS],
[SubaruMsg.ES_Distance, SUBARU_ALT_BUS]]
RELAY_MALFUNCTION_ADDRS = {SUBARU_CAM_BUS: (SubaruMsg.ES_LKAS_ANGLE,
SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)}
FWD_BLACKLISTED_ADDRS = {
SUBARU_MAIN_BUS: [SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment],
}
def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
if increment_timer:
self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY))
self.angle_cmd_cnt += 1
values = {"LKAS_Output": angle, "LKAS_Request": enabled, "SET_3": 3}
return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_CAM_BUS, values)
class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = SubaruSafetyFlags.LONG | SubaruSafetyFlags.GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs()
scripts/model_compiler.py
+11 -6
View File
@@ -57,22 +57,27 @@ class _UsbdevfsControl(ctypes.Structure):
("data", ctypes.c_void_p)]
def build_compile_env() -> dict[str, str]:
def build_compile_env(*, supercombo: bool = False) -> dict[str, str]:
env = os.environ.copy()
pythonpath = env.get("PYTHONPATH", "")
env["PYTHONPATH"] = f"{REPO_ROOT}:{pythonpath}" if pythonpath else str(REPO_ROOT)
for key, default in {
existing_pythonpath = env.get("PYTHONPATH", "")
env["PYTHONPATH"] = f"{REPO_ROOT}{os.pathsep}{existing_pythonpath}" if existing_pythonpath else str(REPO_ROOT)
defaults = {} if supercombo else {
"DEBUG": "0",
"FLOAT16": "1",
"IMAGE": "2",
"JIT_BATCH_SIZE": "0",
"NOLOCALS": "1",
"OPENPILOT_HACKS": "1",
}.items():
}
for key, default in defaults.items():
try:
int(str(env.get(key)), 0)
except (TypeError, ValueError):
env[key] = default
if supercombo:
# Unified supercombo artifacts must use upstream compile defaults. The
# legacy QCOM tuning causes a reproducible HCQ timeline failure here.
env.pop("QCOM_PRIORITY", None)
return env
@@ -576,7 +581,7 @@ def compile_driving(
]
if version:
command += ["--behavior-version", version]
compile_env = build_compile_env()
compile_env = build_compile_env(supercombo=input_format == "supercombo")
if external_gpu:
for qcom_only_flag in ("IMAGE", "NOLOCALS", "OPENPILOT_HACKS"):
compile_env.pop(qcom_only_flag, None)
selfdrive/controls/lib/latcontrol_torque.py
+5
View File
@@ -522,11 +522,16 @@ class LatControlTorque(LatControl):
output_torque *= volt_plexy_center_taper
elif kia_ev6_active:
output_torque *= kia_ev6_low_speed_center_taper
output_torque *= get_kia_ev6_center_output_scale(setpoint, CS.vEgo)
elif kia_carnival_active:
output_torque *= kia_carnival_center_taper
output_torque *= get_kia_carnival_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif palisade_active:
output_torque *= get_palisade_center_output_scale(setpoint, CS.vEgo)
elif tucson_4th_gen_active:
output_torque *= tucson_4th_gen_center_taper
elif sonata_hybrid_active:
output_torque *= sonata_hybrid_center_taper
elif self.is_silverado:
output_torque *= silverado_center_taper
elif kia_niro_phev_2022_active:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+31 -2
View File
@@ -518,6 +518,11 @@ PALISADE_CENTER_TAPER_LAT = 0.28
PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_TAPER_SPEED = 12.0
PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5
PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.10
PALISADE_CENTER_OUTPUT_TAPER_LAT = 0.28
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_OUTPUT_TAPER_SPEED = 18.0
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
GENESIS_G90_FF_GAIN_LEFT = 0.32
@@ -743,7 +748,7 @@ IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5
# Newer Ioniq 6 highway center-chatter correction; activation is firmware-gated.
IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80
IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45
IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX = 0.18
IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX = 0.20
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT = 0.35
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED = 22.0
@@ -810,6 +815,11 @@ KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08
KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4
KIA_EV6_CENTER_OUTPUT_TAPER_MAX = 0.12
KIA_EV6_CENTER_OUTPUT_TAPER_LAT = 0.30
KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED = 12.0
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.4
VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7
VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 1.07
@@ -841,7 +851,7 @@ PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.06
PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.22
PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30
PRIUS_CENTER_TAPER_MAX = 0.15
PRIUS_CENTER_TAPER_MAX = 0.155
PRIUS_CENTER_TAPER_LAT = 0.24
PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035
PRIUS_CENTER_TAPER_SPEED = 18.0
@@ -2406,6 +2416,15 @@ def get_palisade_center_taper_scale(desired_lateral_accel: float, v_ego: float)
return 1.0 - (PALISADE_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_palisade_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Reduce high-speed center corrections without reducing normal turn authority."""
speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_OUTPUT_TAPER_SPEED) /
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _palisade_sigmoid((PALISADE_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - (PALISADE_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight)
def genesis_g90_lateral_testing_ground_active() -> bool:
return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID)
@@ -3070,6 +3089,15 @@ def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego
return 1.0 - reduction
def get_kia_ev6_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_OUTPUT_TAPER_SPEED) /
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH)
reduction = _flm_vehicle_knob("hyundai_kia_ev6.center_output_taper_max", KIA_EV6_CENTER_OUTPUT_TAPER_MAX) * speed_weight * center_weight
return 1.0 - reduction
def volt_plexy_lateral_testing_ground_active() -> bool:
return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID)
@@ -3431,6 +3459,7 @@ FLM_SUPPORTED_VEHICLE_KNOBS = {
"hyundai_kia_ev6.unwind_threshold_increase_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT},
"hyundai_kia_ev6.unwind_threshold_increase_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT},
"hyundai_kia_ev6.center_friction_threshold_gain": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_FRICTION_THRESHOLD_GAIN},
"hyundai_kia_ev6.center_output_taper_max": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_OUTPUT_TAPER_MAX},
"toyota_prius.ff_gain_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_FF_GAIN_LEFT},
"toyota_prius.ff_gain_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_FF_GAIN_RIGHT},
"toyota_prius.turn_in_boost_left": {"profile": "toyota_prius", "min": -0.10, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_BOOST_LEFT},
selfdrive/controls/tests/test_conditional_experimental_mode.py
+53 -1
View File
@@ -82,12 +82,62 @@ def make_update_toggles():
conditional_curves=False,
conditional_curves_lead=False,
conditional_lead=False,
conditional_open_road=False,
conditional_model_stop_time=7.0,
conditional_slower_lead=False,
conditional_stopped_lead=False,
)
def test_open_road_triggers_near_set_speed_without_a_lead():
cem = make_cem(model_length=80.0)
toggles = make_update_toggles()
toggles.conditional_open_road = True
triggered = cem.check_conditions(
55 * CV.MPH_TO_MS,
make_sm(),
toggles,
v_cruise=57 * CV.MPH_TO_MS,
)
assert triggered
assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"]
def test_open_road_requires_at_or_below_set_speed_within_margin():
toggles = make_update_toggles()
toggles.conditional_open_road = True
too_far_below = make_cem(model_length=80.0)
above_set_speed = make_cem(model_length=80.0)
assert not too_far_below.check_conditions(
50 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
)
assert not above_set_speed.check_conditions(
58 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
)
def test_open_road_requires_a_set_speed():
cem = make_cem(model_length=80.0)
toggles = make_update_toggles()
toggles.conditional_open_road = True
assert not cem.check_conditions(55 * CV.MPH_TO_MS, make_sm(), toggles)
def test_open_road_requires_no_lead_vehicle():
cem = make_cem(model_length=80.0, lead_status=True, lead_d_rel=60.0, lead_v_lead=55 * CV.MPH_TO_MS)
toggles = make_update_toggles()
toggles.conditional_open_road = True
assert not cem.check_conditions(
55 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
)
def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped():
v_ego = 10 * CV.MPH_TO_MS
model_length = v_ego * 10.0
@@ -1083,8 +1133,9 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
seen = {}
def cem_update(v_ego, sm, starpilot_toggles):
def cem_update(v_ego, sm, starpilot_toggles, v_cruise):
seen.update({
"v_cruise": v_cruise,
"tracking_lead": planner.tracking_lead,
"following_lead": planner.starpilot_following.following_lead,
"slower_lead": planner.starpilot_following.slower_lead,
@@ -1129,6 +1180,7 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
planner.update(0.0, False, sm, starpilot_toggles)
assert seen == {
"v_cruise": 50.0 * CV.KPH_TO_MS,
"tracking_lead": True,
"following_lead": True,
"slower_lead": True,
selfdrive/controls/tests/test_latcontrol.py
+73
View File
@@ -30,6 +30,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
get_kona_non_scc_center_taper_scale,
get_kona_non_scc_friction_threshold,
get_kona_non_scc_highway_transition_output_scale,
get_kia_ev6_center_output_scale,
get_prius_center_taper_scale,
KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT,
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT,
RAM_1500_MAX_LAT_JERK_UP,
@@ -68,6 +70,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_genesis_gv70_friction_threshold,
get_elantra_non_scc_ff_scale,
get_palisade_ff_scale,
get_palisade_center_output_scale,
get_palisade_center_taper_scale,
get_palisade_friction_scale,
get_palisade_friction_threshold,
@@ -497,6 +500,26 @@ class TestLatControl:
assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0)
assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.20, 30.0) <= 1.0
def test_sonata_hybrid_center_taper_applies_to_output(self, monkeypatch):
monkeypatch.setattr(latcontrol_torque, "get_sonata_hybrid_ff_scale", lambda *_args: 0.0)
monkeypatch.setattr(latcontrol_torque, "get_sonata_hybrid_center_taper_scale", lambda *_args: 1.0)
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_SONATA_HYBRID)
CS.vEgo = 3.0
base_output, _, _ = controller.update(True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles)
monkeypatch.setattr(latcontrol_torque, "get_sonata_hybrid_center_taper_scale", lambda *_args: 0.5)
tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller(
HYUNDAI.HYUNDAI_SONATA_HYBRID,
)
tapered_CS.vEgo = 3.0
tapered_output, _, _ = tapered_controller.update(
True, tapered_CS, tapered_VM, tapered_params, False, 0.0002, False, 0.2, None, None, tapered_toggles,
)
assert controller.is_sonata_hybrid
assert base_output != 0.0
assert tapered_output == pytest.approx(base_output * 0.5)
def test_sonata_ff_scale_curve(self):
assert get_sonata_ff_scale(0.0, 0.0, 20.0) == 1.0
steady_left = get_sonata_ff_scale(0.45, 0.0, 8.0)
@@ -667,6 +690,15 @@ class TestLatControl:
assert get_palisade_center_taper_scale(0.0, 25.0) < get_palisade_center_taper_scale(0.28, 25.0)
assert get_palisade_center_taper_scale(0.28, 25.0) < get_palisade_center_taper_scale(0.6, 25.0)
def test_palisade_center_output_taper_curve(self):
low_speed_center = get_palisade_center_output_scale(0.0, 8.0)
highway_center = get_palisade_center_output_scale(0.0, 30.0)
highway_turn = get_palisade_center_output_scale(0.45, 30.0)
assert low_speed_center > highway_center
assert highway_center < highway_turn <= 1.0
assert highway_center > 0.89
def test_prius_ff_scale_curve(self):
assert get_prius_ff_scale(0.0, 0.0, 20.0) == 1.0
steady_left = get_prius_ff_scale(0.7, 0.0, 8.0)
@@ -710,6 +742,8 @@ class TestLatControl:
assert get_prius_friction_jerk_deadzone(30.0, 0.0) > get_prius_friction_jerk_deadzone(30.0, 0.8)
assert get_prius_friction_jerk_deadzone(8.0, 0.0) < 0.05
assert get_prius_center_taper_scale(0.0, 30.0) < get_prius_center_taper_scale(0.8, 30.0)
assert get_prius_center_taper_scale(0.0, 8.0) > 0.99
assert get_prius_high_speed_output_taper_scale(30.0, 0.0) > get_prius_high_speed_output_taper_scale(30.0, 0.8)
assert get_prius_high_speed_output_taper_scale(15.0, 0.8) > 0.99
@@ -1237,6 +1271,26 @@ class TestLatControl:
assert lac_log.active
assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 0.98)
def test_palisade_center_output_taper_update_path(self, monkeypatch):
monkeypatch.setattr(latcontrol_torque, "get_palisade_center_output_scale", lambda *_args: 1.0)
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_PALISADE_2023)
CS.vEgo = 30.0
base_output, _, _ = controller.update(
True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles,
)
monkeypatch.setattr(latcontrol_torque, "get_palisade_center_output_scale", lambda *_args: 0.5)
tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller(
HYUNDAI.HYUNDAI_PALISADE_2023,
)
tapered_CS.vEgo = 30.0
tapered_output, _, _ = tapered_controller.update(
True, tapered_CS, tapered_VM, tapered_params, False, 0.0025, False, 0.2, None, None, tapered_toggles,
)
assert base_output != 0.0
assert tapered_output == pytest.approx(base_output * 0.5)
def test_sonata_default_update_path(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_SONATA)
CarInterface = interfaces[HYUNDAI.HYUNDAI_SONATA]
@@ -1652,6 +1706,25 @@ class TestLatControl:
assert get_kia_ev6_center_taper_scale(0.0, 25.0) < get_kia_ev6_center_taper_scale(0.0, 10.0)
assert get_kia_ev6_center_taper_scale(0.0, 25.0) < get_kia_ev6_center_taper_scale(0.20, 25.0) <= 1.0
def test_kia_ev6_center_output_taper_curve(self):
assert get_kia_ev6_center_output_scale(0.0, 10.0) > get_kia_ev6_center_output_scale(0.0, 20.0)
assert get_kia_ev6_center_output_scale(0.0, 20.0) < get_kia_ev6_center_output_scale(0.5, 20.0)
assert get_kia_ev6_center_output_scale(0.0, 20.0) > 0.87
def test_kia_ev6_center_output_taper_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6)
base_output, _, _ = controller.update(
True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles,
)
monkeypatch.setattr(latcontrol_torque, "get_kia_ev6_center_output_scale", lambda *_args: 0.5)
tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6)
tapered_output, _, _ = tapered_controller.update(
True, tapered_CS, tapered_VM, tapered_params, False, 0.0025, False, 0.2, None, None, tapered_toggles,
)
assert abs(tapered_output) < abs(base_output)
def test_volt_plexy_testing_ground_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_VOLT_CC)
monkeypatch.setattr(latcontrol_torque, "volt_plexy_lateral_testing_ground_active", lambda: True)
starpilot/common/safe_mode.py
+1
View File
@@ -117,6 +117,7 @@ SAFE_MODE_MANAGED_KEYS = (
"CELead",
"CESlowerLead",
"CEStoppedLead",
"CEOpenRoad",
"CESpeed",
"CESpeedLead",
"CCMLead",
starpilot/common/starpilot_variables.py
+1
View File
@@ -825,6 +825,7 @@ class StarPilotVariables:
toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode)
toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves)
toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode)
toggle.conditional_open_road = self.get_value("CEOpenRoad", condition=toggle.conditional_experimental_mode)
toggle.conditional_slower_lead = self.get_value("CESlowerLead", condition=toggle.conditional_lead)
toggle.conditional_stopped_lead = self.get_value("CEStoppedLead", condition=toggle.conditional_lead)
toggle.conditional_limit = self.get_value("CESpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion)
starpilot/controls/lib/conditional_experimental_mode.py
+30 -3
View File
@@ -5,6 +5,7 @@ import numpy as np
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.common.constants import CV
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
from openpilot.starpilot.common.experimental_state import (
CEStatus,
@@ -63,6 +64,9 @@ class ConditionalExperimentalMode:
POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0
TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS
TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0
# Keep EXP available near the set speed on an empty road, where it can
# anticipate braking sooner. Stay at or below the set speed for safety.
OPEN_ROAD_SET_SPEED_MARGIN = 3.0 * CV.MPH_TO_MS
STOP_LIGHT_FILTER_TIME_OVERRIDES = {
"HYUNDAI_ELANTRA_2021": 0.25,
@@ -118,7 +122,7 @@ class ConditionalExperimentalMode:
self.standstill_stop_release_pending = False
self.post_stop_launch_trigger_suppress_until = 0.0
def update(self, v_ego, sm, starpilot_toggles):
def update(self, v_ego, sm, starpilot_toggles, v_cruise=None):
now = time.monotonic()
standstill = bool(sm["carState"].standstill)
current_standstill_stop_hold = False
@@ -141,7 +145,7 @@ class ConditionalExperimentalMode:
if not is_manual_ce_status(self.status_value) and not standstill:
self.update_conditions(v_ego, sm, starpilot_toggles)
triggered = self.check_conditions(v_ego, sm, starpilot_toggles)
triggered = self.check_conditions(v_ego, sm, starpilot_toggles, v_cruise)
if triggered:
self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME
self.mode_false_since = 0.0
@@ -252,7 +256,7 @@ class ConditionalExperimentalMode:
return bool(self.stop_light_detected or force_stop_active or model_stopped)
def check_conditions(self, v_ego, sm, starpilot_toggles):
def check_conditions(self, v_ego, sm, starpilot_toggles, v_cruise=None):
launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until
below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead
below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead
@@ -260,6 +264,29 @@ class ConditionalExperimentalMode:
self.status_value = CEStatus["SPEED"]
return True
lead = self.starpilot_planner.lead_one
cruise_speed = getattr(sm["carState"], "vCruise", None)
cruise_speed_available = cruise_speed is None or cruise_speed != V_CRUISE_UNSET
no_lead = bool(
not getattr(lead, "status", False) and
not getattr(self.starpilot_planner, "tracking_lead", False) and
not self.starpilot_planner.starpilot_following.following_lead
)
set_speed_error = float(v_cruise) - float(v_ego) if v_cruise is not None else -1.0
open_road = bool(
not launch_trigger_suppressed and
getattr(starpilot_toggles, "conditional_open_road", False) and
v_cruise is not None and
cruise_speed_available and
float(v_cruise) > 0.0 and
1.0 <= v_ego <= float(v_cruise) and
set_speed_error <= self.OPEN_ROAD_SET_SPEED_MARGIN and
no_lead
)
if open_road:
self.status_value = CEStatus["SPEED"]
return True
desired_lane = self.starpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.starpilot_planner.lane_width_right
lane_available = desired_lane >= starpilot_toggles.lane_detection_width or not starpilot_toggles.conditional_signal_lane_detection
if v_ego < starpilot_toggles.conditional_signal and (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and not lane_available:
starpilot/controls/starpilot_planner.py
+1 -1
View File
@@ -222,7 +222,7 @@ class StarPilotPlanner:
conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)):
# Keep CEM's filters warm in AOL so engagement can inherit the current scene.
self.starpilot_cem.update(v_ego, sm, starpilot_toggles)
self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise)
self.starpilot_ccm.experimental_mode = True
elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_chill_mode", False)):
self.starpilot_ccm.update(v_ego, v_cruise, sm, starpilot_toggles)
starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json
+10
View File
@@ -688,6 +688,15 @@
"is_parent_toggle": true,
"settings_tier": "simple"
},
{
"key": "CEOpenRoad",
"label": "Open Road",
"description": "Keep Experimental Mode active on an open road when driving within 3 mph below the set speed and no lead vehicle is detected. This can help the model anticipate braking sooner.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CESlowerLead",
"label": "Slower Lead",
@@ -714,6 +723,7 @@
"ui_type": "numeric",
"min": 0.0,
"max": 9.0,
"step": 0.1,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
starpilot/system/the_galaxy/tests/test_device_settings_layout.py
+2
View File
@@ -133,6 +133,7 @@ def test_requested_simple_and_advanced_settings_tiers():
"QOLLongitudinal",
):
assert longitudinal[key]["settings_tier"] == "simple"
assert sections["Longitudinal (Speed & Following)"]["CEOpenRoad"]["settings_tier"] == "simple"
for key in (
"AdvancedLongitudinalTune",
"CustomPersonalities",
@@ -156,6 +157,7 @@ def test_hidden_feature_defaults_remain_enabled():
assert _declared_default("NavDesiresAllowed") == "1"
assert _declared_default("NavLanePositioningAllowed") == "0"
assert _declared_default("NavLongitudinalAllowed") == "1"
assert _declared_default("CEOpenRoad") == "0"
for key in (
"TrafficPersonalityProfile",
starpilot/system/the_galaxy/the_galaxy.py
+1
View File
@@ -951,6 +951,7 @@ _TROUBLESHOOT_CEM_KEYS = [
"CELead",
"CESlowerLead",
"CEStoppedLead",
"CEOpenRoad",
"CEModelStopTime",
"CESignalSpeed",
"ShowCEMStatus",
tools/StarPilot/generate_galaxy_layout.py
+10
View File
@@ -25,6 +25,16 @@ DROPDOWN_MAPPING = {
# Custom controls implemented outside the tuple vectors in Qt settings panels.
# Inject these so regenerated galaxy layouts retain equivalent functionality.
INJECTED_SECTION_PARAMS = {
"Longitudinal (Speed & Following)": [
{
"key": "CEOpenRoad",
"label": "Open Road",
"description": "Keep Experimental Mode active on an open road when driving within 3 mph below the set speed and no lead vehicle is detected. This can help the model anticipate braking sooner.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
},
],
"Vehicle": [
{
"key": "CarMake",