mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-22 16:53:45 +08:00
rolos
This commit is contained in:
firestar5683
@@ -12,6 +12,7 @@ from openpilot.common.swaglog import cloudlog
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
from opendbc.car.honda.values import CAR as HONDA_CAR
|
||||
from opendbc.car.nissan.values import CAR as NISSAN_CAR
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, clip_curvature, get_lateral_active
|
||||
@@ -23,6 +24,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
|
||||
LatControlTorque,
|
||||
get_bolt_2017_steer_ratio_scale,
|
||||
get_honda_accord_steer_ratio_scale,
|
||||
)
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
|
||||
@@ -394,10 +396,15 @@ class Controls:
|
||||
lp = self.sm['liveParameters']
|
||||
x = max(lp.stiffnessFactor, 0.1)
|
||||
sr = max(lp.steerRatio, 0.1)
|
||||
custom_accord_ratio = getattr(self.starpilot_toggles, "steerRatio", self.CP.steerRatio)
|
||||
accord_ratio_is_explicit = getattr(self.starpilot_toggles, "use_custom_steerRatio", False) and \
|
||||
abs(custom_accord_ratio - self.CP.steerRatio) > 0.01
|
||||
if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017:
|
||||
sr *= get_bolt_2017_steer_ratio_scale(CS.vEgo)
|
||||
elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021:
|
||||
sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE
|
||||
elif self.CP.carFingerprint == HONDA_CAR.HONDA_ACCORD and not accord_ratio_is_explicit:
|
||||
sr *= get_honda_accord_steer_ratio_scale(CS.vEgo)
|
||||
self.VM.update_params(x, sr)
|
||||
|
||||
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
|
||||
|
||||
@@ -483,14 +483,24 @@ class LatControlTorque(LatControl):
|
||||
ff *= get_genesis_g70_unwind_ff_scale(
|
||||
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
|
||||
)
|
||||
if kia_carnival_active:
|
||||
ff *= get_kia_carnival_unwind_ff_scale(
|
||||
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
|
||||
)
|
||||
if ioniq_6_active:
|
||||
vehicle_friction_jerk_deadzone = (
|
||||
IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_ioniq_6_2025 else IONIQ_6_FRICTION_JERK_DEADZONE
|
||||
)
|
||||
elif ioniq_5_active:
|
||||
vehicle_friction_jerk_deadzone = get_ioniq_5_friction_jerk_deadzone(CS.vEgo, setpoint)
|
||||
elif prius_active:
|
||||
vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone(CS.vEgo, setpoint)
|
||||
elif genesis_g70_active:
|
||||
vehicle_friction_jerk_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint)
|
||||
elif kia_carnival_active:
|
||||
vehicle_friction_jerk_deadzone = get_kia_carnival_friction_jerk_deadzone(
|
||||
CS.vEgo, setpoint, desired_lateral_jerk,
|
||||
)
|
||||
else:
|
||||
vehicle_friction_jerk_deadzone = 0.0
|
||||
friction_jerk_deadzone = get_center_chatter_friction_jerk_deadzone(
|
||||
|
||||
@@ -73,6 +73,7 @@ BOLT_2017_CARS = (
|
||||
GM_CAR.CHEVROLET_BOLT_CC_2017,
|
||||
)
|
||||
BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
|
||||
HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33
|
||||
VOLT_STANDARD_CARS = (
|
||||
GM_CAR.CHEVROLET_VOLT,
|
||||
GM_CAR.CHEVROLET_VOLT_2019,
|
||||
@@ -547,6 +548,24 @@ KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK = 0.45
|
||||
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH = 0.15
|
||||
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF = 1.20
|
||||
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH = 0.20
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.34
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 15.0
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.0
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF = 23.0
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH = 2.0
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.18
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.65
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.25
|
||||
KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX = 0.45
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED = 15.0
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH = 2.0
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF = 23.0
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0
|
||||
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.20
|
||||
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
|
||||
KIA_CARNIVAL_UNWIND_FF_JERK = 0.65
|
||||
KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.25
|
||||
|
||||
TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44
|
||||
TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28
|
||||
@@ -688,6 +707,11 @@ IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40
|
||||
IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
|
||||
IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40
|
||||
IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_MAX = 0.30
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_LAT = 1.25
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.35
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED = 18.0
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.0
|
||||
|
||||
IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
|
||||
IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16
|
||||
@@ -1082,9 +1106,6 @@ TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
|
||||
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
|
||||
|
||||
LEXUS_IS_PHASE_SCALE = 0.10
|
||||
# The Lexus route still fell short during a clean high-speed turn-in while
|
||||
# already at the controller limit. Keep this correction small and phase-gated
|
||||
# so straight-line behavior and unwind tuning are unchanged.
|
||||
LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
|
||||
LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
|
||||
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10
|
||||
@@ -1886,6 +1907,10 @@ def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float:
|
||||
return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * _bolt_2017_high_speed_factor(v_ego))
|
||||
|
||||
|
||||
def get_honda_accord_steer_ratio_scale(_v_ego: float) -> float:
|
||||
return HONDA_ACCORD_STEER_RATIO_SCALE
|
||||
|
||||
|
||||
def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH)
|
||||
return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window)
|
||||
@@ -2528,6 +2553,41 @@ def get_kia_carnival_highway_transition_output_scale(desired_lateral_accel: floa
|
||||
return 1.0 - (KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
|
||||
|
||||
|
||||
def get_kia_carnival_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float,
|
||||
desired_lateral_jerk: float) -> float:
|
||||
"""Reduce abrupt friction reversals during mid-speed curve exits only."""
|
||||
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED) /
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
|
||||
speed_cutoff = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF - v_ego) /
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH)
|
||||
center_weight = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH)
|
||||
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK) /
|
||||
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH)
|
||||
return KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX * speed_weight * speed_cutoff * center_weight * jerk_weight
|
||||
|
||||
|
||||
def get_kia_carnival_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
|
||||
desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
"""Remove stale turn feedforward when the measured response carries through an unwind."""
|
||||
if setpoint * desired_lateral_jerk >= 0.0:
|
||||
return 1.0
|
||||
|
||||
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
|
||||
if overshoot <= 0.0:
|
||||
return 1.0
|
||||
|
||||
speed_weight = (_sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FF_SPEED) /
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH) *
|
||||
_sigmoid((KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF - v_ego) /
|
||||
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH))
|
||||
overshoot_weight = _sigmoid((overshoot - KIA_CARNIVAL_UNWIND_FF_OVERSHOOT) /
|
||||
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH)
|
||||
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FF_JERK) /
|
||||
KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH)
|
||||
return 1.0 - (KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX * speed_weight * overshoot_weight * jerk_weight)
|
||||
|
||||
|
||||
def _tucson_4th_gen_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
|
||||
speed_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) / TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH)
|
||||
center_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH)
|
||||
@@ -3029,6 +3089,15 @@ def get_ioniq_5_low_speed_output_limit(desired_lateral_accel: float,
|
||||
return float(np.clip(limit, IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
|
||||
|
||||
|
||||
def get_ioniq_5_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
|
||||
"""Suppress high-speed friction reversals without reducing steady-turn torque."""
|
||||
speed_weight = _ioniq_5_sigmoid((max(v_ego, 0.0) - IONIQ_5_FRICTION_JERK_DEADZONE_SPEED) /
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
|
||||
curve_weight = _ioniq_5_sigmoid((IONIQ_5_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
|
||||
IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH)
|
||||
return IONIQ_5_FRICTION_JERK_DEADZONE_MAX * speed_weight * curve_weight
|
||||
|
||||
|
||||
def _ioniq_ev_old_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
|
||||
@@ -84,6 +84,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_genesis_gv70_high_speed_error_scale,
|
||||
get_genesis_gv70_unwind_ff_scale,
|
||||
get_elantra_non_scc_ff_scale,
|
||||
get_honda_accord_steer_ratio_scale,
|
||||
get_palisade_ff_scale,
|
||||
get_palisade_center_output_scale,
|
||||
get_palisade_center_taper_scale,
|
||||
@@ -113,6 +114,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_ioniq_5_friction_scale,
|
||||
get_ioniq_5_friction_threshold,
|
||||
get_ioniq_5_center_taper_scale,
|
||||
get_ioniq_5_friction_jerk_deadzone,
|
||||
get_ioniq_5_low_speed_output_limit,
|
||||
get_ioniq_ev_old_center_taper_scale,
|
||||
get_ioniq_ev_old_ff_scale,
|
||||
@@ -132,8 +134,10 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_kia_forte_ff_scale,
|
||||
get_kia_carnival_center_taper_scale,
|
||||
get_kia_carnival_friction_center_fade_scale,
|
||||
get_kia_carnival_friction_jerk_deadzone,
|
||||
get_kia_carnival_friction_threshold,
|
||||
get_kia_carnival_highway_transition_output_scale,
|
||||
get_kia_carnival_unwind_ff_scale,
|
||||
get_kia_stinger_2022_center_taper_scale,
|
||||
get_kia_stinger_2022_friction_threshold,
|
||||
get_tucson_4th_gen_center_taper_scale,
|
||||
@@ -668,6 +672,30 @@ class TestLatControl:
|
||||
assert low_speed_abrupt > 0.99
|
||||
assert large_curve_abrupt > 0.96
|
||||
|
||||
def test_kia_carnival_unwind_friction_jerk_deadzone_is_mid_speed_and_center_gated(self):
|
||||
low_speed = get_kia_carnival_friction_jerk_deadzone(8.5, 0.0, 1.5)
|
||||
mid_speed_center = get_kia_carnival_friction_jerk_deadzone(18.0, 0.0, 1.5)
|
||||
mid_speed_curve = get_kia_carnival_friction_jerk_deadzone(18.0, 0.8, 1.5)
|
||||
high_speed = get_kia_carnival_friction_jerk_deadzone(30.0, 0.0, 1.5)
|
||||
calm_transition = get_kia_carnival_friction_jerk_deadzone(18.0, 0.0, 0.2)
|
||||
|
||||
assert low_speed < 0.02
|
||||
assert mid_speed_center > 0.18
|
||||
assert mid_speed_curve < 0.05
|
||||
assert high_speed < 0.05
|
||||
assert calm_transition < 0.05
|
||||
|
||||
def test_kia_carnival_unwind_ff_scale_only_reduces_overshoot(self):
|
||||
steady_turn = get_kia_carnival_unwind_ff_scale(0.80, 0.90, 0.60, 18.0)
|
||||
clean_unwind = get_kia_carnival_unwind_ff_scale(0.20, 0.20, -1.5, 18.0)
|
||||
overshooting_unwind = get_kia_carnival_unwind_ff_scale(0.20, 0.90, -1.5, 18.0)
|
||||
highway_overshoot = get_kia_carnival_unwind_ff_scale(0.20, 0.90, -1.5, 30.0)
|
||||
|
||||
assert steady_turn == pytest.approx(1.0)
|
||||
assert clean_unwind == pytest.approx(1.0)
|
||||
assert overshooting_unwind < 0.70
|
||||
assert highway_overshoot > overshooting_unwind
|
||||
|
||||
def test_genesis_g90_ff_scale_curve(self):
|
||||
assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
assert get_genesis_g90_ff_scale(0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(-0.5, 0.0, 20.0)
|
||||
@@ -904,6 +932,16 @@ class TestLatControl:
|
||||
assert unwind_right_scale <= unwind_left_scale
|
||||
assert get_ioniq_5_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0)
|
||||
|
||||
def test_ioniq_5_friction_jerk_deadzone_is_high_speed_curve_gated(self):
|
||||
low_speed = get_ioniq_5_friction_jerk_deadzone(8.0, 0.9)
|
||||
high_speed_center = get_ioniq_5_friction_jerk_deadzone(25.0, 0.0)
|
||||
high_speed_curve = get_ioniq_5_friction_jerk_deadzone(25.0, 0.9)
|
||||
high_lateral_accel = get_ioniq_5_friction_jerk_deadzone(25.0, 2.0)
|
||||
|
||||
assert low_speed < 0.02
|
||||
assert high_speed_center > high_speed_curve > 0.0
|
||||
assert high_lateral_accel < high_speed_curve
|
||||
|
||||
def test_rav4_prime_phase_shaping(self):
|
||||
left_turn_in = get_rav4_prime_ff_scale(1.0, 0.8, 13.0)
|
||||
right_turn_in = get_rav4_prime_ff_scale(-1.0, -0.8, 13.0)
|
||||
@@ -1665,6 +1703,11 @@ 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_honda_accord_steer_ratio_calibration(self):
|
||||
expected_scale = 14.0 / 16.33
|
||||
assert get_honda_accord_steer_ratio_scale(0.0) == pytest.approx(expected_scale)
|
||||
assert get_honda_accord_steer_ratio_scale(20.0) == pytest.approx(expected_scale)
|
||||
|
||||
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
|
||||
|
||||
@@ -63,6 +63,8 @@ def make_toggles(**overrides):
|
||||
"speed_limit_priority_highest": False,
|
||||
"speed_limit_priority_lowest": False,
|
||||
"vision_speed_limit_detection": False,
|
||||
"vision_speed_limit_low_limit_filter": False,
|
||||
"vision_speed_limit_low_limit_threshold": mph(25),
|
||||
}
|
||||
defaults.update(overrides)
|
||||
return SimpleNamespace(**defaults)
|
||||
@@ -96,6 +98,107 @@ def mph(value):
|
||||
return value * CV.MPH_TO_MS
|
||||
|
||||
|
||||
@pytest.mark.parametrize("limit_mph", [15, 25])
|
||||
def test_low_vision_limit_filter_blocks_configured_boundary(limit_mph):
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
vision_speed_limit_detection=True,
|
||||
vision_speed_limit_low_limit_filter=True,
|
||||
vision_speed_limit_low_limit_threshold=mph(25),
|
||||
)
|
||||
try:
|
||||
controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(limit_mph))
|
||||
sm = make_sm(gas_pressed=False, v_cruise_kph=25 * CV.MPH_TO_KPH)
|
||||
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(25), mph(20), sm)
|
||||
|
||||
assert controller.vision_limit == pytest.approx(mph(limit_mph))
|
||||
assert controller.target == 0
|
||||
assert controller.source == "None"
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_low_vision_limit_filter_allows_limit_above_threshold():
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
vision_speed_limit_detection=True,
|
||||
vision_speed_limit_low_limit_filter=True,
|
||||
vision_speed_limit_low_limit_threshold=mph(25),
|
||||
)
|
||||
try:
|
||||
controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(30))
|
||||
sm = make_sm(gas_pressed=False, v_cruise_kph=30 * CV.MPH_TO_KPH)
|
||||
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(25), sm)
|
||||
|
||||
assert controller.target == pytest.approx(mph(30))
|
||||
assert controller.source == "Vision"
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_low_vision_limit_filter_is_action_only_for_display():
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
vision_speed_limit_detection=True,
|
||||
vision_speed_limit_low_limit_filter=True,
|
||||
vision_speed_limit_low_limit_threshold=mph(25),
|
||||
)
|
||||
try:
|
||||
controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15))
|
||||
sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH)
|
||||
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(20), mph(15), sm, display_only=True)
|
||||
|
||||
assert controller.target == pytest.approx(mph(15))
|
||||
assert controller.source == "Vision"
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_low_vision_limit_filter_does_not_filter_dashboard_source():
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
speed_limit_priority2="Dashboard",
|
||||
vision_speed_limit_detection=True,
|
||||
vision_speed_limit_low_limit_filter=True,
|
||||
vision_speed_limit_low_limit_threshold=mph(25),
|
||||
)
|
||||
try:
|
||||
controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15))
|
||||
sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH)
|
||||
|
||||
controller.update_limits(mph(15), datetime.now(timezone.utc), False, mph(20), mph(15), sm)
|
||||
|
||||
assert controller.target == pytest.approx(mph(15))
|
||||
assert controller.source == "Dashboard"
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_low_vision_limit_filter_does_not_restore_filtered_vision_fallback():
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
slc_fallback_previous_speed_limit=True,
|
||||
vision_speed_limit_detection=True,
|
||||
vision_speed_limit_low_limit_filter=True,
|
||||
vision_speed_limit_low_limit_threshold=mph(25),
|
||||
)
|
||||
try:
|
||||
controller.previous_source = "Vision"
|
||||
controller.previous_target = mph(15)
|
||||
controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15))
|
||||
sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH)
|
||||
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(20), mph(15), sm)
|
||||
|
||||
assert controller.target == 0
|
||||
assert controller.source == "None"
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_large_vision_delta_requires_three_detector_frames():
|
||||
controller = make_controller(
|
||||
speed_limit_priority1="Vision",
|
||||
|
||||
Reference in New Issue
Block a user