diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f5682d566..50b0b2cf5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -76,6 +76,11 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED = 18.0 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH = 2.5 CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50 CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00 @@ -433,6 +438,15 @@ def civic_bosch_modified_a_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "A") +def get_civic_bosch_modified_a_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED) / + CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH) + reduction = CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def _civic_bosch_modified_b_low_speed_factor(v_ego: float) -> float: return 1.0 / (1.0 + (max(v_ego, 0.0) / CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED) ** 2) @@ -1450,6 +1464,9 @@ class LatControlTorque(LatControl): volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0 ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0 sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0 + civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( + self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() + ) else 1.0 friction_threshold = get_friction_threshold(CS.vEgo) friction_scale = 1.0 if bolt_2022_2023_tuned_path_active: @@ -1488,9 +1505,10 @@ class LatControlTorque(LatControl): friction_threshold = get_volt_plexy_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_volt_plexy_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif self.is_civic_bosch_modified: - ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * civic_bosch_modified_a_center_taper friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: @@ -1510,6 +1528,8 @@ class LatControlTorque(LatControl): output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif volt_standard_test_active: output_torque *= volt_standard_center_taper + elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active(): + output_torque *= civic_bosch_modified_a_center_taper pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0d98b199f..db7737a94 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import ( get_civic_bosch_modified_pid_output_scale, ) from openpilot.selfdrive.controls.lib.latcontrol_torque import ( + get_civic_bosch_modified_a_center_taper_scale, LatControlTorque, get_civic_bosch_modified_b_ff_scale, get_civic_bosch_modified_b_friction_scale, @@ -588,6 +589,10 @@ class TestLatControl: assert a_variant_unwind_right < base_unwind_right assert a_variant_unwind_right_friction < base_unwind_right_friction + def test_modified_civic_a_variant_center_taper_curve(self): + 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): 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)