diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8390155a..44dd8125 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -18,10 +18,10 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol_torque import ( BOLT_2018_2021_STEER_RATIO_TEST_SCALE, - BOLT_2017_STEER_RATIO_TEST_SCALE, LatControlTorque, bolt_2018_2021_lateral_testing_ground_active, bolt_2017_lateral_testing_ground_active, + get_bolt_2017_steer_ratio_scale, ) from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS @@ -100,7 +100,7 @@ class Controls: x = max(lp.stiffnessFactor, 0.1) sr = max(lp.steerRatio, 0.1) if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017 and bolt_2017_lateral_testing_ground_active(): - sr *= BOLT_2017_STEER_RATIO_TEST_SCALE + sr *= get_bolt_2017_steer_ratio_scale(CS.vEgo) elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021 and bolt_2018_2021_lateral_testing_ground_active(): sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE self.VM.update_params(x, sr) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 72a6dff3..a637b05e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -60,6 +60,8 @@ BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 +BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS +BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5] BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.065, 1.060, 1.055, 1.045] BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.020, 0.995, 0.985] @@ -135,6 +137,15 @@ def bolt_2017_lateral_testing_ground_active() -> bool: return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID) +def _bolt_2017_sigmoid(x: float) -> float: + return 1.0 / (1.0 + math.exp(-x)) + + +def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float: + onset = _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) + return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * onset) + + def _bolt_2017_low_speed_factor(v_ego: float) -> float: return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2017_TRANSITION_SPEED) ** 2) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index a9b3000c..a04047d3 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -16,6 +16,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( LatControlTorque, get_friction_threshold, get_bolt_2017_base_torque_scale, + get_bolt_2017_steer_ratio_scale, get_bolt_2017_torque_scale, get_bolt_2022_2023_ff_scale, get_bolt_2022_2023_friction_scale, @@ -57,6 +58,9 @@ class TestLatControl: assert get_bolt_2017_base_torque_scale(0.5) > get_bolt_2017_base_torque_scale(-0.5) assert 1.0 < get_bolt_2017_base_torque_scale(1.2) < get_bolt_2017_base_torque_scale(0.5) assert get_bolt_2017_base_torque_scale(-2.5) < 1.0 + assert 1.0 < get_bolt_2017_steer_ratio_scale(10.0 * 0.44704) < get_bolt_2017_steer_ratio_scale(20.0 * 0.44704) < get_bolt_2017_steer_ratio_scale(30.0 * 0.44704) + assert get_bolt_2017_steer_ratio_scale(5.0 * 0.44704) < 1.01 + assert get_bolt_2017_steer_ratio_scale(35.0 * 0.44704) > 1.04 assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(0.6, -0.6, 8.0) assert get_bolt_2017_torque_scale(-0.6, -0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.6, 8.0) assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, -0.6, 8.0)