mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-19 23:34:14 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
603f9b56f9fa721de1ee3c83d41578e97600063f
StarPilot/selfdrive/controls/tests/test_longcontrol.py
T
firestar5683 603f9b56f9 BURN THE WITCH
2026-08-14 22:36:56 -05:00

1465 lines
48 KiB
Python
Raw Blame History

from types import SimpleNamespace
from cereal import car
import pytest
import openpilot.selfdrive.controls.lib.longcontrol as longcontrol
import openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes as vehicle_tunes
from opendbc.car.gm.values import CAR, GMFlags
from opendbc.car.subaru.values import CAR as SUBARU_CAR
from opendbc.car.toyota.values import CAR as TOYOTA_CAR
from openpilot.selfdrive.controls.lib.longcontrol import (
LongControl,
LongCtrlState,
long_control_state_trans,
)
def make_toggles(**overrides):
defaults = {
"custom_accel_profile": False,
"startAccel": 1.5,
"stopAccel": -0.5,
"stoppingDecelRate": 0.8,
"vEgoStarting": 0.5,
"vEgoStopping": 0.5,
}
defaults.update(overrides)
return SimpleNamespace(**defaults)
def make_longcontrol_cp(**overrides):
CP = car.CarParams.new_message()
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.0]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.0]
CP.longitudinalTuning.kfDEPRECATED = 1.0
for key, value in overrides.items():
setattr(CP, key, value)
return CP
class TestLongControlStateTransition:
def test_stay_stopped(self):
CP = car.CarParams.new_message()
toggles = make_toggles()
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=True, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=True, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles,
allow_stopping_release=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.pid
active = False
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.off
def test_engage():
CP = car.CarParams.new_message()
toggles = make_toggles()
active = True
current_state = LongCtrlState.off
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=True, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=True, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.pid
def test_starting():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
toggles = make_toggles(vEgoStarting=0.5)
active = True
current_state = LongCtrlState.starting
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.starting
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
assert next_state == LongCtrlState.pid
def test_stopping_release_hysteresis_blocks_immediate_launch():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
toggles = make_toggles(vEgoStarting=0.5)
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.0,
should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles,
allow_stopping_release=False)
assert next_state == LongCtrlState.stopping
def test_stopping_release_allows_launch_while_cruise_standstill_latched():
CP = car.CarParams.new_message()
toggles = make_toggles(vEgoStarting=0.5)
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.0,
should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles,
allow_stopping_release=True)
assert next_state == LongCtrlState.pid
def test_starting_accel_unchanged_when_custom_profile_disabled():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.1,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == 1.5
def test_starting_accel_uses_small_planner_target_for_lead_gap_settle():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.18,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
has_lead=True,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == pytest.approx(0.18)
def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.1,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5, custom_accel_profile=True),
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == 0.1
def test_starting_accel_obeys_a_target_cap_when_traffic_mode_enabled():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
# Large manually-tuned startAccel override (e.g. 3.5) should not fire a raw
# launch kick while Traffic Mode is active; output must track the soft a_target.
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.10,
should_stop=False,
accel_limits=(-3.0, 4.0),
starpilot_toggles=make_toggles(startAccel=3.5),
traffic_mode_enabled=True,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == pytest.approx(1.10)
def test_starting_accel_uses_raw_start_accel_when_no_profile_ceiling():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
# No usable profile ceiling published (e.g. a stale/zero starpilotPlan) -> keep the
# raw StartAccel shove so a publish gap never zeroes out the launch.
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.10,
should_stop=False,
accel_limits=(-3.0, 4.0),
starpilot_toggles=make_toggles(startAccel=3.5),
traffic_mode_enabled=False,
profile_max_accel=0.0,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == pytest.approx(3.5)
def test_starting_accel_capped_by_profile_ceiling():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
# A large StartAccel override (3.5) must be capped to the selected profile's launch
# ceiling (e.g. Eco = 1.5) so a soft profile launches soft.
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.10,
should_stop=False,
accel_limits=(-3.0, 4.0),
starpilot_toggles=make_toggles(startAccel=3.5),
traffic_mode_enabled=False,
profile_max_accel=1.5,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == pytest.approx(1.5)
def test_starting_accel_keeps_start_accel_shove_below_profile_ceiling():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
# StartAccel below the profile ceiling (e.g. Sport+ = 3.5) is preserved in full -
# the cap only trims launches that exceed the profile, it does not weaken the shove.
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.10,
should_stop=False,
accel_limits=(-3.0, 4.0),
starpilot_toggles=make_toggles(startAccel=1.5),
traffic_mode_enabled=False,
profile_max_accel=3.5,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel == pytest.approx(1.5)
def test_bolt_acc_pedal_starting_handoff_keeps_small_positive_command():
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kpV = [0.8]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=1.5, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.55,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.35),
has_lead=True,
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel == pytest.approx(0.188, abs=0.01)
def test_tesla_pedal_override_keeps_longitudinal_state_warm_for_release():
CP = make_longcontrol_cp(
brand="tesla",
carFingerprint="TESLA_MODEL_3",
startingState=True,
vEgoStarting=0.35,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
lc.last_output_accel = 0.8
CS = car.CarState.new_message(vEgo=12.0, aEgo=0.8, brakePressed=False, gasPressed=True)
CS.cruiseState.standstill = False
override_output = lc.update(
active=False,
CS=CS,
a_target=0.6,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
pedal_override=True,
)
assert override_output == 0.0
assert lc.long_control_state == LongCtrlState.pid
assert lc.last_output_accel == pytest.approx(0.8)
CS.gasPressed = False
release_output = lc.update(
active=True,
CS=CS,
a_target=0.6,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
)
assert release_output >= 0.6
def test_tesla_pedal_release_guard_blocks_mild_regen_pulse():
CP = make_longcontrol_cp(
brand="tesla",
carFingerprint="TESLA_MODEL_3",
startingState=True,
vEgoStarting=0.35,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
CS = car.CarState.new_message(vEgo=12.0, aEgo=0.8, brakePressed=False, gasPressed=True)
CS.cruiseState.standstill = False
lc.update(False, CS, -0.2, False, (-3.0, 2.0), make_toggles(), pedal_override=True)
CS.gasPressed = False
release_output = lc.update(True, CS, -0.2, False, (-3.0, 2.0), make_toggles())
assert release_output == 0.0
@pytest.mark.parametrize(("a_target", "should_stop"), ((-0.2, False), (0.55, True)))
def test_bolt_acc_pedal_starting_handoff_never_overrides_stop_request(a_target, should_stop):
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=a_target,
should_stop=should_stop,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.35),
has_lead=True,
)
assert output_accel <= 0.0
def test_bolt_acc_pedal_starting_handoff_floor_clears_when_lead_brakes_again():
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kpV = [0.8]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=1.5, brakePressed=False)
CS.cruiseState.standstill = False
toggles = make_toggles(vEgoStarting=0.35)
launch_output = lc.update(True, CS, 0.55, False, (-3.0, 2.0), toggles, has_lead=True)
assert launch_output > 0.0
CS.vEgo = 0.5
CS.aEgo = 0.0
brake_output = lc.update(True, CS, -0.5, True, (-3.0, 2.0), toggles, has_lead=True)
assert lc.long_control_state == LongCtrlState.stopping
assert brake_output < 0.0
assert lc.vehicle_tuning.bolt_start_handoff_frames == 0
def test_update_requires_sustained_moderate_positive_target_to_leave_stopping():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
for _ in range(release_frames - 1):
output_accel = lc.update(
active=True,
CS=CS,
a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.stopping
assert output_accel <= 0.0
lc.update(
active=True,
CS=CS,
a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.starting
def test_update_releases_stopping_immediately_on_strong_positive_target():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel > 0.0
def test_update_releases_stopping_on_small_sustained_positive_target():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
for _ in range(release_frames - 1):
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.16,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.stopping
assert output_accel <= 0.0
lc.update(
active=True,
CS=CS,
a_target=0.16,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.starting
def test_corolla_tss2_stop_release_ramps_positive_target():
CP = make_longcontrol_cp(
brand="toyota",
carFingerprint=TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
tuning.reset()
first_target = tuning.shape_toyota_corolla_accel_target(1.5, 0.0, False, -0.15)
assert first_target < 0.0
assert first_target < 1.5
target = first_target
for _ in range(100):
target = tuning.shape_toyota_corolla_accel_target(1.5, 0.0, False, target)
assert target > 1.4
for _ in range(100):
target = tuning.shape_toyota_corolla_accel_target(1.5, 0.0, False, target)
assert target == pytest.approx(1.5, abs=0.01)
def test_corolla_tss2_target_filter_does_not_delay_hard_braking():
CP = make_longcontrol_cp(
brand="toyota",
carFingerprint=TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
tuning.shape_toyota_corolla_accel_target(1.0, 1.0, False, 0.0)
assert tuning.shape_toyota_corolla_accel_target(-1.0, 1.0, False, 0.5) == -1.0
def test_corolla_tss2_longcontrol_release_does_not_step_to_full_accel():
CP = make_longcontrol_cp(
brand="toyota",
carFingerprint=TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
lc.last_output_accel = -0.15
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.5,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.1),
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel < 0.0
def test_subaru_impreza_stop_release_caps_launch_accel():
CP = make_longcontrol_cp(
brand="subaru",
carFingerprint=SUBARU_CAR.SUBARU_IMPREZA_2020,
vEgoStarting=0.5,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=1.8,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.5),
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel == pytest.approx(vehicle_tunes.SUBARU_IMPREZA_STOP_RELEASE_MAX_ACCEL)
def test_update_releases_stopping_immediately_after_confirmed_lead_departure():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = True
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.16,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
has_lead=True,
)
assert lc.long_control_state == LongCtrlState.starting
assert output_accel > 0.0
@pytest.mark.parametrize(("should_stop", "brake_pressed"), [(True, False), (False, True)])
def test_confirmed_lead_departure_does_not_override_stop_or_driver_brake(should_stop, brake_pressed):
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=brake_pressed)
CS.cruiseState.standstill = True
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.5,
should_stop=should_stop,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
has_lead=True,
)
assert lc.long_control_state == LongCtrlState.stopping
assert output_accel <= 0.0
def test_update_releases_stopping_with_cruise_standstill_latched():
CP = car.CarParams.new_message(vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
lc.last_output_accel = -2.003
CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = True
release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
for _ in range(release_frames - 1):
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.5,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.stopping
assert output_accel <= 0.0
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.5,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(startAccel=1.5),
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel > 0.0
def test_stopping_state_follows_stronger_moving_stop_target():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.stopping
lc.last_output_accel = -1.40
CS = car.CarState.new_message(vEgo=4.0, aEgo=-1.2, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=-3.5,
should_stop=True,
accel_limits=(-3.5, 2.0),
starpilot_toggles=make_toggles(stopAccel=-0.5, stoppingDecelRate=0.8, vEgoStopping=0.5),
)
assert lc.long_control_state == LongCtrlState.stopping
assert output_accel < -1.43
def test_elantra_lead_stop_releases_stale_hard_brake_after_target_eases():
CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
assert tuning.shape_stopping_accel(-1.20, -0.25, True, 1.0, True, -0.85) == pytest.approx(-0.85)
assert tuning.shape_stopping_accel(-1.20, -1.50, True, 1.0, True, -0.85) == pytest.approx(-1.20)
assert tuning.shape_stopping_accel(-1.20, -0.25, True, 1.0, False, -0.85) == pytest.approx(-1.20)
def test_elantra_stopped_lead_handoff_holds_braking_direction_without_touching_brakes():
CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
assert tuning.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
assert tuning.cap_hyundai_elantra_lead_output(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
assert tuning.cap_hyundai_elantra_lead_output(-0.5, 1.1, False, (stopped_lead,)) == pytest.approx(-0.5)
def test_elantra_stopped_lead_handoff_releases_for_moving_lead_and_other_cars():
elantra = vehicle_tunes.LongControlVehicleTuning(
make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
)
other_car = vehicle_tunes.LongControlVehicleTuning(
make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_SONATA")
)
moving_lead = SimpleNamespace(status=True, vLead=0.8, dRel=14.0)
stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (moving_lead,)) == pytest.approx(0.14)
assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, True, (stopped_lead,)) == pytest.approx(0.14)
assert other_car.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.14)
def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.enableGasInterceptorDEPRECATED = True
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
monkeypatch.setattr(vehicle_tunes, "testing_ground", SimpleNamespace(use_2=True))
lc = LongControl(CP)
freeze = lc.vehicle_tuning.get_integrator_freeze(
lc.last_output_accel, a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0),
)
assert freeze
assert lc.vehicle_tuning.integrator_hold_frames > 0
def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.enableGasInterceptorDEPRECATED = False
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
monkeypatch.setattr(vehicle_tunes, "testing_ground", SimpleNamespace(use_2=True))
lc = LongControl(CP)
freeze = lc.vehicle_tuning.get_integrator_freeze(
lc.last_output_accel, a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0),
)
assert freeze
assert lc.vehicle_tuning.integrator_hold_frames > 0
def test_volt_cruise_integrator_releases_stale_negative_bias():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.0]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.5]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
lc.pid.i = -0.20
CS = car.CarState.new_message(vEgo=18.0, aEgo=0.0, brakePressed=False, gasPressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.0,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
has_lead=False,
)
assert lc.pid.i > -0.20
assert output_accel > -0.20
@pytest.mark.parametrize("kwargs", [
{"has_lead": True},
{"should_stop": True},
{"a_target": -0.25},
{"aEgo": 0.25},
{"vEgo": 4.0},
])
def test_volt_cruise_integrator_does_not_release_outside_settled_open_road(kwargs):
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.0]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.5]
lc = LongControl(CP)
pid = SimpleNamespace(i=-0.20)
v_ego = kwargs.get("vEgo", 18.0)
a_ego = kwargs.get("aEgo", 0.0)
a_target = kwargs.get("a_target", 0.0)
lc.vehicle_tuning.trim_volt_cruise_integrator(
pid,
a_target=a_target,
error=a_target - a_ego,
v_ego=v_ego,
should_stop=kwargs.get("should_stop", False),
has_lead=kwargs.get("has_lead", False),
)
assert pid.i == pytest.approx(-0.20)
def test_negative_target_unwinds_positive_accel_command_after_sign_flip():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
lc.last_output_accel = 1.2
lc.pid.i = 1.2
CS = car.CarState.new_message(vEgo=30.0, aEgo=0.9, brakePressed=False, gasPressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=-0.5,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel <= 0.01
def test_negative_target_unwinds_positive_accel_command_at_low_speed():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
lc.last_output_accel = 0.9
lc.pid.i = 0.9
CS = car.CarState.new_message(vEgo=1.3, aEgo=0.35, brakePressed=False, gasPressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=-1.2,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel <= 0.01
def test_negative_target_creep_guard_keeps_mild_crawl_request():
capped = LongControl._cap_positive_output_on_negative_target(
output_accel=0.18,
a_target=-0.2,
error=-0.5,
CS=car.CarState.new_message(vEgo=0.2, aEgo=0.0),
)
assert capped == pytest.approx(0.18)
def test_pedal_long_brake_bias_adds_small_negative_nudge_for_strong_decel_request():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.enableGasInterceptorDEPRECATED = True
CP.flags = 1
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
biased = lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.0, -3.0, CS)
assert biased < -1.0
assert biased == pytest.approx(-1.15, abs=0.03)
def test_pedal_long_brake_bias_does_not_touch_non_pedal_or_mild_decel():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.enableGasInterceptorDEPRECATED = False
CP.flags = 0
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.1]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
assert lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.0, -3.0, CS) == -1.0
assert lc.vehicle_tuning.apply_pedal_long_brake_bias(-0.4, -0.6, CS) == -0.4
def test_bolt_acc_pedal_friction_feedforward_preserves_regen_scaling_within_envelope():
CP = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kfDEPRECATED = 0.20
lc = LongControl(CP)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -1.8, 4.73,
) == pytest.approx(-0.36)
def test_bolt_acc_pedal_friction_feedforward_restores_full_gain_beyond_regen_envelope():
CP = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kfDEPRECATED = 0.20
lc = LongControl(CP)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -3.22, 4.73,
) == pytest.approx(-3.22)
def test_bolt_acc_pedal_friction_feedforward_blends_back_in_for_small_friction_request():
CP = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kfDEPRECATED = 0.20
lc = LongControl(CP)
pedal_regen_limit = float(vehicle_tunes.interp(20.0, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
a_target = pedal_regen_limit - 0.10
expected_gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.20, a_target, 20.0, pedal_regen_limit, 0.0)
expected = a_target * expected_gain
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, a_target, 20.0,
) == pytest.approx(expected)
def test_bolt_acc_pedal_friction_floor_holds_friction_only_authority():
pedal_regen_limit = float(vehicle_tunes.interp(9.85, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = vehicle_tunes.get_bolt_acc_pedal_friction_floor(-3.47, 9.85, pedal_regen_limit)
assert floor is not None
assert floor < pedal_regen_limit
assert floor > -3.47
def test_bolt_acc_pedal_friction_bias_applies_floor_only_on_experimental_fingerprint():
pedal_cp = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
bolt_cc_cp = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_CC_2022_2023,
)
pedal_lc = LongControl(pedal_cp)
bolt_cc_lc = LongControl(bolt_cc_cp)
CS = car.CarState.new_message(vEgo=9.85, aEgo=-2.0, brakePressed=False)
pedal_regen_limit = float(vehicle_tunes.interp(CS.vEgo, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = vehicle_tunes.get_bolt_acc_pedal_friction_floor(-3.47, CS.vEgo, pedal_regen_limit)
assert floor is not None
pedal_biased = pedal_lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.85, -3.47, CS)
bolt_cc_biased = bolt_cc_lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.85, -3.47, CS)
assert pedal_biased == pytest.approx(floor)
assert bolt_cc_biased > pedal_biased + 0.5
def test_bolt_acc_pedal_feedforward_gain_stays_base_for_mild_regen():
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -1.0, 10.0, -2.75, -0.4)
assert gain == pytest.approx(0.2)
def test_bolt_acc_pedal_feedforward_gain_restores_for_authority_gap():
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -1.83, 12.38, -2.79, -0.70)
assert gain > 0.55
def test_bolt_acc_pedal_feedforward_gain_restores_near_friction_handoff():
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -2.63, 9.35, -2.69, -1.30)
assert gain > 0.45
def test_bolt_cc_pedal_friction_feedforward_remains_fully_scaled_by_kf():
CP = make_longcontrol_cp(
brand="gm",
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_CC_2022_2023,
)
CP.longitudinalTuning.kfDEPRECATED = 0.20
lc = LongControl(CP)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -3.22, 4.73,
) == pytest.approx(-0.644)
def test_gm_stock_truck_positive_i_bleeds_on_coast_request():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_SILVERADO"
CP.enableGasInterceptorDEPRECATED = False
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.02]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.28]
lc = LongControl(CP)
lc.pid.i = 0.25
lc.last_output_accel = 0.20
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, -0.02, -0.02, CS,
)
assert lc.pid.i < 0.25
def test_gm_stock_truck_target_filter_smooths_mild_follow_reversals():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
tuning = LongControl(CP).vehicle_tuning
assert tuning.shape_gm_truck_accel_target(0.25, 20.0, False) == pytest.approx(0.25)
filtered_brake = tuning.shape_gm_truck_accel_target(-0.10, 20.0, False)
filtered_accel = tuning.shape_gm_truck_accel_target(0.25, 20.0, False)
assert -0.10 < filtered_brake < 0.25
assert filtered_brake < filtered_accel < 0.25
def test_gm_stock_truck_target_filter_uses_comfort_slew_for_mild_braking():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
tuning = LongControl(CP).vehicle_tuning
tuning.shape_gm_truck_accel_target(0.30, 25.0, False)
filtered = tuning.shape_gm_truck_accel_target(-0.10, 25.0, False)
expected = 0.30 + vehicle_tunes.DT_CTRL / (vehicle_tunes.GM_TRUCK_TARGET_FILTER_DOWN_TAU + vehicle_tunes.DT_CTRL) * (-0.40)
assert filtered == pytest.approx(expected)
assert filtered > -0.10
def test_gm_stock_truck_target_filter_bypasses_urgent_braking():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
tuning = LongControl(CP).vehicle_tuning
tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
assert tuning.shape_gm_truck_accel_target(-0.70, 20.0, False) == pytest.approx(-0.70)
tuning.reset()
tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
assert tuning.shape_gm_truck_accel_target(-0.10, 20.0, False) == pytest.approx(-0.10)
tuning.reset()
tuning.shape_gm_truck_accel_target(0.25, 20.0, False)
assert tuning.shape_gm_truck_accel_target(0.10, 20.0, True) == pytest.approx(0.10)
def test_gm_stock_truck_target_filter_bypasses_low_speed_and_other_cars():
truck_cp = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
truck_tuning = LongControl(truck_cp).vehicle_tuning
truck_tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
bolt_cp = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023,
enableGasInterceptorDEPRECATED=False,
)
bolt_tuning = LongControl(bolt_cp).vehicle_tuning
assert truck_tuning.shape_gm_truck_accel_target(-0.10, 10.0, False) == pytest.approx(-0.10)
assert bolt_tuning.shape_gm_truck_accel_target(-0.10, 20.0, False) == pytest.approx(-0.10)
def test_santa_fe_final_stop_cap_softens_only_last_kmh():
CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_SANTA_FE_2022")
tuning = LongControl(CP).vehicle_tuning
assert tuning.shape_stopping_accel(-2.0, -0.2, True, 0.2, False, -2.0) == pytest.approx(-0.30)
assert tuning.shape_stopping_accel(-2.0, -0.2, True, 1.5, False, -2.0) == pytest.approx(-2.0)
assert tuning.shape_stopping_accel(-2.0, 0.3, False, 0.2, False, -2.0) == pytest.approx(-2.0)
def test_toyota_sienna_target_filter_smooths_mild_high_speed_handoffs():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
assert tuning.shape_toyota_sienna_accel_target(0.30, 20.0, False) == pytest.approx(0.30)
filtered = tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, False)
assert -0.20 < filtered < 0.30
def test_toyota_sienna_target_filter_unwinds_braking_before_acceleration():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
tuning.shape_toyota_sienna_accel_target(-1.2, 20.0, False)
recovering = tuning.shape_toyota_sienna_accel_target(1.2, 20.0, False)
assert recovering == pytest.approx(-1.1272727273)
def test_toyota_sienna_target_filter_ramps_low_speed_acceleration():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
first = tuning.shape_toyota_sienna_accel_target(1.5, 2.0, False)
assert 0.0 < first < 1.5
for _ in range(100):
filtered = tuning.shape_toyota_sienna_accel_target(1.5, 3.0, False)
assert filtered < 1.5
assert tuning.shape_toyota_sienna_accel_target(-1.5, 3.0, False) == pytest.approx(-1.5)
def test_toyota_sienna_target_filter_bypasses_stop_and_urgent_braking():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
tuning.shape_toyota_sienna_accel_target(0.30, 20.0, False)
assert tuning.shape_toyota_sienna_accel_target(-0.80, 20.0, False) == pytest.approx(-0.80)
assert tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, True) == pytest.approx(-0.20)
def test_toyota_sienna_target_filter_smooths_comfortable_lead_braking():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
lead = SimpleNamespace(status=True, yRel=0.0, dRel=24.0, vLead=9.0, aLeadK=-1.2)
tuning.shape_toyota_sienna_accel_target(0.50, 10.0, False, leads=(lead,))
filtered = tuning.shape_toyota_sienna_accel_target(-1.5, 10.0, False, leads=(lead,))
assert -1.5 < filtered < 0.50
def test_toyota_sienna_target_filter_keeps_authority_when_lead_is_urgent():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
lead = SimpleNamespace(status=True, yRel=0.0, dRel=12.0, vLead=2.0, aLeadK=-2.0)
tuning.shape_toyota_sienna_accel_target(0.50, 12.0, False, leads=(lead,))
urgent = tuning.shape_toyota_sienna_accel_target(-1.5, 12.0, False, leads=(lead,))
assert urgent == pytest.approx(-1.5)
def test_toyota_sienna_target_filter_does_not_change_other_vehicles():
CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_CAMRY)
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
assert tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, False) == pytest.approx(-0.20)
def test_gm_stock_truck_positive_i_bleeds_during_light_highway_accel_request():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_SILVERADO"
CP.enableGasInterceptorDEPRECATED = False
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.02]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.28]
lc = LongControl(CP)
lc.pid.i = 0.25
lc.last_output_accel = 0.20
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.05, 0.05, CS,
)
assert lc.pid.i < 0.25
def test_gm_stock_truck_positive_i_trim_keeps_meaningful_accel_request():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_SILVERADO"
CP.enableGasInterceptorDEPRECATED = False
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.02]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.28]
lc = LongControl(CP)
lc.pid.i = 0.25
lc.last_output_accel = 0.20
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.12, 0.12, CS,
)
assert lc.pid.i == pytest.approx(0.25, abs=1e-9)
def test_gm_stock_truck_positive_i_trim_preserves_low_speed_launch():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_SILVERADO"
CP.enableGasInterceptorDEPRECATED = False
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.02]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.28]
lc = LongControl(CP)
lc.pid.i = 0.25
lc.last_output_accel = 0.20
CS = car.CarState.new_message(vEgo=5.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.05, 0.05, CS,
)
assert lc.pid.i == pytest.approx(0.25, abs=1e-9)
def test_gm_stock_truck_negative_i_unwinds_when_already_overbraking():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
lc = LongControl(CP)
lc.pid.i = -0.22
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=-0.49, brakePressed=False)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, 0.05, CS,
)
assert -0.22 < lc.pid.i < 0.0
def test_gm_stock_truck_negative_i_stays_when_decel_is_not_achieved():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
lc = LongControl(CP)
lc.pid.i = -0.22
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=0.05, brakePressed=False)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, -0.49, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
def test_gm_stock_truck_negative_i_stays_for_urgent_braking():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
lc = LongControl(CP)
lc.pid.i = -0.22
lc.last_output_accel = -1.30
CS = car.CarState.new_message(vEgo=29.6, aEgo=-1.20, brakePressed=False)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -1.00, 0.20, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
def test_gm_stock_truck_negative_i_trim_does_not_affect_other_gm_cars():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023,
enableGasInterceptorDEPRECATED=False,
)
lc = LongControl(CP)
lc.pid.i = -0.22
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=-0.49, brakePressed=False)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, 0.05, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
def test_gm_stock_truck_update_gradually_releases_stale_brake_integral():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
lc = LongControl(CP)
lc.pid.i = -0.22
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=-0.49, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=-0.44,
should_stop=False,
accel_limits=(-3.5, 2.0),
starpilot_toggles=make_toggles(),
has_lead=True,
)
assert -0.66 < output_accel < -0.44
Reference in New Issue View Git Blame Copy Permalink