From 14cde4a64c7f2dec2239d6213cd7144f9d37ed93 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 4 May 2026 00:00:28 -0500 Subject: [PATCH] You could be drivin a honda --- selfdrive/controls/lib/latcontrol_torque.py | 81 +++++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 29 ++++++++ 2 files changed, 110 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1f274d8c5..204f41853 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -47,6 +47,22 @@ UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 +CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 +CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.10 +CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 +CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07 +CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.00 +CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH = 0.30 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.11 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.08 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.08 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.10 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.22 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.24 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.05 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.15 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.18 BOLT_2022_2023_CARS = ( GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, @@ -325,6 +341,69 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") +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) + + +def _civic_bosch_modified_b_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE) + + +def _civic_bosch_modified_b_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) + cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) + base_reduction = _civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT) * onset * cutoff + + phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego) + + turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.40 + 0.60 * low_speed_factor)) + unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) + cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) + envelope = onset * cutoff * _civic_bosch_modified_b_low_speed_factor(v_ego) + phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + + friction_scale = 1.0 + friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) + return min(max(friction_scale, 0.82), 1.06) + + def bolt_2017_lateral_testing_ground_active() -> bool: return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID) @@ -1101,7 +1180,9 @@ 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 and civic_bosch_modified_lateral_testing_ground_active(): + ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD + friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) 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: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 4a5405586..2f08550fb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -21,6 +21,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import ( ) from openpilot.selfdrive.controls.lib.latcontrol_torque import ( LatControlTorque, + get_civic_bosch_modified_b_ff_scale, + get_civic_bosch_modified_b_friction_scale, get_bolt_2017_center_taper_scale, get_friction_threshold, get_bolt_2017_base_torque_scale, @@ -412,6 +414,33 @@ class TestLatControl: assert captured["threshold"] == pytest.approx(0.3) + def test_modified_civic_b_torque_ff_scale_curve(self): + steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0) + steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0) + turn_in_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.8, 12.0) + turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) + unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0) + unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + + assert steady_left < 1.0 + assert steady_right < 1.0 + assert steady_left < steady_right + assert turn_in_left > steady_left + assert turn_in_right > steady_right + assert unwind_left < steady_left + assert unwind_right < steady_right + + def test_modified_civic_b_torque_friction_scale_curve(self): + turn_in_left = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, 0.8) + turn_in_right = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, -0.8) + unwind_left = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, -0.8) + unwind_right = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) + + assert turn_in_left > 1.0 + assert turn_in_right > turn_in_left + assert unwind_left < 1.0 + assert unwind_right < unwind_left + 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)