mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-24 18:52:09 +08:00
What If You Lived On The Bottom Of A Rubber Duck?
This commit is contained in:
firestar5683
@@ -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
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)}
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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")
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user