diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7ce229218..c8a525abb 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -152,21 +152,26 @@ VOLT_STANDARD_FF_CUTOFF = 1.30 VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.24 VOLT_STANDARD_TRANSITION_SPEED = 10.0 VOLT_STANDARD_PHASE_SCALE = 0.10 -VOLT_STANDARD_TURN_IN_BOOST_LEFT = -0.04 -VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.24 -VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.03 -VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.08 +VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.02 +VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.30 +VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.08 +VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.14 VOLT_STANDARD_FRICTION_MULT = 1.04 VOLT_STANDARD_FRICTION_LAT_RISE = 0.20 VOLT_STANDARD_FRICTION_JERK_RISE = 0.24 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = -0.04 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = -0.06 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.04 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = -0.02 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.08 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = -0.04 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.04 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.02 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.18 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.02 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.10 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.01 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.10 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.02 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08 +VOLT_STANDARD_CENTER_TAPER_MAX = 0.11 +VOLT_STANDARD_CENTER_TAPER_LAT = 0.16 +VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.03 +VOLT_STANDARD_CENTER_TAPER_SPEED = 17.0 +VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5 GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 GENESIS_G90_FF_GAIN_LEFT = 0.09 @@ -545,6 +550,13 @@ def get_volt_standard_friction_scale(v_ego: float, desired_lateral_accel: float, return min(max(friction_scale, 0.90), 1.14) +def get_volt_standard_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _volt_standard_sigmoid((v_ego - VOLT_STANDARD_CENTER_TAPER_SPEED) / VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH) + center_weight = _volt_standard_sigmoid((VOLT_STANDARD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH) + reduction = VOLT_STANDARD_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def genesis_g90_lateral_testing_ground_active() -> bool: return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID) @@ -919,6 +931,8 @@ class LatControlTorque(LatControl): output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif bolt_2018_2021_tuned_path_active: output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) + elif volt_standard_test_active: + output_torque *= get_volt_standard_center_taper_scale(setpoint, CS.vEgo) pid_log.active = True pid_log.p = float(self.pid.p) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 9702d9c29..a9b2edeed 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -33,6 +33,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_kia_ev6_ff_scale, get_kia_ev6_friction_scale, get_kia_ev6_friction_threshold, + get_volt_standard_center_taper_scale, get_volt_standard_ff_scale, get_volt_standard_friction_scale, get_volt_standard_friction_threshold, @@ -147,8 +148,8 @@ class TestLatControl: def test_volt_standard_ff_scale_curve(self): assert get_volt_standard_ff_scale(0.0, 0.0, 20.0) == 1.0 assert get_volt_standard_ff_scale(-0.5, 0.0, 20.0) > get_volt_standard_ff_scale(0.5, 0.0, 20.0) - assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) < get_volt_standard_ff_scale(0.6, 0.0, 8.0) - assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0) + assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) > get_volt_standard_ff_scale(0.6, 0.0, 8.0) > get_volt_standard_ff_scale(0.6, -0.7, 8.0) + assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0) > get_volt_standard_ff_scale(-0.6, 0.7, 8.0) assert get_volt_standard_ff_scale(2.0, 0.0, 20.0) < get_volt_standard_ff_scale(0.8, 0.0, 20.0) def test_volt_standard_friction_threshold_curve(self): @@ -157,8 +158,8 @@ class TestLatControl: right_turn_in = get_volt_standard_friction_threshold(6.0, -0.7, -0.8) left_unwind = get_volt_standard_friction_threshold(6.0, 0.7, -0.8) right_unwind = get_volt_standard_friction_threshold(6.0, -0.7, 0.8) - assert left_turn_in > base > right_turn_in - assert left_unwind < base < right_unwind + assert right_turn_in < left_turn_in < base + assert base < left_unwind < right_unwind def test_volt_standard_friction_scale_curve(self): base = get_volt_standard_friction_scale(25.0, 0.7, 0.8) @@ -166,8 +167,13 @@ class TestLatControl: right_turn_in = get_volt_standard_friction_scale(6.0, -0.7, -0.8) left_unwind = get_volt_standard_friction_scale(6.0, 0.7, -0.8) right_unwind = get_volt_standard_friction_scale(6.0, -0.7, 0.8) - assert left_turn_in < base < right_turn_in - assert left_unwind > base > right_unwind + assert base < left_turn_in < right_turn_in + assert left_unwind < base and right_unwind < left_unwind + + def test_volt_standard_center_taper_curve(self): + assert get_volt_standard_center_taper_scale(0.0, 10.0) > get_volt_standard_center_taper_scale(0.0, 25.0) + assert get_volt_standard_center_taper_scale(0.0, 25.0) < get_volt_standard_center_taper_scale(0.10, 25.0) < get_volt_standard_center_taper_scale(0.20, 25.0) <= 1.0 + assert get_volt_standard_center_taper_scale(0.0, 25.0) > 0.85 def test_genesis_g90_ff_scale_curve(self): assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0