diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ca7a51676..890b64ea5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index d98cac584..dfc09a4ad 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -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) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index ebae574ea..65ba6d0f7 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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 diff --git a/starpilot/system/the_galaxy/flm_workspace.py b/starpilot/system/the_galaxy/flm_workspace.py index ab7032936..e15a792a9 100644 --- a/starpilot/system/the_galaxy/flm_workspace.py +++ b/starpilot/system/the_galaxy/flm_workspace.py @@ -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 ""))