diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index c8c0c79d7..9dd3724aa 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -69,6 +69,8 @@ TRUCK_LONG_SMOOTH_CARS = { TRUCK_FRICTION_BRAKE_ENGAGE = 40 TRUCK_FRICTION_BRAKE_RELEASE = 8 TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL = -0.85 +TRUCK_FOLLOW_MICRO_ACCEL_MAX = 0.30 +TRUCK_FOLLOW_MICRO_ACCEL_SLEW = 1.5 ACC_DASHBOARD_ZERO_RESERVED_CARS = { CAR.CHEVROLET_BLAZER, CAR.CHEVROLET_EQUINOX, @@ -215,11 +217,11 @@ def shape_truck_positive_accel(accel: float, v_ego: float, enabled: bool, if not enabled or accel <= 0.0 or v_ego < 12.0: return accel - low_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.93, 0.84, 0.76, 0.70])) - mid_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.97, 0.91, 0.85, 0.79])) + low_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.93, 0.84, 0.76, 0.76])) + mid_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.97, 0.91, 0.85, 0.85])) if lead_visible and set_speed_error > 0.0: - follow_relief = float(np.interp(set_speed_error, [0.0, 1.0, 2.5, 4.0, 6.0], [0.0, 0.04, 0.10, 0.18, 0.30])) + follow_relief = float(np.interp(set_speed_error, [0.0, 1.0, 2.5, 4.0, 6.0], [0.0, 0.04, 0.10, 0.24, 0.42])) low_scale += (1.0 - low_scale) * follow_relief mid_scale += (1.0 - mid_scale) * follow_relief @@ -232,6 +234,19 @@ def shape_truck_positive_accel(accel: float, v_ego: float, enabled: bool, return accel +def smooth_truck_follow_accel(accel: float, previous_accel: float, v_ego: float, + enabled: bool, lead_visible: bool, stopping: bool) -> float: + if ( + not enabled or not lead_visible or stopping or v_ego < 25.0 or + abs(accel) > TRUCK_FOLLOW_MICRO_ACCEL_MAX or accel <= TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL or + abs(previous_accel) > TRUCK_FOLLOW_MICRO_ACCEL_MAX or previous_accel <= TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL + ): + return accel + + max_delta = TRUCK_FOLLOW_MICRO_ACCEL_SLEW * DT_CTRL * 4 + return previous_accel + float(np.clip(accel - previous_accel, -max_delta, max_delta)) + + def shape_truck_pitch_accel(pitch_accel: float, v_ego: float, enabled: bool) -> float: if not enabled: return pitch_accel @@ -557,6 +572,7 @@ class CarController(CarControllerBase): self.bolt_acc_pedal_friction_release_frames = 0 self.bolt_acc_pedal_friction_low_speed_active = False self.truck_friction_brake_active = False + self.truck_follow_accel = 0.0 def _reset_volt_one_pedal(self): self.volt_one_pedal_pid.reset() @@ -978,10 +994,12 @@ class CarController(CarControllerBase): self.regen_release_counter = 0 self.regen_min_on_frames = 0 self.regen_min_off_frames = 0 + self.truck_follow_accel = 0.0 elif should_use_fixed_stopping_brake(self.CP, near_stop, stopping, CC.cruiseControl.resume): stop_accel = getattr(starpilot_toggles, "stopAccel", self.CP.stopAccel) self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = int(min(-100 * stop_accel, self.params.MAX_BRAKE)) + self.truck_follow_accel = 0.0 else: long_pitch_enabled = bool(getattr(starpilot_toggles, "long_pitch", True)) pedal_long_path = bool(self.CP.enableGasInterceptorDEPRECATED and (self.CP.flags & GMFlags.PEDAL_LONG.value)) @@ -1040,6 +1058,16 @@ class CarController(CarControllerBase): ) accel_cmd = float(np.clip(accel_input, self.params.ACCEL_MIN, accel_max)) + if truck_long_smoothing: + accel_cmd = smooth_truck_follow_accel( + accel_cmd, + self.truck_follow_accel, + CS.out.vEgo, + True, + CC.hudControl.leadVisible, + stopping, + ) + self.truck_follow_accel = accel_cmd torque = self.tireRadius * ((self.mass * accel_cmd) + (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2)) scaled_torque = torque + self.params.ZERO_GAS apply_gas_torque = np.clip(scaled_torque, self.params.MAX_ACC_REGEN, gas_max) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index b97fed0c2..aae318968 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -57,6 +57,7 @@ from opendbc.car.gm.carcontroller import ( shape_truck_friction_brake, shape_truck_pitch_accel, shape_truck_positive_accel, + smooth_truck_follow_accel, should_use_fixed_stopping_brake, should_activate_auto_hold, should_activate_volt_one_pedal, @@ -862,6 +863,27 @@ def test_shape_truck_positive_accel_does_not_relax_without_speed_error(): assert no_error == base +def test_shape_truck_positive_accel_keeps_more_highway_follow_authority(): + city = shape_truck_positive_accel(0.28, 26.0, True) + highway = shape_truck_positive_accel(0.28, 34.0, True) + + assert highway >= city + + +def test_smooth_truck_follow_accel_slews_small_highway_commands(): + shaped = smooth_truck_follow_accel(0.20, -0.20, 30.0, True, True, False) + + assert shaped == pytest.approx(-0.14) + + +def test_smooth_truck_follow_accel_does_not_delay_safety_requests(): + assert smooth_truck_follow_accel(-0.40, 0.20, 30.0, True, True, False) == -0.40 + assert smooth_truck_follow_accel(0.20, -0.40, 30.0, True, True, False) == 0.20 + assert smooth_truck_follow_accel(-0.20, 0.20, 30.0, True, True, True) == -0.20 + assert smooth_truck_follow_accel(-0.20, 0.20, 20.0, True, True, False) == -0.20 + assert smooth_truck_follow_accel(-0.20, 0.20, 30.0, True, False, False) == -0.20 + + def test_shape_truck_pitch_accel_attenuates_highway_grade_feedforward(): assert shape_truck_pitch_accel(-0.30, 30.0, True) == pytest.approx(-0.0825) assert shape_truck_pitch_accel(0.30, 30.0, True) == pytest.approx(0.0825) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index be65d996d..4b3f1f4ff 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -809,10 +809,10 @@ PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2 -CAMRY_CENTER_FRICTION_THRESHOLD_GAIN = 0.06 +CAMRY_CENTER_FRICTION_THRESHOLD_GAIN = 0.09 CAMRY_CENTER_FRICTION_THRESHOLD_LAT = 0.22 CAMRY_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06 -CAMRY_CENTER_FRICTION_THRESHOLD_SPEED = 25.0 +CAMRY_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 CAMRY_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 3.0 RAV4_PRIME_PHASE_SCALE = 0.12