diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index cf75369e1..34e0829f9 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -603,6 +603,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [0.095, 0.085, 0.065, 0.050] ret.longitudinalTuning.kiV = [0.07, 0.10, 0.15, 0.24] ret.longitudinalTuning.kfDEPRECATED = 0.20 + ret.longitudinalActuatorDelay = 0.6 else: ret.longitudinalTuning.kfDEPRECATED = 0.25 diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index c5cdf6dc5..9bfb7526a 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -66,6 +66,38 @@ class TestGMFingerprint: class TestGMInterface: + @parameterized.expand([ + CAR.CHEVROLET_BOLT_CC_2017, + CAR.CHEVROLET_BOLT_CC_2018_2021, + CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + CAR.CHEVROLET_BOLT_CC_2022_2023, + CAR.CHEVROLET_MALIBU_HYBRID_CC, + ]) + def test_bolt_pedal_long_uses_shared_planning_delay_without_retuning_pid(self, car_model): + CarInterface = interfaces[car_model] + fingerprint = _empty_fingerprint() + fingerprint[0][0x201] = 8 + params = Params() + + try: + params.put_bool("GMPedalLongitudinal", True) + car_params = CarInterface.get_params( + car_model, + fingerprint, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=_test_starpilot_toggles(), + ) + finally: + params.remove("GMPedalLongitudinal") + + assert car_params.longitudinalActuatorDelay == pytest.approx(0.6) + assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.095, 0.085, 0.065, 0.050]) + assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.07, 0.10, 0.15, 0.24]) + assert car_params.longitudinalTuning.kfDEPRECATED == pytest.approx(0.20) + def test_bolt_acc_pedal_pid_accel_limits_keep_full_negative_authority(self): cp = SimpleNamespace( enableGasInterceptorDEPRECATED=True, diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index f7c08534d..9cce8585d 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -194,6 +194,9 @@ class CarInterface(CarInterfaceBase): ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 + if candidate == CAR.TOYOTA_HIGHLANDER and ret.openpilotLongitudinalControl and not ret.flags & ToyotaFlags.HYBRID.value: + ret.longitudinalActuatorDelay = 0.4 + if ret.enableGasInterceptorDEPRECATED: # Pedal/SDSU Toyotas feel best with a softer final stop clamp. ret.longitudinalActuatorDelay = max(ret.longitudinalActuatorDelay, 0.2) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index a4c585baf..55679c1df 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -101,6 +101,32 @@ class TestToyotaInterfaces: assert abs(car_params.vEgoStopping - 0.25) < 1e-6 assert abs(car_params.vEgoStarting - 0.25) < 1e-6 + def test_highlander_ice_openpilot_long_uses_measured_actuator_delay(self): + stock_params = CarInterface.get_params( + CAR.TOYOTA_HIGHLANDER, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(), + ) + long_params = CarInterface.get_params( + CAR.TOYOTA_HIGHLANDER, + {bus: ({0x2FF: 8} if bus == 0 else {}) for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(), + ) + + assert not stock_params.openpilotLongitudinalControl + assert stock_params.longitudinalActuatorDelay == pytest.approx(0.15) + assert long_params.openpilotLongitudinalControl + assert not long_params.flags & ToyotaFlags.HYBRID.value + assert long_params.longitudinalActuatorDelay == pytest.approx(0.4) + @pytest.mark.parametrize("camera_message", [0x343, 0x4CB]) def test_dsu_bypass_enables_longitudinal(self, camera_message): fingerprint = {bus: {} for bus in range(8)} diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 56928f269..5f674b2c7 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -5,6 +5,7 @@ from opendbc.car.honda.carcontroller import get_civic_bosch_modified_steering_pr from opendbc.car.honda.values import CAR as HONDA, HondaFlags from openpilot.starpilot.common.testing_grounds import testing_ground from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import SUBARU_IMPREZA_CARS, get_subaru_impreza_pid_output_scale from openpilot.common.pid import PIDController HONDA_PID_GAIN_SCALE_MIN = 0.1 @@ -97,6 +98,7 @@ class LatControlPID(LatControl): self.honda_lateral_pid_kp_scale = 1.0 self.honda_lateral_pid_ki_scale = 1.0 self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) + self.is_subaru_impreza = CP.carFingerprint in SUBARU_IMPREZA_CARS self.prev_angle_steers_des_no_offset = 0.0 self.modified_civic_steering_pressed_filter_s = 0.0 self.modified_civic_steering_pressed_prev = False @@ -158,6 +160,11 @@ class LatControlPID(LatControl): speed=CS.vEgo, freeze_integrator=freeze_integrator) + if self.is_subaru_impreza: + raw_output_torque = self.pid.p + self.pid.i + self.pid.d + self.pid.f + output_torque = raw_output_torque * get_subaru_impreza_pid_output_scale(error) + output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max)) + 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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index bd61c584f..ca048a438 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -88,6 +88,7 @@ class LatControlTorque(LatControl): self.is_palisade = CP.carFingerprint in PALISADE_CARS self.is_prius = CP.carFingerprint in PRIUS_CARS self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS + self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS @@ -243,6 +244,7 @@ class LatControlTorque(LatControl): palisade_active = self.is_palisade prius_active = self.is_prius rav4_prime_active = self.is_rav4_prime + lexus_is_active = self.is_lexus_is ioniq_5_active = self.is_ioniq_5 ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 @@ -311,6 +313,8 @@ class LatControlTorque(LatControl): ff *= get_rav4_prime_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_rav4_prime_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_rav4_prime_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif lexus_is_active: + ff *= get_lexus_is_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif ioniq_5_active: ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 72537ebff..29878918e 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -4,6 +4,7 @@ import numpy as np from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR +from opendbc.car.subaru.values import CAR as SUBARU_CAR from opendbc.car.toyota.values import CAR as TOYOTA_CAR from openpilot.common.constants import CV from openpilot.starpilot.common.testing_grounds import testing_ground @@ -136,6 +137,14 @@ RAV4_PRIME_CARS = ( TOYOTA_CAR.TOYOTA_RAV4_PRIME, ) +LEXUS_IS_CARS = ( + TOYOTA_CAR.LEXUS_IS, +) + +SUBARU_IMPREZA_CARS = ( + SUBARU_CAR.SUBARU_IMPREZA, +) + 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 @@ -739,6 +748,18 @@ RAV4_PRIME_SPEED_ONSET_WIDTH = 1.5 RAV4_PRIME_SPEED_MAX = 20.0 RAV4_PRIME_SPEED_MAX_WIDTH = 2.5 +LEXUS_IS_PHASE_SCALE = 0.10 +LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.06 +LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.12 +LEXUS_IS_UNWIND_LAT_ONSET = 0.18 +LEXUS_IS_UNWIND_LAT_WIDTH = 0.07 +LEXUS_IS_UNWIND_SPEED_ONSET = 9.0 +LEXUS_IS_UNWIND_SPEED_WIDTH = 2.0 + +SUBARU_IMPREZA_PID_TAPER_START_DEG = 0.75 +SUBARU_IMPREZA_PID_TAPER_FULL_DEG = 4.0 +SUBARU_IMPREZA_PID_TAPER_MIN = 0.58 + TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS @@ -1037,6 +1058,24 @@ def get_rav4_prime_output_taper_scale(desired_lateral_accel: float, desired_late _rav4_prime_speed_weight(v_ego)) +def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / LEXUS_IS_PHASE_SCALE) + unwind_weight = max(-phase, 0.0) + lat_weight = _sigmoid((abs(desired_lateral_accel) - LEXUS_IS_UNWIND_LAT_ONSET) / LEXUS_IS_UNWIND_LAT_WIDTH) + speed_weight = _sigmoid((v_ego - LEXUS_IS_UNWIND_SPEED_ONSET) / LEXUS_IS_UNWIND_SPEED_WIDTH) + reduction = LEXUS_IS_UNWIND_FF_REDUCTION_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT + return 1.0 - (reduction * unwind_weight * lat_weight * speed_weight) + + +def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float: + error_weight = min(max((abs(angle_error_deg) - SUBARU_IMPREZA_PID_TAPER_START_DEG) / + (SUBARU_IMPREZA_PID_TAPER_FULL_DEG - SUBARU_IMPREZA_PID_TAPER_START_DEG), 0.0), 1.0) + return 1.0 - ((1.0 - SUBARU_IMPREZA_PID_TAPER_MIN) * error_weight) + + def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6e62c5ded..576b055d7 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import shoul from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_far_follow_output_slew_rates from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog @@ -70,6 +71,22 @@ LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 LEAD_DEPART_CONFIDENT_CONFIRM_TIME = 0.35 +LEAD_DEPART_RELEASE_HOLD_TIME = 1.5 +LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME = 0.15 +LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE = 3.0 +LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED = 0.55 +LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA = 0.25 +LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE = 0.15 +LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB = 0.95 +LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET = 1.0 +LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED = 0.25 +LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN = 3.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED = 10.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE = 25.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35 +VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35 +VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0 +VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5 STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED = 0.5 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED = 0.45 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.35 @@ -162,6 +179,14 @@ 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_EARLY_MIN_MODEL_PROB = 0.95 +VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED = 2.0 +VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO = 0.12 +VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC = 24.0 +VISION_UNTRACKED_SLOW_LEAD_NEAR_MAX_DISTANCE = 45.0 +VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_MODEL_PROB = 0.90 +VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO = 0.09 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_ENTRY_MAX_LATERAL_OFFSET = 1.2 VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.4 VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0 VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0 @@ -372,7 +397,11 @@ FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL = 0.04 FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL = 0.14 FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME = 0.75 -EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED = 12.0 +# At low speed, preserve the existing handoff slew long enough to bridge a +# brief slow-lead CEM dropout without extending high-speed release behavior. +EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME = 3.0 +EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_THRESHOLD = 12.0 +EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED = 2.5 EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_SPEED = 5.0 EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_DELTA = -1.0 EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_DELTA = 1.5 @@ -644,10 +673,15 @@ class LongitudinalPlanner: self.v_model_error = 0.0 self.output_a_target = 0.0 self.output_should_stop = False + self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP) + self.far_follow_output_slew_active = False self.model_launch_armed = False self.model_launch_stop_seen = False self.confident_lead_depart_elapsed = 0.0 self.slow_creep_lead_depart_elapsed = 0.0 + self.lead_depart_release_candidate_elapsed = 0.0 + self.lead_depart_release_pending = False + self.lead_depart_release_hold_remaining = 0.0 self.radar_standstill_gap_settle_elapsed = 0.0 self.radar_standstill_gap_settle_active = False @@ -904,16 +938,43 @@ class LongitudinalPlanner: 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: + projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) + + standard_entry = bool( + projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED and + closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO and + projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC + ) + centered_relaxed_entry = ( + abs(float(getattr(lead, "yRel", 0.0))) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_ENTRY_MAX_LATERAL_OFFSET + ) + high_confidence_early_entry = bool( + centered_relaxed_entry and + lead_prob + 1e-6 >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_MODEL_PROB and + projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED and + closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO and + projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC + ) + near_early_entry = bool( + centered_relaxed_entry and + float(lead.dRel) <= VISION_UNTRACKED_SLOW_LEAD_NEAR_MAX_DISTANCE and + lead_prob + 1e-6 >= VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_MODEL_PROB and + projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED and + closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO and + projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC + ) + if not (standard_entry or high_confidence_early_entry or near_early_entry): return None - projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) - if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC: - return None + trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC + min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO + if high_confidence_early_entry: + trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC + min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO + if near_early_entry: + trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC + min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO min_model_prob = VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB max_distance_time = VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME @@ -939,17 +1000,17 @@ class LongitudinalPlanner: if float(lead.dRel) > max_distance: return None - if lead_prob < min_model_prob: + if lead_prob + 1e-6 < min_model_prob: 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), + time_factor = float(np.clip((trigger_ttc - projected_ttc) / + (trigger_ttc - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) prob_factor = float(np.clip((lead_prob - min_model_prob) / (VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - 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), + closing_factor = float(np.clip((closing_ratio - min_closing_ratio) / + (VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - 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) @@ -1362,6 +1423,68 @@ class LongitudinalPlanner: lead_accel >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL ) + def get_safe_depart_release_hold_lead(self, v_ego): + credible_leads = [ + lead for lead in (self.lead_one, self.lead_two) + if lead is not None and + bool(getattr(lead, "status", False)) and + (bool(getattr(lead, "radar", False)) or + float(getattr(lead, "modelProb", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB) and + abs(float(getattr(lead, "yRel", 0.0))) <= LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET + ] + moving_leads = [ + lead for lead in credible_leads + if float(getattr(lead, "dRel", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE and + float(getattr(lead, "vLead", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED and + float(getattr(lead, "vLead", 0.0)) - float(v_ego) >= LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA and + max(0.0, -float(getattr(lead, "aLeadK", 0.0))) <= LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE + ] + if not moving_leads: + return None + + selected_lead = min(moving_leads, key=lambda lead: float(lead.dRel)) + stopped_conflict = any( + lead is not selected_lead and + float(getattr(lead, "dRel", float("inf"))) <= float(selected_lead.dRel) + LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN and + float(getattr(lead, "vLead", 0.0)) < LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED + for lead in credible_leads + ) + return None if stopped_conflict else selected_lead + + def get_vehicle_far_follow_slew_target(self, v_ego, prev_target, target, output_should_stop, panic_bypass): + if self.far_follow_brake_slew_rate <= 0.0 or self.far_follow_release_slew_rate <= 0.0 or output_should_stop or panic_bypass: + self.far_follow_output_slew_active = False + return target + + centered_leads = [ + lead for lead in (self.lead_one, self.lead_two) + if bool(getattr(lead, "status", False)) and + abs(float(getattr(lead, "yRel", 0.0))) <= VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET + ] + safe_far_follow = bool(centered_leads and float(v_ego) >= VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED) + for lead in centered_leads: + distance = float(getattr(lead, "dRel", 0.0)) + closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", v_ego))) + ttc = distance / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") + headway = distance / max(float(v_ego), 1e-3) + safe_far_follow &= bool( + distance >= max(VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE, + VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME * float(v_ego)) and + headway >= VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY and + ttc >= VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC + ) + + slew_was_active = self.far_follow_output_slew_active + self.far_follow_output_slew_active = safe_far_follow + if not safe_far_follow or not slew_was_active: + return target + + return float(np.clip( + target, + float(prev_target) - self.far_follow_brake_slew_rate * self.dt, + float(prev_target) + self.far_follow_release_slew_rate * self.dt, + )) + @staticmethod def is_radar_standstill_gap_settle_candidate(lead, v_ego, target_gap, active=False): if lead is None or not lead.status or not bool(getattr(lead, "radar", False)): @@ -2172,9 +2295,11 @@ class LongitudinalPlanner: )) return -max(0.0, cap_decel - relax_decel) - def update_experimental_release_accel_state(self, experimental_mode, now_t): + def update_experimental_release_accel_state(self, experimental_mode, now_t, v_ego=None): if self.prev_experimental_mode is True and not experimental_mode: - self.experimental_release_accel_until = now_t + EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME + low_speed_release = v_ego is not None and float(v_ego) < EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_THRESHOLD + hold_time = EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME if low_speed_release else EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME + self.experimental_release_accel_until = now_t + hold_time elif experimental_mode: self.experimental_release_accel_until = 0.0 self.prev_experimental_mode = bool(experimental_mode) @@ -2804,7 +2929,7 @@ class LongitudinalPlanner: # Lead stability estimation and recent-brake timer now_t = time.monotonic() - self.update_experimental_release_accel_state(experimental_mode, now_t) + self.update_experimental_release_accel_state(experimental_mode, now_t, scene_v_ego) # relative speed (ego - lead) positive when closing v_rel = (v_ego - self.lead_one.vLead) if lead_one_active else 0.0 if self.prev_lead_dist is None: @@ -3169,6 +3294,29 @@ class LongitudinalPlanner: ) depart_safety_veto = (not bool(getattr(starpilot_toggles, "radar_takeoffs", False)) and self.has_offcenter_radar_depart_conflict(sm)) + safe_depart_release_hold_lead = self.get_safe_depart_release_hold_lead(float(sm['carState'].vEgo)) + depart_release_hold_context = bool( + lead_control_active and + float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED and + not depart_safety_veto and + not bool(getattr(sm['carState'], 'brakePressed', False)) and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + safe_depart_release_hold_lead is not None + ) + if depart_release_hold_context: + self.lead_depart_release_candidate_elapsed = min( + LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME, + self.lead_depart_release_candidate_elapsed + self.dt, + ) + else: + self.lead_depart_release_candidate_elapsed = 0.0 + self.lead_depart_release_pending = False + self.lead_depart_release_hold_remaining = 0.0 + + if self.lead_depart_release_hold_remaining > 0.0: + self.lead_depart_release_hold_remaining = max(0.0, self.lead_depart_release_hold_remaining - self.dt) + depart_release_hold_active = bool(depart_release_hold_context and self.lead_depart_release_hold_remaining > 0.0) if ( lead_control_active and sm['carState'].standstill and @@ -3214,7 +3362,10 @@ class LongitudinalPlanner: standstill_stopped_lead_guard_cap = None standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): - release_ready = bool(lead_depart_ready or confident_depart_ready or slow_creep_depart_ready or radar_gap_settle_active) + release_ready = bool( + lead_depart_ready or confident_depart_ready or slow_creep_depart_ready or + radar_gap_settle_active or depart_release_hold_active + ) standstill_stopped_lead_guard_caps = [ cap for cap in ( self.get_standstill_stopped_lead_guard_cap( @@ -3259,6 +3410,25 @@ class LongitudinalPlanner: output_a_target = max(output_a_target, depart_min_accel) self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME + if depart_release_hold_context and bool(self.output_should_stop) and not bool(output_should_stop): + self.lead_depart_release_pending = True + if ( + depart_release_hold_context and + self.lead_depart_release_pending and + not bool(output_should_stop) and + self.lead_depart_release_candidate_elapsed >= LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME + ): + self.lead_depart_release_hold_remaining = LEAD_DEPART_RELEASE_HOLD_TIME + self.lead_depart_release_pending = False + depart_release_hold_active = True + elif bool(output_should_stop) and self.lead_depart_release_hold_remaining <= 0.0: + self.lead_depart_release_pending = False + + if depart_release_hold_active: + vision_low_speed_stop_active = False + output_should_stop = False + self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME + if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME @@ -3660,6 +3830,17 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, experimental_release_accel_target) output_a_target = min(output_a_target, experimental_release_accel_target) + if depart_release_hold_active: + output_a_target = max(output_a_target, STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL) + + output_a_target = self.get_vehicle_far_follow_slew_target( + scene_v_ego, + prev_output_a_target, + output_a_target, + bool(output_should_stop or vision_low_speed_stop_active), + panic_bypass, + ) + if radar_gap_settle_active: output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL output_should_stop = False diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py new file mode 100644 index 000000000..c9647ba17 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -0,0 +1,11 @@ +HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 +HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 + + +def get_far_follow_output_slew_rates(CP): + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G": + return ( + HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE, + HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE, + ) + return 0.0, 0.0 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2a44188fd..35d43b9de 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -13,6 +13,7 @@ from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.nissan.values import CAR as NISSAN from opendbc.car.gm.values import CAR as GM from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.subaru.values import CAR as SUBARU from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle @@ -25,6 +26,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( clear_flm_runtime_overrides, get_flm_runtime_overrides, get_hkg_canfd_base_friction_threshold, + get_subaru_impreza_pid_output_scale, normalize_flm_overrides, set_flm_runtime_overrides, ) @@ -62,6 +64,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_rav4_prime_friction_scale, get_rav4_prime_friction_threshold, get_rav4_prime_output_taper_scale, + get_lexus_is_ff_scale, get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, @@ -754,6 +757,17 @@ class TestLatControl: assert right_turn_in > left_turn_in > base assert base > left_unwind >= right_unwind + def test_lexus_is_ff_scale_curve(self): + steady_left = get_lexus_is_ff_scale(0.6, 0.0, 22.0) + turn_in_left = get_lexus_is_ff_scale(0.6, 0.5, 22.0) + unwind_left = get_lexus_is_ff_scale(0.6, -0.5, 22.0) + unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 22.0) + low_speed_unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 5.0) + assert steady_left == 1.0 + assert turn_in_left == 1.0 + assert unwind_right < unwind_left < steady_left + assert unwind_right < low_speed_unwind_right < 1.0 + def test_volt_plexy_friction_threshold_curve(self): base = get_gm_base_friction_threshold(6.0) left_turn_in = get_volt_plexy_friction_threshold(6.0, 0.7, 0.8) @@ -956,6 +970,26 @@ class TestLatControl: assert controller.pid._k_p[1] == pytest.approx([value * 2.0 for value in base_kp_v]) assert controller.pid._k_i[1] == pytest.approx([value * 1.25 for value in base_ki_v]) + def test_subaru_impreza_pid_output_scale_preserves_small_errors(self): + assert get_subaru_impreza_pid_output_scale(0.0) == 1.0 + assert get_subaru_impreza_pid_output_scale(0.75) == 1.0 + assert get_subaru_impreza_pid_output_scale(2.0) < 1.0 + assert get_subaru_impreza_pid_output_scale(4.0) == pytest.approx(0.58) + assert get_subaru_impreza_pid_output_scale(-4.0) == pytest.approx(0.58) + + def test_subaru_impreza_pid_output_taper_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(SUBARU.SUBARU_IMPREZA) + CS.steeringAngleDeg = 3.0 + tapered_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles) + + monkeypatch.setattr(latcontrol_pid, "get_subaru_impreza_pid_output_scale", lambda _error: 1.0) + base_controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(SUBARU.SUBARU_IMPREZA) + CS.steeringAngleDeg = 3.0 + base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + assert abs(tapered_output) < abs(base_output) + def test_modified_civic_b_torque_path_uses_fixed_friction_threshold(self, monkeypatch): CarInterface = interfaces[HONDA.HONDA_CIVIC_BOSCH] CP = CarInterface.get_non_essential_params(HONDA.HONDA_CIVIC_BOSCH) @@ -1119,6 +1153,22 @@ class TestLatControl: assert lac_log.active assert calls == 1 + def test_lexus_is_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.LEXUS_IS) + calls = 0 + + def record_ff_scale(*_args): + nonlocal calls + calls += 1 + return 1.0 + + monkeypatch.setattr(latcontrol_torque, "get_lexus_is_ff_scale", record_ff_scale) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + assert calls == 1 + def test_kia_ev6_ff_scale_curve(self): clear_flm_runtime_overrides() assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 03d63c342..5e50e1d60 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -100,6 +100,107 @@ def test_mpc_panic_bypass_immediately_removes_duplicate_vision_filter(): assert mpc.lead_v_filter.x == pytest.approx(10.0) +def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow(): + v_ego = 24.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) + planner.lead_two = make_lead(status=False) + + initial = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=0.0, target=-0.6, output_should_stop=False, panic_bypass=False, + ) + + assert initial == pytest.approx(-0.6) + + smoothed = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=initial, target=0.4, output_should_stop=False, panic_bypass=False, + ) + assert smoothed == pytest.approx(-0.5) + + +@pytest.mark.parametrize("d_rel,v_lead,output_should_stop,panic_bypass", [ + (20.0, 20.0, False, False), + (35.0, 18.0, False, False), + (58.0, 20.0, True, False), + (58.0, 20.0, False, True), +]) +def test_hrv_far_follow_output_slew_bypasses_urgent_scenes(d_rel, v_lead, output_should_stop, panic_bypass): + v_ego = 24.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) + planner.lead_two = make_lead(status=False) + planner.far_follow_output_slew_active = True + planner.lead_one.dRel = d_rel + planner.lead_one.vLead = v_lead + + target = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=0.4, target=-1.0, + output_should_stop=output_should_stop, panic_bypass=panic_bypass, + ) + + assert target == pytest.approx(-1.0) + assert not planner.far_follow_output_slew_active + + +def test_non_hrv_has_no_vehicle_far_follow_output_slew(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=24.0) + planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) + planner.lead_two = make_lead(status=False) + + target = planner.get_vehicle_far_follow_slew_target( + 24.0, prev_target=0.4, target=-1.0, output_should_stop=False, panic_bypass=False, + ) + + assert target == pytest.approx(-1.0) + + +def test_depart_release_hold_rejects_nearby_stopped_lead_conflict(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + planner.lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0) + planner.lead_two = make_lead(status=True, d_rel=5.5, v_lead=0.0, model_prob=1.0) + + assert planner.get_safe_depart_release_hold_lead(0.0) is None + + +def test_depart_release_hold_survives_stale_stop_then_cancels_for_braking_lead(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0) + lead_two = make_lead(status=False) + sm = make_sm( + 0.0, + desired_accel=0.4, + min_accel=-1.0, + experimental_mode=True, + tracking_lead=True, + lead_one=lead_one, + lead_two=lead_two, + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + sm["modelV2"].position.x = [0.0] * len(ModelConstants.T_IDXS) + sm["modelV2"].velocity.x = [0.0] * len(ModelConstants.T_IDXS) + sm["modelV2"].acceleration.x = [0.0] * len(ModelConstants.T_IDXS) + planner.lead_depart_release_candidate_elapsed = longitudinal_planner_module.LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME + planner.lead_depart_release_hold_remaining = 1.0 + + planner.update(sm, make_toggles()) + + assert not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL + + lead_one.vLead = 0.1 + lead_one.aLeadK = -0.5 + planner.update(sm, make_toggles()) + assert planner.lead_depart_release_hold_remaining == 0.0 + assert planner.output_should_stop + + def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0): model = log.ModelDataV2.new_message() model.init('leadsV3', 3) @@ -475,6 +576,42 @@ def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_cas assert far_cap is None +def test_vision_untracked_slow_lead_cap_starts_earlier_for_high_confidence_rav4_approach(): + v_ego = 21.4 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + route_like_lead = make_lead(status=True, d_rel=68.3, v_lead=18.3, radar=False, model_prob=0.95) + + route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) + + assert route_cap is not None + assert -0.6 < route_cap < -0.2 + + +def test_vision_untracked_slow_lead_cap_catches_near_rav4_lead_before_tracking(): + v_ego = 21.3 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + route_like_lead = make_lead(status=True, d_rel=41.1, v_lead=19.2, radar=False, model_prob=0.90) + + route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) + + assert route_cap is not None + assert -0.35 < route_cap <= -0.1 + + +def test_vision_untracked_slow_lead_relaxed_entry_requires_centered_lead(): + v_ego = 21.4 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + off_path_lead = make_lead(status=True, d_rel=68.3, v_lead=18.3, radar=False, model_prob=0.99, y_rel=1.5) + + assert planner.get_vision_untracked_slow_lead_cap(off_path_lead, v_ego, -1.0) is None + + def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_before_raw_close_lead(): v_ego = 21.48 @@ -2874,16 +3011,48 @@ def test_experimental_release_state_arms_only_on_falling_edge(): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP) - planner.update_experimental_release_accel_state(True, 10.0) + planner.update_experimental_release_accel_state(True, 10.0, v_ego=23.96) assert planner.experimental_release_accel_until == 0.0 - planner.update_experimental_release_accel_state(False, 10.1) - assert planner.experimental_release_accel_until > 10.1 + planner.update_experimental_release_accel_state(False, 10.1, v_ego=23.96) + assert planner.experimental_release_accel_until == pytest.approx( + 10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME + ) planner.update_experimental_release_accel_state(True, 10.2) assert planner.experimental_release_accel_until == 0.0 +def test_experimental_release_state_holds_longer_at_low_speed(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + + planner.update_experimental_release_accel_state(True, 10.0, v_ego=6.75) + planner.update_experimental_release_accel_state(False, 10.1, v_ego=6.75) + + assert planner.experimental_release_accel_until == pytest.approx( + 10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME + ) + + +def test_experimental_release_accel_transition_damps_low_speed_slow_lead_handoff(): + v_ego = 6.75 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=11.0, v_lead=6.8, a_lead=0.0, radar=False, model_prob=0.99) + + target = planner.get_experimental_release_accel_target( + lead, + v_ego, + 1.13, + prev_output_a_target=-0.25, + output_a_target=-0.03, + release_active=True, + ) + + assert target == pytest.approx(-0.19) + + def test_experimental_release_accel_transition_damps_moving_lead_handoff(): v_ego = 23.96 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)