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

oh jonny boi

Browse Source
This commit is contained in:
firestar5683
2026-08-02 18:05:56 -05:00
parent dbe927c629
commit 2833fdc54d
4 changed files with 97 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+7
View File
@@ -88,6 +88,7 @@ class LatControlTorque(LatControl):
self.is_palisade = CP.carFingerprint in PALISADE_CARS
self.is_prius = CP.carFingerprint in PRIUS_CARS
self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
self.is_sienna_4th_gen = CP.carFingerprint in SIENNA_4TH_GEN_CARS
self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS
self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
@@ -256,6 +257,7 @@ class LatControlTorque(LatControl):
palisade_active = self.is_palisade
prius_active = self.is_prius
rav4_prime_active = self.is_rav4_prime
sienna_4th_gen_active = self.is_sienna_4th_gen
lexus_is_active = self.is_lexus_is
ioniq_5_active = self.is_ioniq_5
ioniq_ev_old_active = self.is_ioniq_ev_old
@@ -327,6 +329,9 @@ class LatControlTorque(LatControl):
ff *= get_rav4_prime_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_rav4_prime_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_rav4_prime_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
elif sienna_4th_gen_active:
ff *= get_sienna_4th_gen_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_sienna_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
elif lexus_is_active:
ff *= get_lexus_is_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif ioniq_5_active:
@@ -438,6 +443,8 @@ class LatControlTorque(LatControl):
output_torque *= get_kona_non_scc_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif rav4_prime_active:
output_torque *= get_rav4_prime_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif sienna_4th_gen_active:
output_torque *= get_sienna_4th_gen_center_taper_scale(setpoint, CS.vEgo)
elif prius_active:
output_torque *= prius_center_taper
elif volt_standard_test_active:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+68 -2
View File
@@ -144,6 +144,10 @@ RAV4_PRIME_CARS = (
TOYOTA_CAR.TOYOTA_RAV4_PRIME,
)
SIENNA_4TH_GEN_CARS = (
TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN,
)
LEXUS_IS_CARS = (
TOYOTA_CAR.LEXUS_IS,
)
@@ -798,6 +802,27 @@ RAV4_PRIME_SPEED_ONSET_WIDTH = 1.5
RAV4_PRIME_SPEED_MAX = 20.0
RAV4_PRIME_SPEED_MAX_WIDTH = 2.5
SIENNA_4TH_GEN_PHASE_SCALE = 0.12
SIENNA_4TH_GEN_TURN_IN_FF_BOOST = 0.08
SIENNA_4TH_GEN_TURN_IN_LAT = 0.24
SIENNA_4TH_GEN_TURN_IN_LAT_WIDTH = 0.08
SIENNA_4TH_GEN_TURN_IN_SPEED_ONSET = 3.0
SIENNA_4TH_GEN_TURN_IN_SPEED_WIDTH = 1.5
SIENNA_4TH_GEN_TURN_IN_SPEED_MAX = 14.0
SIENNA_4TH_GEN_TURN_IN_SPEED_MAX_WIDTH = 2.0
SIENNA_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.30
SIENNA_4TH_GEN_FRICTION_CENTER_LAT = 0.30
SIENNA_4TH_GEN_FRICTION_CENTER_LAT_WIDTH = 0.07
SIENNA_4TH_GEN_FRICTION_SPEED_ONSET = 3.0
SIENNA_4TH_GEN_FRICTION_SPEED_WIDTH = 1.5
SIENNA_4TH_GEN_FRICTION_SPEED_MAX = 14.0
SIENNA_4TH_GEN_FRICTION_SPEED_MAX_WIDTH = 2.0
SIENNA_4TH_GEN_CENTER_TAPER_MAX = 0.12
SIENNA_4TH_GEN_CENTER_TAPER_LAT = 0.20
SIENNA_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.06
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_MAX = 13.0
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 2.0
LEXUS_IS_PHASE_SCALE = 0.10
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.06
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.12
@@ -1144,6 +1169,47 @@ def get_rav4_prime_output_taper_scale(desired_lateral_accel: float, desired_late
_rav4_prime_speed_weight(v_ego))
def _sienna_4th_gen_speed_weight(v_ego: float) -> float:
onset = _sigmoid((v_ego - SIENNA_4TH_GEN_TURN_IN_SPEED_ONSET) / SIENNA_4TH_GEN_TURN_IN_SPEED_WIDTH)
cutoff = _sigmoid((SIENNA_4TH_GEN_TURN_IN_SPEED_MAX - v_ego) / SIENNA_4TH_GEN_TURN_IN_SPEED_MAX_WIDTH)
return onset * cutoff
def _sienna_4th_gen_turn_in_weight(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / SIENNA_4TH_GEN_PHASE_SCALE)
lat_weight = _sigmoid((abs(desired_lateral_accel) - SIENNA_4TH_GEN_TURN_IN_LAT) /
SIENNA_4TH_GEN_TURN_IN_LAT_WIDTH)
return max(phase, 0.0) * lat_weight
def get_sienna_4th_gen_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
return 1.0 + (SIENNA_4TH_GEN_TURN_IN_FF_BOOST *
_sienna_4th_gen_turn_in_weight(desired_lateral_accel, desired_lateral_jerk) *
_sienna_4th_gen_speed_weight(v_ego))
def get_sienna_4th_gen_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
center_weight = _sigmoid((SIENNA_4TH_GEN_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
SIENNA_4TH_GEN_FRICTION_CENTER_LAT_WIDTH)
onset = _sigmoid((v_ego - SIENNA_4TH_GEN_FRICTION_SPEED_ONSET) / SIENNA_4TH_GEN_FRICTION_SPEED_WIDTH)
cutoff = _sigmoid((SIENNA_4TH_GEN_FRICTION_SPEED_MAX - v_ego) / SIENNA_4TH_GEN_FRICTION_SPEED_MAX_WIDTH)
return get_standard_friction_threshold(v_ego) * (
1.0 + SIENNA_4TH_GEN_FRICTION_THRESHOLD_GAIN * center_weight * onset * cutoff
)
def get_sienna_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((SIENNA_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) /
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((SIENNA_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SIENNA_4TH_GEN_CENTER_TAPER_LAT_WIDTH)
return 1.0 - (SIENNA_4TH_GEN_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
@@ -3137,11 +3203,11 @@ def get_flm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool =
dedicated_friction = car_fingerprint in (
set(BOLT_2022_2023_CARS) | set(BOLT_2018_2021_CARS) | set(VOLT_STANDARD_CARS) | set(PALISADE_CARS) |
set(PRIUS_CARS) | set(RAV4_PRIME_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) |
set(PRIUS_CARS) | set(RAV4_PRIME_CARS) | set(SIENNA_4TH_GEN_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) |
set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_CARNIVAL_CARS) | set(GENESIS_G90_CARS)
)
dedicated_center_taper = car_fingerprint in (
set(PRIUS_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) |
set(PRIUS_CARS) | set(SIENNA_4TH_GEN_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) |
set(IONIQ_EV_OLD_CARS) | set(IONIQ_6_CARS) | set(SONATA_CARS) | set(SONATA_HYBRID_CARS) |
set(KIA_XCEED_CARS) | set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_FORTE_CARS) | set(KIA_EV6_CARS) |
set(KIA_CARNIVAL_CARS) | set(TUCSON_4TH_GEN_CARS) | set(SILVERADO_CARS)
selfdrive/controls/tests/test_latcontrol.py
+21
View File
@@ -70,6 +70,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_rav4_prime_friction_scale,
get_rav4_prime_friction_threshold,
get_rav4_prime_output_taper_scale,
get_sienna_4th_gen_center_taper_scale,
get_sienna_4th_gen_ff_scale,
get_sienna_4th_gen_friction_threshold,
get_lexus_is_ff_scale,
get_ioniq_5_ff_scale,
get_ioniq_5_friction_scale,
@@ -711,6 +714,24 @@ class TestLatControl:
assert hard_right_unwind < right_unwind - 0.01
assert get_rav4_prime_output_taper_scale(-1.0, 0.8, 25.0) > right_unwind
def test_sienna_4th_gen_turn_in_and_center_shaping(self):
steady = get_sienna_4th_gen_ff_scale(0.8, 0.0, 9.0)
turn_in = get_sienna_4th_gen_ff_scale(0.8, 0.8, 9.0)
high_speed = get_sienna_4th_gen_ff_scale(0.8, 0.8, 25.0)
assert turn_in > steady >= 1.0
assert high_speed < turn_in
base = get_standard_friction_threshold(9.0)
center = get_sienna_4th_gen_friction_threshold(9.0, 0.0)
turn = get_sienna_4th_gen_friction_threshold(9.0, 0.8)
assert center > turn >= base
calm = get_sienna_4th_gen_center_taper_scale(0.0, 8.0)
turn_taper = get_sienna_4th_gen_center_taper_scale(0.8, 8.0)
fast = get_sienna_4th_gen_center_taper_scale(0.0, 25.0)
assert calm < turn_taper <= 1.0
assert fast > calm
def test_rav4_prime_forced_torque_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True)
CS.vEgo = 13.0
starpilot/system/the_galaxy/flm_workspace.py
+1 -1
View File
@@ -3008,7 +3008,7 @@ def _apply_param_bundle(params: Params, bundle: dict[str, Any]) -> None:
elif kind == "float":
params.put_float(key, float(value))
elif kind == "json":
params.put(key, normalize_flm_overrides(value))
params.put(key, json.dumps(normalize_flm_overrides(value), separators=(",", ":")))
elif kind == "string":
params.put(key, str(value or ""))