diff --git a/opendbc_repo/opendbc/car/honda/carcontroller.py b/opendbc_repo/opendbc/car/honda/carcontroller.py index 68f1b1b35..e85aa6f7e 100644 --- a/opendbc_repo/opendbc/car/honda/carcontroller.py +++ b/opendbc_repo/opendbc/car/honda/carcontroller.py @@ -6,9 +6,21 @@ from opendbc.car.honda import hondacan from opendbc.car.honda.values import CAR, CruiseButtons, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \ HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from opendbc.car.interfaces import CarControllerBase +from openpilot.starpilot.common.testing_grounds import testing_ground VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState +CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX = 4864 + + +def civic_bosch_modified_lateral_testing_ground_active() -> bool: + return testing_ground.use("8", "B") + + +def get_civic_bosch_modified_steer_can_max(base_steer_can_max: int, CP) -> int: + if CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH and CP.dashcamOnly and civic_bosch_modified_lateral_testing_ground_active(): + return CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX + return base_steer_can_max def compute_gb_honda_bosch(accel, speed): @@ -140,8 +152,12 @@ class CarController(CarControllerBase): # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) - apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX, - self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) + steer_can_max = get_civic_bosch_modified_steer_can_max(self.params.STEER_MAX, self.CP) + if steer_can_max != self.params.STEER_MAX: + apply_torque = int(np.clip(-limited_torque * steer_can_max, -steer_can_max, steer_can_max)) + else: + apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX, + self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) # Send CAN commands can_sends = [] diff --git a/opendbc_repo/opendbc/car/honda/tests/test_honda.py b/opendbc_repo/opendbc/car/honda/tests/test_honda.py index e72e6479b..ad6d6a7dd 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_honda.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_honda.py @@ -1,7 +1,9 @@ import re +from opendbc.car.structs import CarParams +from opendbc.car.honda.carcontroller import CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX, get_civic_bosch_modified_steer_can_max from opendbc.car.honda.fingerprints import FW_VERSIONS -from opendbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL +from opendbc.car.honda.values import CAR, HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL, HondaFlags HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" @@ -16,3 +18,15 @@ class TestHondaFingerprint: def test_tja_bosch_only(self): assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list" + + def test_modified_civic_steer_can_max_is_scoped(self, monkeypatch): + CP = CarParams.new_message() + CP.carFingerprint = CAR.HONDA_CIVIC_BOSCH + CP.flags = int(HondaFlags.BOSCH) + CP.dashcamOnly = True + + monkeypatch.setattr("opendbc.car.honda.carcontroller.civic_bosch_modified_lateral_testing_ground_active", lambda: True) + assert get_civic_bosch_modified_steer_can_max(4096, CP) == CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX + + CP.dashcamOnly = False + assert get_civic_bosch_modified_steer_can_max(4096, CP) == 4096 diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 6e4b8e885..33fadb78e 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -27,7 +27,9 @@ HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 IONIQ_6_CANFD_SCC_ACCEL_STEP = 6.0 / 50.0 IONIQ_6_CANFD_SCC_DECEL_STEP = 15.0 / 50.0 GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5] -GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.30, -0.55] +GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.36, -0.72] +GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5] +GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.10, 0.09, 0.08, 0.07, 0.05] GENESIS_G90_RELEASE_SPEED_BP = [0.0, 0.3, 0.6] GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.24, 0.20, 0.14] GENESIS_G90_RELEASE_DECEL_STEP_V = [0.30, 0.24, 0.18] @@ -156,7 +158,12 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt stopping = long_control_state == LongCtrlState.stopping if stopping and v_ego <= GENESIS_G90_STOP_HOLD_SPEED_BP[-1]: state.release_active = False - state.actual_accel = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V)) + target_hold = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V)) + if state.actual_accel < target_hold: + relax_step = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP, GENESIS_G90_STOP_HOLD_RELAX_STEP_V)) + state.actual_accel = min(state.actual_accel + relax_step, target_hold) + else: + state.actual_accel = target_hold else: if state.long_control_state_last == LongCtrlState.stopping and long_control_state == LongCtrlState.pid and \ accel_cmd > 0.0 and v_ego < GENESIS_G90_RELEASE_MAX_SPEED: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index d31c7eb78..aa4a80ea0 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -313,6 +313,17 @@ class TestHyundaiFingerprint: assert state.actual_accel == pytest.approx(-0.12) assert not state.release_active + def test_genesis_g90_longitudinal_tuning_gradually_unwinds_into_stop_hold(self): + state = GenesisG90LongitudinalTuningState(actual_accel=-2.0) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.5, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.actual_accel == pytest.approx(-1.95) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.3, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.actual_accel == pytest.approx(-1.88) + def test_genesis_g90_longitudinal_tuning_ramps_out_of_stop_hold(self): state = GenesisG90LongitudinalTuningState(actual_accel=-0.12, long_control_state_last=LongCtrlState.stopping) @@ -563,10 +574,15 @@ class TestHyundaiFingerprint: ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT) sportage_params = CarControllerParams(sportage) + sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0) + sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0) comparison_params = CarControllerParams(comparison_angle) ioniq6_params = CarControllerParams(ioniq6) assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK + assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK + assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK + assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK def test_ioniq_5_canfd_aux_messages_are_optional(self): diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index de150dc73..b2cc4a5eb 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -53,12 +53,15 @@ class CarControllerParams: if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: self.STEER_THRESHOLD = 175 - # The Sportage angle port is rough at low speed on the higher global jerk limit. - # Keep the branch-wide higher limit for other cars, but restore the older calmer - # jerk ceiling on this port only. + # The Sportage angle port gets rough if it keeps the branch-wide higher jerk + # limit everywhere, but the fully calmed 3.0 m/s^3 cap lags too much in tight + # low-speed turns. Give it extra low-speed authority, then fade back to the + # calmer ceiling by normal road speeds. if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026: + sportage_low_speed_weight = min(max((15.0 - vEgoRaw) / 7.0, 0.0), 1.0) + sportage_lateral_jerk = 3.0 + (1.2 * sportage_low_speed_weight) self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS, - MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL)) + MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL)) # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 614f0a14d..4c1e6b18f 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -31,6 +31,7 @@ MAX_PITCH_COMPENSATION = 1.5 # m/s^2 TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s TOYOTA_COAST_BRAKE_ENABLE_ACCEL = -0.10 # m/s^2 TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06 # m/s^2 +TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30 # m/s^2 # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -64,10 +65,13 @@ def get_long_tune(CP, params): def update_permit_braking(current: bool, net_acceleration_request_min: float, stopping: bool, - long_active: bool, v_ego: float) -> bool: + long_active: bool, v_ego: float, lead_visible: bool) -> bool: if stopping or not long_active: return True + if not lead_visible and v_ego >= TOYOTA_COAST_BRAKE_MIN_SPEED: + return net_acceleration_request_min <= TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL + # At cruising speeds, some Toyota platforms turn tiny negative accel corrections # into noticeable brake taps. Keep a small coast-only band so mild follow/cruise # trims stay off the brakes while still allowing real negative requests through. @@ -316,7 +320,8 @@ class CarController(CarControllerBase): net_acceleration_request_min, stopping, CC.longActive, - CS.out.vEgo) + CS.out.vEgo, + lead) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 7a0b80339..51ff27de9 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -222,18 +222,25 @@ class TestToyotaCarController: assert controller.standstill_req is False def test_permit_braking_high_speed_coasts_for_tiny_decel(self): - assert update_permit_braking(True, -0.05, False, True, 25.0) is False - assert update_permit_braking(False, -0.05, False, True, 25.0) is False + assert update_permit_braking(True, -0.05, False, True, 25.0, True) is False + assert update_permit_braking(False, -0.05, False, True, 25.0, True) is False - def test_permit_braking_high_speed_brakes_for_meaningful_decel(self): - assert update_permit_braking(False, -0.15, False, True, 25.0) is True + def test_permit_braking_high_speed_brakes_for_meaningful_decel_with_lead(self): + assert update_permit_braking(False, -0.15, False, True, 25.0, True) is True + + def test_permit_braking_high_speed_no_lead_coasts_for_mild_decel(self): + assert update_permit_braking(True, -0.25, False, True, 25.0, False) is False + assert update_permit_braking(False, -0.25, False, True, 25.0, False) is False + + def test_permit_braking_high_speed_no_lead_brakes_for_stronger_decel(self): + assert update_permit_braking(False, -0.35, False, True, 25.0, False) is True def test_permit_braking_low_speed_keeps_legacy_behavior(self): - assert update_permit_braking(False, -0.05, False, True, 10.0) is True + assert update_permit_braking(False, -0.05, False, True, 10.0, False) is True def test_permit_braking_forces_on_when_stopping_or_inactive(self): - assert update_permit_braking(False, 0.10, True, True, 25.0) is True - assert update_permit_braking(False, 0.10, False, False, 25.0) is True + assert update_permit_braking(False, 0.10, True, True, 25.0, False) is True + assert update_permit_braking(False, 0.10, False, False, 25.0, False) is True def test_sng_hack_clears_existing_standstill_latch(self): controller = self._make_controller(standstill_req=True, last_standstill=True) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 85bffee5d..6398bc211 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,10 +1,39 @@ import math from cereal import log +from opendbc.car.honda.values import CAR as HONDA +from openpilot.starpilot.common.testing_grounds import testing_ground from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController +def civic_bosch_modified_lateral_testing_ground_active() -> bool: + return testing_ground.use("8", "B") + + +def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float: + abs_angle = abs(desired_angle_deg) + if abs_angle < 8.0: + return 1.0 + + speed_weight = min(max((v_ego - 2.0) / 8.0, 0.0), 1.0) + angle_weight = min(max((abs_angle - 8.0) / 32.0, 0.0), 1.0) + phase = desired_angle_deg * desired_angle_delta_deg + + is_left = desired_angle_deg > 0.0 + base_scale = 0.02 if is_left else 0.05 + turn_in_scale = 0.02 if is_left else 0.03 + unwind_scale = 0.00 if is_left else 0.02 + + scale = 1.0 + (speed_weight * angle_weight * base_scale) + if phase > 0.2: + scale += speed_weight * angle_weight * turn_in_scale + elif phase < -0.2: + scale += speed_weight * angle_weight * unwind_scale + + return scale + + class LatControlPID(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -13,6 +42,8 @@ class LatControlPID(LatControl): pos_limit=self.steer_max, neg_limit=-self.steer_max) self.ff_factor = CP.lateralTuning.pid.kf self.get_steer_feedforward = CI.get_steer_feedforward_function() + self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and CP.dashcamOnly + self.prev_angle_steers_des_no_offset = 0.0 def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): pid_log = log.ControlsState.LateralPIDState.new_message() @@ -28,6 +59,7 @@ class LatControlPID(LatControl): if not active: output_torque = 0.0 pid_log.active = False + self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset else: # offset does not contribute to resistive torque @@ -39,11 +71,17 @@ class LatControlPID(LatControl): speed=CS.vEgo, freeze_integrator=freeze_integrator) + if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active(): + desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset + output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo) + output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max)) + pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_torque) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset return output_torque, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 23f7f56a9..1b4c8c438 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -211,21 +211,21 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 0.88 -IONIQ_6_TURN_IN_BOOST_RIGHT = 0.92 -IONIQ_6_UNWIND_TAPER_LEFT = 1.52 -IONIQ_6_UNWIND_TAPER_RIGHT = 2.92 +IONIQ_6_TURN_IN_BOOST_LEFT = 0.94 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.00 +IONIQ_6_UNWIND_TAPER_LEFT = 1.60 +IONIQ_6_UNWIND_TAPER_RIGHT = 3.08 IONIQ_6_FRICTION_MULT = 0.995 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.34 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.40 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.10 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.11 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.18 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.22 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.58 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.26 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.38 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.48 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.30 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.12 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.20 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.32 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.82 IONIQ_6_CENTER_TAPER_MAX = 0.042 IONIQ_6_CENTER_TAPER_LAT = 0.18 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 @@ -242,8 +242,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08 IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05 IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.44 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.72 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.58 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.70 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1194cf95e..fe44606af 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,6 +44,18 @@ VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30 VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0 VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20 +VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9 +VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97 +VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0 +VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO = 0.16 +VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO = 0.24 +VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC = 16.0 +VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0 +VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.25 +VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0 +VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0 +VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.7 +VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1 VISION_SLOW_LEAD_MAX_SPEED = 5.0 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 @@ -342,6 +354,51 @@ class LongitudinalPlanner: return max(accel_min, -approach_decel) + def get_vision_untracked_slow_lead_cap(self, lead, v_ego, accel_min): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB: + return None + + lead_brake = max(0.0, -float(lead.aLeadK)) + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + closing_speed = max(0.0, v_ego - lead.vLead) + projected_closing_speed = closing_speed + lead_brake * reaction_t + if projected_closing_speed < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED: + return None + + closing_ratio = projected_closing_speed / max(float(v_ego), 0.1) + if closing_ratio < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO: + return None + + max_distance = float(np.clip(VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME * v_ego, + VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE, + VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE)) + if float(lead.dRel) > max_distance: + return None + + projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) + if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC: + return None + + time_factor = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - projected_ttc) / + (VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), + 0.0, 1.0)) + prob_factor = float(np.clip((lead_prob - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB) / + (VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB), + 0.0, 1.0)) + closing_factor = float(np.clip((closing_ratio - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO) / + (VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO), + 0.0, 1.0)) + approach_decel = VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL * np.clip( + 0.5 * time_factor + 0.3 * prob_factor + 0.2 * closing_factor, 0.0, 1.0) + if approach_decel < VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL: + return None + + return max(accel_min, -approach_decel) + def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow): if lead is None or not lead.status or bool(getattr(lead, "radar", False)): return None @@ -742,6 +799,19 @@ class LongitudinalPlanner: vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) output_accel_min = comfort_output_accel_min + if not tracking_lead: + pretracking_vision_caps = [] + for lead in (self.lead_one, self.lead_two): + if lead.status and not bool(getattr(lead, "radar", False)): + pretracking_cap = self.get_vision_untracked_slow_lead_cap(lead, v_ego, vision_cap_accel_min) + if pretracking_cap is not None: + pretracking_vision_caps.append(pretracking_cap) + + if pretracking_vision_caps: + pretracking_vision_cap = min(pretracking_vision_caps) + self.a_desired = min(self.a_desired, pretracking_vision_cap) + output_a_target = min(output_a_target, pretracking_vision_cap) + close_lead_caps = [] vision_low_speed_stop_active = False vision_brake_cap_active = False diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index bfc544fb5..a66e8038d 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -3,6 +3,7 @@ from types import SimpleNamespace from cereal import car, custom, log import openpilot.selfdrive.controls.lib.latcontrol_torque as latcontrol_torque +import openpilot.selfdrive.controls.lib.latcontrol_pid as latcontrol_pid from opendbc.car.car_helpers import interfaces from opendbc.car.honda.values import CAR as HONDA from opendbc.car.toyota.values import CAR as TOYOTA @@ -12,7 +13,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID, get_civic_bosch_modified_pid_output_scale from openpilot.selfdrive.controls.lib.latcontrol_torque import ( LatControlTorque, get_bolt_2017_center_taper_scale, @@ -73,6 +74,30 @@ class TestLatControl: starpilot_toggles = SimpleNamespace() return controller, VM, CS, params, starpilot_toggles + @staticmethod + def _build_pid_controller(car_name): + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CP.dashcamOnly = True + CI = CarInterface(CP, custom.StarPilotCarParams.new_message()) + controller = LatControlPID(CP.as_reader(), CI, DT_CTRL) + VM = VehicleModel(CP) + + CS = car.CarState.new_message() + CS.vEgo = 12 + CS.steeringPressed = False + CS.steeringAngleDeg = -6.0 + CS.steeringRateDeg = 0.0 + + params = log.LiveParametersData.new_message() + params.steerRatio = CP.steerRatio + params.stiffnessFactor = 1.0 + params.roll = 0.0 + params.angleOffsetDeg = 0.0 + + starpilot_toggles = SimpleNamespace() + return controller, VM, CS, params, starpilot_toggles + def test_bolt_2017_testing_ground_scale_curve(self): assert get_bolt_2017_base_torque_scale(0.1) == 1.0 assert get_bolt_2017_base_torque_scale(-0.1) == 1.0 @@ -354,6 +379,25 @@ class TestLatControl: assert lac_log.active + def test_civic_bosch_modified_pid_scale_curve(self): + assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) == 1.0 + assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) + + def test_civic_bosch_modified_pid_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(HONDA.HONDA_CIVIC_BOSCH) + monkeypatch.setattr(latcontrol_pid, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) + + base_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.004, False, 0.2, None, None, starpilot_toggles) + assert lac_log.active + + # Use a second update to create a turn-in phase with a larger desired angle delta. + tuned_output, _, _ = controller.update(True, CS, VM, params, False, 0.006, False, 0.2, None, None, starpilot_toggles) + + assert abs(tuned_output) >= abs(base_output) + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)]) def test_saturation(self, car_name, controller): diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index e4622c8a2..2b94f5f79 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -237,6 +237,23 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None +def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_case(): + route_v_ego = 23.23 + far_v_ego = 29.0 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=route_v_ego) + route_like_lead = make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92) + far_mild_lead = make_lead(status=True, d_rel=82.0, v_lead=25.0, a_lead=0.0, radar=False, model_prob=0.9) + + route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, route_v_ego, -1.0) + far_cap = planner.get_vision_untracked_slow_lead_cap(far_mild_lead, far_v_ego, -1.0) + + assert route_cap is not None + assert route_cap < -0.1 + assert far_cap is None + + def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop(): v_ego = 13.207 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -370,6 +387,41 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_version): + v_ego = 23.23 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) + planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) + sm_no_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + ) + sm_with_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92), + ) + sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 + sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 + + for _ in range(6): + planner_no_lead.update(sm_no_lead, make_toggles(model_version)) + planner_with_lead.update(sm_with_lead, make_toggles(model_version)) + + assert planner_with_lead.mode == "acc" + assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) + assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.04 + assert planner_with_lead.output_a_target < -0.2 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version): v_ego = 3.8 diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index edb7379aa..a8803a58a 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -14,6 +14,7 @@ TESTING_GROUND_4 = "4" TESTING_GROUND_5 = "5" TESTING_GROUND_6 = "6" TESTING_GROUND_7 = "7" +TESTING_GROUND_8 = "8" TESTING_GROUND_IDS = ( TESTING_GROUND_1, @@ -23,6 +24,7 @@ TESTING_GROUND_IDS = ( TESTING_GROUND_5, TESTING_GROUND_6, TESTING_GROUND_7, + TESTING_GROUND_8, ) TESTING_GROUNDS_STATE_PATH = Path("/tmp/the_pond_testing_grounds_slots.json") if PC else Path("/data/testing_grounds/slots.json") @@ -80,6 +82,13 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( "aLabel": "A - Installed tune", "bLabel": "B - Firestar Tune", }, + { + "id": TESTING_GROUND_8, + "name": "Modified Civic Lateral", + "description": "Modified Honda Civic Bosch lateral sandbox.", + "aLabel": "A - Installed tune", + "bLabel": "B - Firestar Tune", + }, ) @@ -248,6 +257,7 @@ use_testing_ground_4 = _TestingGroundFlag(TESTING_GROUND_4) use_testing_ground_5 = _TestingGroundFlag(TESTING_GROUND_5) use_testing_ground_6 = _TestingGroundFlag(TESTING_GROUND_6) use_testing_ground_7 = _TestingGroundFlag(TESTING_GROUND_7) +use_testing_ground_8 = _TestingGroundFlag(TESTING_GROUND_8) class _TestingGroundNamespace: @@ -260,6 +270,7 @@ class _TestingGroundNamespace: id_5 = TESTING_GROUND_5 id_6 = TESTING_GROUND_6 id_7 = TESTING_GROUND_7 + id_8 = TESTING_GROUND_8 use_1 = use_testing_ground_1 use_2 = use_testing_ground_2 @@ -268,6 +279,7 @@ class _TestingGroundNamespace: use_5 = use_testing_ground_5 use_6 = use_testing_ground_6 use_7 = use_testing_ground_7 + use_8 = use_testing_ground_8 def use(self, slot_id, variant=TESTING_GROUND_TEST_VARIANT): return is_testing_ground_active(slot_id, variant)