diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4394855e0..bd61c584f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -252,7 +252,7 @@ class LatControlTorque(LatControl): kia_xceed_active = self.is_kia_xceed kia_niro_phev_2022_active = self.is_kia_niro_phev_2022 kia_forte_active = self.is_kia_forte - kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() + kia_ev6_active = self.is_kia_ev6 kia_carnival_active = self.is_kia_carnival tucson_4th_gen_active = self.is_tucson_4th_gen volt_plexy_test_active = self.is_volt_standard and volt_plexy_lateral_testing_ground_active() @@ -267,8 +267,8 @@ class LatControlTorque(LatControl): kia_xceed_center_taper = get_kia_xceed_center_taper_scale(setpoint, CS.vEgo) if kia_xceed_active else 1.0 kia_niro_phev_2022_center_taper = get_kia_niro_phev_2022_center_taper_scale(setpoint, CS.vEgo) if kia_niro_phev_2022_active else 1.0 kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 - kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 - kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 + kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_active else 1.0 + kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_active else 1.0 kia_carnival_center_taper = get_kia_carnival_center_taper_scale(setpoint, CS.vEgo) if kia_carnival_active else 1.0 tucson_4th_gen_center_taper = get_tucson_4th_gen_center_taper_scale(setpoint, CS.vEgo) if tucson_4th_gen_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 @@ -342,7 +342,7 @@ class LatControlTorque(LatControl): elif kia_forte_active: ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper friction_threshold = get_kia_forte_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) - elif kia_ev6_test_active: + elif kia_ev6_active: ff *= get_kia_ev6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_ev6_center_taper friction_threshold = get_kia_ev6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_kia_ev6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) @@ -410,13 +410,15 @@ class LatControlTorque(LatControl): if ioniq_6_active: output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo) output_torque *= get_ioniq_6_highway_transition_output_taper_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 prius_active: output_torque *= prius_center_taper elif volt_standard_test_active: output_torque *= volt_standard_center_taper elif volt_plexy_test_active: output_torque *= volt_plexy_center_taper - elif kia_ev6_test_active: + elif kia_ev6_active: output_torque *= kia_ev6_low_speed_center_taper elif kia_carnival_active: output_torque *= kia_carnival_center_taper diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 154507d4a..cc2b5808b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -350,12 +350,12 @@ KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0 KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.24 KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.34 -TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.36 +TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44 TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28 TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.055 TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX = 14.0 TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 1.5 -TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.22 +TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.28 KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 KIA_FORTE_FF_REDUCTION_LEFT = 0.05 @@ -721,6 +721,8 @@ RAV4_PRIME_UNWIND_FF_REDUCTION_LEFT = 0.15 RAV4_PRIME_UNWIND_FF_REDUCTION_RIGHT = 0.13 RAV4_PRIME_UNWIND_FRICTION_REDUCTION_LEFT = 0.16 RAV4_PRIME_UNWIND_FRICTION_REDUCTION_RIGHT = 0.14 +RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_LEFT = 0.12 +RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_RIGHT = 0.14 RAV4_PRIME_FRICTION_THRESHOLD_GAIN = 0.24 RAV4_PRIME_FRICTION_CENTER_LAT = 0.30 RAV4_PRIME_FRICTION_CENTER_LAT_WIDTH = 0.07 @@ -1019,6 +1021,14 @@ def get_rav4_prime_friction_scale(v_ego: float, desired_lateral_accel: float, de _rav4_prime_speed_weight(v_ego)) +def get_rav4_prime_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + reduction = _rav4_prime_side_value(desired_lateral_accel, + RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_LEFT, + RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_RIGHT) + return 1.0 - (reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) * + _rav4_prime_speed_weight(v_ego)) + + def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index cb90c589d..b1d2d4b7b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -60,6 +60,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_rav4_prime_ff_scale, get_rav4_prime_friction_scale, get_rav4_prime_friction_threshold, + get_rav4_prime_output_taper_scale, get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, @@ -593,14 +594,32 @@ class TestLatControl: assert get_rav4_prime_friction_scale(13.0, 1.0, 0.8) == pytest.approx(1.0) assert get_rav4_prime_friction_scale(13.0, 1.0, -0.8) < 1.0 - def test_rav4_prime_forced_torque_update_path(self): + def test_rav4_prime_output_taper_only_targets_unwind(self): + assert get_rav4_prime_output_taper_scale(1.0, 0.8, 13.0) == pytest.approx(1.0) + assert get_rav4_prime_output_taper_scale(-1.0, -0.8, 13.0) == pytest.approx(1.0) + + left_unwind = get_rav4_prime_output_taper_scale(1.0, -0.8, 13.0) + right_unwind = get_rav4_prime_output_taper_scale(-1.0, 0.8, 13.0) + assert right_unwind < left_unwind < 1.0 + assert get_rav4_prime_output_taper_scale(-1.0, 0.8, 25.0) > right_unwind + + 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 + base_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) - _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + monkeypatch.setattr(latcontrol_torque, "get_rav4_prime_output_taper_scale", lambda *_args: 0.5) + tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller( + TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True, + ) + tapered_CS.vEgo = 13.0 + tapered_output, _, _ = tapered_controller.update( + True, tapered_CS, tapered_VM, tapered_params, False, 0.0025, False, 0.2, None, None, tapered_toggles, + ) assert controller.is_rav4_prime assert lac_log.active + assert tapered_output == pytest.approx(base_output * 0.5) def test_ioniq_5_center_taper_curve(self): assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0) @@ -882,7 +901,7 @@ class TestLatControl: low_speed_turn = get_tucson_4th_gen_friction_threshold(8.5, 0.50) high_speed_center = get_tucson_4th_gen_friction_threshold(20.0, 0.0) - assert low_speed_center == pytest.approx(base * 1.22, rel=0.01) + assert low_speed_center == pytest.approx(base * 1.28, rel=0.01) assert low_speed_turn == pytest.approx(base, rel=0.01) assert high_speed_center == pytest.approx(get_hkg_canfd_base_friction_threshold(20.0), rel=0.01) @@ -1082,13 +1101,21 @@ class TestLatControl: assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0) assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.35, 25.0) <= 1.0 - def test_kia_ev6_testing_ground_update_path(self, monkeypatch): + def test_kia_ev6_default_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6) - monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True) + calls = 0 + + def record_ff_scale(*_args): + nonlocal calls + calls += 1 + return 1.0 + + monkeypatch.setattr(latcontrol_torque, "get_kia_ev6_ff_scale", record_ff_scale) _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) assert lac_log.active + assert calls == 1 def test_kia_ev6_ff_scale_curve(self): assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0