diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index c91e67b5f..b1bfb15f7 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -356,7 +356,7 @@ def shape_bolt_acc_pedal_low_speed_friction(apply_brake: int, v_ego: float, stop return 0, False if stopping: - stop_fade = float(np.interp(v_ego, [0.0, 0.5, 1.0, 2.0], [0.0, 0.0, 0.45, 0.85])) + stop_fade = float(np.interp(v_ego, [0.0, 0.6, 0.9, 1.2, 1.8, 2.8], [0.0, 0.0, 0.05, 0.12, 0.32, 0.78])) apply_brake = int(round(apply_brake * stop_fade)) if apply_brake <= 0 or apply_brake < release_threshold: return 0, False diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 2c0588c6e..96d83d5c4 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -208,6 +208,13 @@ def test_bolt_acc_pedal_low_speed_friction_preserves_rolling_stop_authority(): assert active +def test_bolt_acc_pedal_low_speed_friction_drops_out_before_two_clamp(): + apply_brake, active = shape_bolt_acc_pedal_low_speed_friction(80, 0.9, True, True) + + assert apply_brake == 0 + assert not active + + def test_bolt_pedal_long_accel_limit_matches_planner_regen_envelope(): assert get_bolt_pedal_long_accel_limit(6.66) == pytest.approx(-2.379, abs=1e-3) assert get_bolt_pedal_long_accel_limit(3.0) == pytest.approx(-1.70, abs=1e-3) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index b755b09c0..49b5a3972 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,6 +4,7 @@ from collections import deque from cereal import log from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags +from opendbc.car.hyundai.values import HyundaiFlags from opendbc.car.lateral import get_friction from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.filter_simple import FirstOrderFilter @@ -86,6 +87,7 @@ class LatControlTorque(LatControl): self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_silverado = CP.carFingerprint in SILVERADO_CARS + self.is_hkg_canfd_torque = CP.brand == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD) if self.is_ioniq_6: self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED) self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017 @@ -239,7 +241,7 @@ class LatControlTorque(LatControl): 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_threshold = get_hkg_canfd_base_friction_threshold(CS.vEgo) if self.is_hkg_canfd_torque else get_friction_threshold(CS.vEgo) friction_scale = 1.0 if bolt_2022_2023_tuned_path_active: ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 0ffbc27a7..cdc59b98d 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -8,6 +8,7 @@ from openpilot.common.constants import CV from openpilot.starpilot.common.testing_grounds import testing_ground CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 +HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75 @@ -455,7 +456,7 @@ IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2 IONIQ_6_FF_GAIN_LEFT = 0.045 IONIQ_6_FF_GAIN_RIGHT = 0.015 IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 -IONIQ_6_BASE_FRICTION_THRESHOLD = 0.36 +IONIQ_6_BASE_FRICTION_THRESHOLD = HKG_CANFD_BASE_FRICTION_THRESHOLD IONIQ_6_FF_ONSET = 0.10 IONIQ_6_FF_ONSET_WIDTH = 0.04 IONIQ_6_FF_CUTOFF = 0.48 @@ -659,6 +660,10 @@ def get_friction_threshold(v_ego: float) -> float: return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) +def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float: + return max(get_friction_threshold(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + + def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0) speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0]) @@ -1631,7 +1636,7 @@ def get_ioniq_5_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo def get_ioniq_5_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) + base_threshold = get_hkg_canfd_base_friction_threshold(v_ego) envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) @@ -1771,7 +1776,7 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = max(get_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD) + base_threshold = max(get_hkg_canfd_base_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD) transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) @@ -1964,7 +1969,7 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) + base_threshold = get_hkg_canfd_base_friction_threshold(v_ego) transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 54267e8fd..9901f9c54 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -32,6 +32,19 @@ def apply_deadzone(error, deadzone): return error +def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego): + if output_accel >= -0.05 or a_target >= -0.80 or v_ego <= 5.0: + return 0.0 + + authority_gap = max(0.0, abs(a_target) - abs(output_accel)) + if authority_gap <= 0.25: + return 0.0 + + speed_factor = interp(v_ego, [5.0, 10.0, 15.0, 25.0], [0.0, 0.55, 0.85, 1.0]) + max_bias = interp(abs(a_target), [0.8, 1.4, 2.2, 3.5], [0.0, 0.14, 0.42, 0.70]) + return float(min(authority_gap * 0.30, max_bias) * speed_factor) + + def long_control_state_trans(CP, active, long_control_state, v_ego, should_stop, brake_pressed, cruise_standstill, starpilot_toggles, allow_stopping_release=True): @@ -301,6 +314,10 @@ class LongControl: return output_accel authority_gap = max(0.0, abs(a_target) - abs(output_accel)) + if self.is_bolt_acc_pedal_friction_car: + bias = get_bolt_acc_pedal_friction_bias(output_accel, a_target, CS.vEgo) + return output_accel - float(bias) + if authority_gap <= 0.40: return output_accel diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 36c89a494..16a8e19ae 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -19,6 +19,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import ( get_civic_bosch_modified_pid_output_alpha, get_civic_bosch_modified_pid_output_scale, ) +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_hkg_canfd_base_friction_threshold from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_civic_bosch_modified_a_center_taper_scale, LatControlTorque, @@ -432,7 +433,7 @@ class TestLatControl: assert unwind_right < unwind_left def test_ioniq_5_friction_curves(self): - base = get_friction_threshold(12.0) + base = get_hkg_canfd_base_friction_threshold(12.0) turn_in_left_threshold = get_ioniq_5_friction_threshold(12.0, 0.7, 0.8) turn_in_right_threshold = get_ioniq_5_friction_threshold(12.0, -0.7, -0.8) unwind_left_threshold = get_ioniq_5_friction_threshold(12.0, 0.7, -0.8) @@ -449,6 +450,7 @@ class TestLatControl: assert turn_in_left_scale > turn_in_right_scale > 1.0 assert unwind_left_scale < 1.0 assert unwind_right_scale <= unwind_left_scale + assert get_ioniq_5_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0) 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) @@ -519,7 +521,7 @@ class TestLatControl: assert get_ioniq_6_output_taper_scale(-1.2, 0.7, 25.0) <= get_ioniq_6_output_taper_scale(-1.2, 0.0, 25.0) def test_ioniq_6_friction_threshold_curve(self): - base = max(get_friction_threshold(6.0), 0.36) + base = get_hkg_canfd_base_friction_threshold(6.0) left_turn_in = get_ioniq_6_friction_threshold(6.0, 0.5, 0.8) right_turn_in = get_ioniq_6_friction_threshold(6.0, -0.5, -0.8) left_unwind = get_ioniq_6_friction_threshold(6.0, 0.5, -0.8) @@ -527,7 +529,7 @@ class TestLatControl: assert max(left_turn_in, right_turn_in) < base assert left_unwind >= base assert right_unwind >= base - assert get_ioniq_6_friction_threshold(25.0, 0.0, 0.0) >= 0.36 + assert get_ioniq_6_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0) def test_ioniq_6_friction_scale_curve(self): base = get_ioniq_6_friction_scale(25.0, 0.5, 0.8) @@ -555,12 +557,13 @@ class TestLatControl: assert get_kia_ev6_ff_scale(1.2, 0.0, 20.0) < get_kia_ev6_ff_scale(0.4, 0.0, 20.0) def test_kia_ev6_friction_threshold_curve(self): - base = get_friction_threshold(6.0) + base = get_hkg_canfd_base_friction_threshold(6.0) left_turn_in = get_kia_ev6_friction_threshold(6.0, 0.5, 0.8) right_turn_in = get_kia_ev6_friction_threshold(6.0, -0.5, -0.8) left_unwind = get_kia_ev6_friction_threshold(6.0, 0.5, -0.8) right_unwind = get_kia_ev6_friction_threshold(6.0, -0.5, 0.8) assert right_turn_in < left_turn_in < base < right_unwind <= left_unwind + assert get_kia_ev6_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0) def test_kia_ev6_friction_scale_curve(self): base = get_kia_ev6_friction_scale(25.0, 0.5, 0.8)