mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-11 18:53:47 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 030ef12370 | |||
| 2f4b88c231 |
@@ -1159,10 +1159,7 @@ class CarController(CarControllerBase):
|
|||||||
if should_send_cc_button_spam(self.CP, CC, CS):
|
if should_send_cc_button_spam(self.CP, CC, CS):
|
||||||
if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC:
|
if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC:
|
||||||
# Using extend instead of append since the message is only sent intermittently
|
# Using extend instead of append since the message is only sent intermittently
|
||||||
lead_visible = bool(getattr(CS, "openpilot_lead_visible", CC.hudControl.leadVisible))
|
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles))
|
||||||
can_sends.extend(gmcan.create_gm_cc_spam_command(
|
|
||||||
self.packer_pt, self, CS, actuators, starpilot_toggles, lead_visible=lead_visible,
|
|
||||||
))
|
|
||||||
else:
|
else:
|
||||||
if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and
|
if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and
|
||||||
CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise):
|
CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise):
|
||||||
|
|||||||
@@ -31,8 +31,8 @@ BOLT_CC_DIRECTION_MEMORY_S = 1.5
|
|||||||
VOLT_CC_CARS = {
|
VOLT_CC_CARS = {
|
||||||
CAR.CHEVROLET_VOLT_CC,
|
CAR.CHEVROLET_VOLT_CC,
|
||||||
}
|
}
|
||||||
VOLT_CC_FREE_REQUEST_DEADBAND_MPH = 5.0
|
VOLT_CC_TARGET_DEADBAND_MPH = 5.0
|
||||||
VOLT_CC_LEAD_REQUEST_DEADBAND_MPH = 2.0
|
VOLT_CC_ACCEL_DEADBAND_MS2 = 0.15
|
||||||
|
|
||||||
|
|
||||||
def malibu_phase_map_for_button(button):
|
def malibu_phase_map_for_button(button):
|
||||||
@@ -341,18 +341,20 @@ def stabilize_bolt_cc_button(controller, CP, requested_button):
|
|||||||
return requested_button
|
return requested_button
|
||||||
|
|
||||||
|
|
||||||
def _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible):
|
def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
||||||
accel = float(actuators.accel)
|
accel = float(actuators.accel)
|
||||||
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
||||||
ego_speed = CS.out.vEgo * ms_convert
|
ego_speed = CS.out.vEgo * ms_convert
|
||||||
requested_setpoint = (CS.out.vEgo * 1.01 + 3 * accel) * ms_convert
|
|
||||||
deadband_mph = VOLT_CC_LEAD_REQUEST_DEADBAND_MPH if lead_visible else VOLT_CC_FREE_REQUEST_DEADBAND_MPH
|
|
||||||
request_deadband = deadband_mph * (CV.MPH_TO_KPH if ms_convert == CV.MS_TO_KPH else 1.0)
|
|
||||||
|
|
||||||
if abs(requested_setpoint - speed_setpoint) <= request_deadband:
|
v_cruise_kph = float(getattr(CS.out, "vCruise", 0.0))
|
||||||
return CruiseButtons.INIT, float("inf")
|
if 0.0 < v_cruise_kph < 255.0:
|
||||||
|
is_metric = ms_convert == CV.MS_TO_KPH
|
||||||
|
target_setpoint = v_cruise_kph if is_metric else v_cruise_kph * CV.KPH_TO_MPH
|
||||||
|
target_deadband = VOLT_CC_TARGET_DEADBAND_MPH * (CV.MPH_TO_KPH if is_metric else 1.0)
|
||||||
|
if abs(target_setpoint - speed_setpoint) <= target_deadband:
|
||||||
|
return CruiseButtons.INIT, float("inf")
|
||||||
|
|
||||||
if accel == 0.0:
|
if abs(accel) <= VOLT_CC_ACCEL_DEADBAND_MS2:
|
||||||
return CruiseButtons.INIT, float("inf")
|
return CruiseButtons.INIT, float("inf")
|
||||||
|
|
||||||
if accel < 0.0:
|
if accel < 0.0:
|
||||||
@@ -369,7 +371,7 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible):
|
|||||||
return CruiseButtons.RES_ACCEL, rate
|
return CruiseButtons.RES_ACCEL, rate
|
||||||
|
|
||||||
|
|
||||||
def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles, lead_visible=False):
|
def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles):
|
||||||
accel = actuators.accel
|
accel = actuators.accel
|
||||||
v_ego = CS.out.vEgo
|
v_ego = CS.out.vEgo
|
||||||
cruise_btn = CruiseButtons.INIT
|
cruise_btn = CruiseButtons.INIT
|
||||||
@@ -384,7 +386,7 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl
|
|||||||
comparison_setpoint = projected_setpoint if bolt_cc else desired_setpoint
|
comparison_setpoint = projected_setpoint if bolt_cc else desired_setpoint
|
||||||
|
|
||||||
if CS.CP.carFingerprint in VOLT_CC_CARS:
|
if CS.CP.carFingerprint in VOLT_CC_CARS:
|
||||||
cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible)
|
cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert)
|
||||||
else:
|
else:
|
||||||
if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25:
|
if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25:
|
||||||
cruise_btn = CruiseButtons.CANCEL
|
cruise_btn = CruiseButtons.CANCEL
|
||||||
|
|||||||
@@ -919,7 +919,7 @@ class TestGMCarController:
|
|||||||
|
|
||||||
def test_volt_cc_redneck_rate_limits_setpoint_changes_by_planner_acceleration(self):
|
def test_volt_cc_redneck_rate_limits_setpoint_changes_by_planner_acceleration(self):
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||||
controller = SimpleNamespace(frame=int(0.2 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
controller = SimpleNamespace(frame=int(0.5 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||||
cs = SimpleNamespace(
|
cs = SimpleNamespace(
|
||||||
CP=SimpleNamespace(
|
CP=SimpleNamespace(
|
||||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||||
@@ -935,19 +935,19 @@ class TestGMCarController:
|
|||||||
)
|
)
|
||||||
|
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
msgs = gmcan.create_gm_cc_spam_command(
|
||||||
packer, controller, cs, SimpleNamespace(accel=1.0), SimpleNamespace(is_metric=True),
|
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||||
)
|
)
|
||||||
|
|
||||||
assert msgs == []
|
assert msgs == []
|
||||||
|
|
||||||
controller.frame = int(0.3 / DT_CTRL)
|
controller.frame = int(0.7 / DT_CTRL)
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
msgs = gmcan.create_gm_cc_spam_command(
|
||||||
packer, controller, cs, SimpleNamespace(accel=1.0), SimpleNamespace(is_metric=True),
|
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||||
)
|
)
|
||||||
|
|
||||||
assert len(msgs) == 1
|
assert len(msgs) == 1
|
||||||
|
|
||||||
def test_volt_cc_redneck_holds_when_pseudo_speed_request_is_within_deadband(self):
|
def test_volt_cc_redneck_holds_when_stock_setpoint_is_within_target_deadband(self):
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||||
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||||
cs = SimpleNamespace(
|
cs = SimpleNamespace(
|
||||||
@@ -972,9 +972,9 @@ class TestGMCarController:
|
|||||||
assert msgs == []
|
assert msgs == []
|
||||||
assert controller.apply_speed == 99
|
assert controller.apply_speed == 99
|
||||||
|
|
||||||
def test_volt_cc_redneck_uses_smaller_request_deadband_with_lead(self):
|
def test_volt_cc_redneck_catches_up_when_target_exceeds_deadband(self):
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||||
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
controller = SimpleNamespace(frame=int(0.5 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||||
cs = SimpleNamespace(
|
cs = SimpleNamespace(
|
||||||
CP=SimpleNamespace(
|
CP=SimpleNamespace(
|
||||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||||
@@ -984,75 +984,25 @@ class TestGMCarController:
|
|||||||
),
|
),
|
||||||
buttons_counter=2,
|
buttons_counter=2,
|
||||||
out=SimpleNamespace(
|
out=SimpleNamespace(
|
||||||
vEgo=100.0 * CV.KPH_TO_MS,
|
vEgo=90.0 * CV.KPH_TO_MS,
|
||||||
cruiseState=SimpleNamespace(speed=99.0 * CV.KPH_TO_MS),
|
cruiseState=SimpleNamespace(speed=90.0 * CV.KPH_TO_MS),
|
||||||
vCruise=100.0,
|
vCruise=100.0,
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
msgs = gmcan.create_gm_cc_spam_command(
|
||||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True), lead_visible=True,
|
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||||
)
|
|
||||||
|
|
||||||
assert len(msgs) == 1
|
|
||||||
assert controller.apply_speed == 100
|
|
||||||
|
|
||||||
def test_volt_cc_redneck_accelerates_when_pseudo_speed_request_exceeds_deadband(self):
|
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
|
||||||
controller = SimpleNamespace(frame=int(0.1 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
|
||||||
cs = SimpleNamespace(
|
|
||||||
CP=SimpleNamespace(
|
|
||||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
|
||||||
flags=GMFlags.NO_CAMERA.value,
|
|
||||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
|
||||||
minEnableSpeed=0.0,
|
|
||||||
),
|
|
||||||
buttons_counter=2,
|
|
||||||
out=SimpleNamespace(
|
|
||||||
vEgo=44.0 * CV.KPH_TO_MS,
|
|
||||||
cruiseState=SimpleNamespace(speed=44.0 * CV.KPH_TO_MS),
|
|
||||||
vCruise=50.0,
|
|
||||||
),
|
|
||||||
)
|
|
||||||
|
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
|
||||||
packer, controller, cs, SimpleNamespace(accel=2.0), SimpleNamespace(is_metric=True),
|
|
||||||
)
|
)
|
||||||
|
|
||||||
assert msgs == []
|
assert msgs == []
|
||||||
|
|
||||||
controller.frame = int(0.3 / DT_CTRL)
|
controller.frame = int(0.7 / DT_CTRL)
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
msgs = gmcan.create_gm_cc_spam_command(
|
||||||
packer, controller, cs, SimpleNamespace(accel=2.0), SimpleNamespace(is_metric=True),
|
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||||
)
|
)
|
||||||
|
|
||||||
assert len(msgs) == 1
|
assert len(msgs) == 1
|
||||||
assert controller.apply_speed == 45
|
assert controller.apply_speed == 91
|
||||||
|
|
||||||
def test_volt_cc_redneck_brakes_when_pseudo_speed_request_exceeds_deadband(self):
|
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
|
||||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
|
||||||
cs = SimpleNamespace(
|
|
||||||
CP=SimpleNamespace(
|
|
||||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
|
||||||
flags=GMFlags.NO_CAMERA.value,
|
|
||||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
|
||||||
minEnableSpeed=0.0,
|
|
||||||
),
|
|
||||||
buttons_counter=2,
|
|
||||||
out=SimpleNamespace(
|
|
||||||
vEgo=50.7 * CV.KPH_TO_MS,
|
|
||||||
cruiseState=SimpleNamespace(speed=49.0 * CV.KPH_TO_MS),
|
|
||||||
vCruise=50.0,
|
|
||||||
),
|
|
||||||
)
|
|
||||||
|
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
|
||||||
packer, controller, cs, SimpleNamespace(accel=-1.36), SimpleNamespace(is_metric=True),
|
|
||||||
)
|
|
||||||
|
|
||||||
assert len(msgs) == 1
|
|
||||||
assert controller.apply_speed == 48
|
|
||||||
|
|
||||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||||
|
|||||||
@@ -492,10 +492,7 @@ class Car:
|
|||||||
if self.starpilot_toggles.speed_limit_controller:
|
if self.starpilot_toggles.speed_limit_controller:
|
||||||
overridden_speed = float(starpilot_plan.slcOverriddenSpeed)
|
overridden_speed = float(starpilot_plan.slcOverriddenSpeed)
|
||||||
slc_limit = float(starpilot_plan.slcSpeedLimit) + float(starpilot_plan.slcSpeedLimitOffset)
|
slc_limit = float(starpilot_plan.slcSpeedLimit) + float(starpilot_plan.slcSpeedLimitOffset)
|
||||||
allow_lower_override = (
|
allow_lower_override = getattr(self.starpilot_toggles, "redneck_cruise", False)
|
||||||
getattr(self.starpilot_toggles, "redneck_cruise", False) and
|
|
||||||
getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False)
|
|
||||||
)
|
|
||||||
slc_target_speed = overridden_speed if allow_lower_override and overridden_speed > 0 else max(overridden_speed, slc_limit)
|
slc_target_speed = overridden_speed if allow_lower_override and overridden_speed > 0 else max(overridden_speed, slc_limit)
|
||||||
|
|
||||||
# Use acceleration projection only when SLC has no resolved target.
|
# Use acceleration projection only when SLC has no resolved target.
|
||||||
|
|||||||
@@ -265,7 +265,6 @@ class TestRedneckCruise(unittest.TestCase):
|
|||||||
starpilot_toggles=SimpleNamespace(
|
starpilot_toggles=SimpleNamespace(
|
||||||
speed_limit_controller=True,
|
speed_limit_controller=True,
|
||||||
redneck_cruise=True,
|
redneck_cruise=True,
|
||||||
speed_limit_controller_override_set_speed=True,
|
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
car_state = SimpleNamespace(
|
car_state = SimpleNamespace(
|
||||||
|
|||||||
@@ -64,10 +64,10 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
|
|||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE = 30.0
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE = 45.0
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE = 105.0
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE = 105.0
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED = 0.75
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED = 4.0
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.35
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.8
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE = 2.0
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE = 2.0
|
||||||
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL = 0.5
|
TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL = 0.5
|
||||||
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_SPEED = 5.0
|
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_SPEED = 5.0
|
||||||
@@ -77,8 +77,8 @@ TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_DISTANCE = 100.0
|
|||||||
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_TIME = 4.5
|
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_TIME = 4.5
|
||||||
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0
|
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0
|
||||||
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75
|
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75
|
||||||
TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.0
|
TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
|
||||||
TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.25
|
TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
|
||||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70
|
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70
|
||||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20
|
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20
|
||||||
TOYOTA_RAV4_TSS2_LEAD_CREEP_MIN_LEAD_SPEED = 0.15
|
TOYOTA_RAV4_TSS2_LEAD_CREEP_MIN_LEAD_SPEED = 0.15
|
||||||
@@ -427,10 +427,7 @@ def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
|
|||||||
(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE),
|
(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE),
|
||||||
0.0, 1.0,
|
0.0, 1.0,
|
||||||
)
|
)
|
||||||
closing_factor = np.clip(
|
closing_factor = np.clip((closing_speed - 4.0) / 6.0, 0.0, 1.0)
|
||||||
(closing_speed - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED) / 4.0,
|
|
||||||
0.0, 1.0,
|
|
||||||
)
|
|
||||||
brake_factor = np.clip(
|
brake_factor = np.clip(
|
||||||
(lead_brake - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE) /
|
(lead_brake - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE) /
|
||||||
(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE),
|
(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE),
|
||||||
|
|||||||
@@ -3454,15 +3454,6 @@ def test_rav4_tss2_early_lead_cap_starts_a_mild_response():
|
|||||||
assert -0.5 <= cap < 0.0
|
assert -0.5 <= cap < 0.0
|
||||||
|
|
||||||
|
|
||||||
def test_rav4_tss2_early_lead_cap_handles_moderate_closing_before_hard_approach():
|
|
||||||
CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
|
||||||
lead = make_lead(status=True, d_rel=35.0, v_lead=16.4, a_lead=-0.5, model_prob=0.99)
|
|
||||||
|
|
||||||
cap = get_toyota_rav4_tss2_early_lead_cap(CP, lead, 17.4, -3.5)
|
|
||||||
|
|
||||||
assert cap == pytest.approx(-0.30, abs=0.03)
|
|
||||||
|
|
||||||
|
|
||||||
def test_rav4_tss2_early_lead_cap_does_not_change_other_paths():
|
def test_rav4_tss2_early_lead_cap_does_not_change_other_paths():
|
||||||
rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
||||||
other = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
|
other = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
|
||||||
|
|||||||
@@ -47,8 +47,6 @@ def make_toggles(**overrides):
|
|||||||
"slc_mapbox_filler": False,
|
"slc_mapbox_filler": False,
|
||||||
"speed_limit_confirmation_higher": False,
|
"speed_limit_confirmation_higher": False,
|
||||||
"speed_limit_confirmation_lower": False,
|
"speed_limit_confirmation_lower": False,
|
||||||
"speed_limit_controller_override_manual": True,
|
|
||||||
"speed_limit_controller_override_set_speed": False,
|
|
||||||
"redneck_cruise": False,
|
"redneck_cruise": False,
|
||||||
"speed_limit_filler": False,
|
"speed_limit_filler": False,
|
||||||
"speed_limit_offset1": 0.0,
|
"speed_limit_offset1": 0.0,
|
||||||
@@ -75,7 +73,7 @@ def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=Fal
|
|||||||
"carControl": SimpleNamespace(longActive=long_active),
|
"carControl": SimpleNamespace(longActive=long_active),
|
||||||
"carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, vCruise=v_cruise_kph),
|
"carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, vCruise=v_cruise_kph),
|
||||||
"liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
|
"liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
|
||||||
"mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0),
|
"mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0, roadName=""),
|
||||||
"selfdriveState": SimpleNamespace(enabled=enabled),
|
"selfdriveState": SimpleNamespace(enabled=enabled),
|
||||||
"starpilotCarState": SimpleNamespace(accelPressed=accel_pressed, decelPressed=decel_pressed),
|
"starpilotCarState": SimpleNamespace(accelPressed=accel_pressed, decelPressed=decel_pressed),
|
||||||
}
|
}
|
||||||
@@ -315,106 +313,60 @@ def test_display_only_applies_large_delta_guard():
|
|||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_new_source_limit_clears_override_until_gas_release():
|
def test_set_speed_override_survives_source_changes_and_fallback_until_driver_clears():
|
||||||
controller = make_controller()
|
controller = make_controller(
|
||||||
|
speed_limit_priority1="Map Data",
|
||||||
|
speed_limit_priority2="Dashboard",
|
||||||
|
slc_fallback_set_speed=True,
|
||||||
|
)
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(55)
|
controller.target = mph(45)
|
||||||
controller.previous_source = "Dashboard"
|
controller.previous_source = "Dashboard"
|
||||||
controller.previous_target = mph(55)
|
controller.previous_target = mph(45)
|
||||||
controller.overridden_speed = mph(65)
|
controller.last_valid_limit = mph(45)
|
||||||
|
|
||||||
sm = make_sm(gas_pressed=True)
|
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
controller.update_override(mph(55), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
|
||||||
|
|
||||||
assert controller.target == pytest.approx(mph(45))
|
|
||||||
assert controller.source == "Dashboard"
|
|
||||||
assert controller.overridden_speed == 0
|
|
||||||
assert not controller.override_slc
|
|
||||||
|
|
||||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
|
||||||
|
|
||||||
assert controller.overridden_speed == 0
|
|
||||||
assert not controller.override_slc
|
|
||||||
|
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False))
|
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
|
||||||
|
|
||||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
|
||||||
# --- Dropout / Fallback Test Condition ---
|
# Dashboard 45 -> Map Data 45 is not a new speed zone.
|
||||||
controller.starpilot_toggles = make_toggles(slc_fallback_set_speed=True)
|
map_sm = make_sm(gas_pressed=False)
|
||||||
# No limit available → falls back to v_cruise (75 mph) with source "None".
|
map_sm["mapdOut"].speedLimit = mph(45)
|
||||||
# Override persists because target_to_use resolves to last_valid_limit (45 mph) which is
|
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), map_sm)
|
||||||
# below overridden_speed (65 mph) — the sticky override_slc chain stays True.
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, map_sm)
|
||||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
assert controller.source == "Map Data"
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
assert controller.override_slc
|
||||||
|
|
||||||
assert controller.target == pytest.approx(mph(75))
|
# A temporary fallback, and even a complete source dropout, do not clear the override.
|
||||||
|
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.source == "None"
|
assert controller.source == "None"
|
||||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
assert controller.target == pytest.approx(mph(55))
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
|
||||||
# Recovery to a confirmed limit (55 mph) clears the override: this is a genuinely new
|
controller.starpilot_toggles.slc_fallback_set_speed = False
|
||||||
# speed zone (55 != last_valid 45), so clear_override_for_source_limit fires correctly.
|
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
assert controller.target == 0
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
assert controller.override_slc
|
||||||
|
|
||||||
assert controller.target == pytest.approx(mph(55))
|
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.source == "Dashboard"
|
assert controller.source == "Dashboard"
|
||||||
assert controller.overridden_speed == 0
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
assert controller.override_slc
|
||||||
|
|
||||||
|
# Set-speed fallback does not clear passively, but a fresh - to the retained target does.
|
||||||
|
controller.starpilot_toggles.slc_fallback_set_speed = True
|
||||||
|
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
|
controller.update_override(mph(45), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
assert not controller.override_slc
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
# --- Override Clipping Check (set-speed fallback) ---
|
|
||||||
# Separate controller: active override, then fallback to v_cruise that is BELOW the override.
|
|
||||||
# overridden_speed clips to v_cruise (override_slc stays True via sticky chain, but
|
|
||||||
# np.clip clamps overridden_speed to the new target+offset).
|
|
||||||
clip_controller = make_controller(slc_fallback_set_speed=True)
|
|
||||||
try:
|
|
||||||
clip_controller.source = "Dashboard"
|
|
||||||
clip_controller.target = mph(55)
|
|
||||||
clip_controller.previous_source = "Dashboard"
|
|
||||||
clip_controller.previous_target = mph(55)
|
|
||||||
clip_controller.last_valid_limit = mph(55)
|
|
||||||
clip_controller.override_slc = True
|
|
||||||
clip_controller.overridden_speed = mph(65)
|
|
||||||
|
|
||||||
sm_no_gas = make_sm(gas_pressed=False)
|
|
||||||
# v_cruise = 30 mph (below last_valid 55), so target_to_use returns mph(30).
|
|
||||||
# override_slc sticky: overridden_speed=65 > 30+0=30 > 0 — still True from chain.
|
|
||||||
# np.clip(65, 30, 30) = 30, so overridden_speed clips to mph(30).
|
|
||||||
clip_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(30), sm_no_gas)
|
|
||||||
clip_controller.update_override(mph(30), 0.0, mph(30), 0.0, sm_no_gas)
|
|
||||||
|
|
||||||
assert clip_controller.target == pytest.approx(mph(30))
|
|
||||||
# Clipped to v_cruise — not locked at mph(55) or mph(65)
|
|
||||||
assert clip_controller.overridden_speed == pytest.approx(mph(30))
|
|
||||||
assert clip_controller.override_slc
|
|
||||||
finally:
|
|
||||||
clip_controller.shutdown()
|
|
||||||
|
|
||||||
# --- Lost Speed Limit (no fallback) clears target to 0 ---
|
|
||||||
# When all limit sources drop to 0 with no fallback, target becomes 0
|
|
||||||
# and override_slc is False (target_to_use=0, chain evaluates False).
|
|
||||||
lost_controller = make_controller(slc_fallback_set_speed=False, slc_fallback_previous_speed_limit=False)
|
|
||||||
try:
|
|
||||||
lost_controller.source = "Dashboard"
|
|
||||||
lost_controller.target = mph(45)
|
|
||||||
lost_controller.previous_source = "Dashboard"
|
|
||||||
lost_controller.previous_target = mph(45)
|
|
||||||
|
|
||||||
sm_on = make_sm(gas_pressed=False)
|
|
||||||
lost_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm_on)
|
|
||||||
lost_controller.update_override(mph(75), 0.0, mph(65), 0.0, sm_on)
|
|
||||||
|
|
||||||
assert lost_controller.target == 0
|
|
||||||
assert lost_controller.overridden_speed == 0
|
|
||||||
assert not lost_controller.override_slc
|
|
||||||
finally:
|
|
||||||
lost_controller.shutdown()
|
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
@@ -487,81 +439,88 @@ def test_unconfirmed_lower_limit_keeps_existing_override():
|
|||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_higher_limit_does_not_clear_override():
|
def test_set_speed_override_handles_higher_limit_changes():
|
||||||
controller = make_controller()
|
controller = make_controller()
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(35)
|
controller.target = mph(35)
|
||||||
controller.previous_source = "Dashboard"
|
controller.previous_source = "Dashboard"
|
||||||
controller.previous_target = mph(35)
|
controller.previous_target = mph(35)
|
||||||
controller.overridden_speed = mph(55)
|
controller.last_valid_limit = mph(35)
|
||||||
controller.override_slc = True
|
|
||||||
|
|
||||||
sm = make_sm(gas_pressed=True)
|
controller.update_override(mph(35), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
||||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(55), sm)
|
controller.update_override(mph(55), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
||||||
controller.update_override(mph(75), 0.0, mph(55), 0.0, sm)
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
|
||||||
|
# A higher limit below the selected override preserves it.
|
||||||
|
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
|
|
||||||
assert controller.target == pytest.approx(mph(45))
|
assert controller.target == pytest.approx(mph(45))
|
||||||
assert controller.source == "Dashboard"
|
assert controller.source == "Dashboard"
|
||||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
|
||||||
controller_overridden_below = make_controller()
|
# A higher effective target that reaches the override clears it without re-arming.
|
||||||
try:
|
controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
controller_overridden_below.source = "Dashboard"
|
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
controller_overridden_below.target = mph(35)
|
assert controller.target == pytest.approx(mph(55))
|
||||||
controller_overridden_below.previous_source = "Dashboard"
|
assert controller.overridden_speed == 0
|
||||||
controller_overridden_below.previous_target = mph(35)
|
assert not controller.override_slc
|
||||||
controller_overridden_below.overridden_speed = mph(40)
|
|
||||||
controller_overridden_below.override_slc = True
|
|
||||||
|
|
||||||
sm = make_sm(gas_pressed=True)
|
|
||||||
controller_overridden_below.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(55), sm)
|
|
||||||
controller_overridden_below.update_override(mph(75), 0.0, mph(55), 0.0, sm)
|
|
||||||
|
|
||||||
assert controller_overridden_below.target == pytest.approx(mph(45))
|
|
||||||
assert controller_overridden_below.overridden_speed == 0
|
|
||||||
assert not controller_overridden_below.override_slc
|
|
||||||
finally:
|
|
||||||
controller_overridden_below.shutdown()
|
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_set_speed_mode_overrides_on_raise_without_gas():
|
def test_pedal_and_set_speed_overrides_are_independent():
|
||||||
# Max Set Speed mode: raising the set speed (+/-) above the posted limit must override
|
controller = make_controller()
|
||||||
# the SLC hold with no gas pedal, targeting the set speed.
|
|
||||||
controller = make_controller(
|
|
||||||
speed_limit_controller_override_manual=False,
|
|
||||||
speed_limit_controller_override_set_speed=True,
|
|
||||||
)
|
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(45)
|
controller.target = mph(45)
|
||||||
controller.last_valid_limit = mph(45)
|
controller.last_valid_limit = mph(45)
|
||||||
|
|
||||||
# Baseline frame at the limit establishes the previous set speed (no rising edge yet).
|
# A pedal pass is temporary; a set-speed increase is the fixed persistent action.
|
||||||
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
assert not controller.override_slc
|
controller.update_override(mph(45), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||||
|
|
||||||
# Driver presses + to 60 (rising edge): override arms and targets the set speed.
|
|
||||||
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
|
||||||
|
controller.update_override(mph(45), 0.0, mph(55), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
|
|
||||||
|
# A fresh + above the effective SLC target arms the override.
|
||||||
|
controller.update_override(mph(55), 0.0, mph(55), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
|
||||||
|
# Pedaling temporarily takes priority, then returns to the selected set speed.
|
||||||
|
controller.update_override(mph(55), 0.0, mph(60), 0.0, make_sm(gas_pressed=True))
|
||||||
assert controller.overridden_speed == pytest.approx(mph(60))
|
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||||
|
|
||||||
# Holding 60 with no further press: override stays latched.
|
controller.update_override(mph(55), 0.0, mph(60), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
|
||||||
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
assert controller.overridden_speed == pytest.approx(mph(60))
|
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||||
|
|
||||||
|
controller.update_override(mph(50), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(50))
|
||||||
|
|
||||||
|
# Returning to the effective SLC target ends the persistent override.
|
||||||
|
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_set_speed_mode_waits_until_above_slc_target_with_offset():
|
def test_persistent_override_waits_until_above_slc_target_with_offset():
|
||||||
controller = make_controller(
|
controller = make_controller(
|
||||||
is_metric=True,
|
is_metric=True,
|
||||||
speed_limit_controller_override_manual=False,
|
|
||||||
speed_limit_controller_override_set_speed=True,
|
|
||||||
speed_limit_offset2=3 * CV.KPH_TO_MS,
|
speed_limit_offset2=3 * CV.KPH_TO_MS,
|
||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
@@ -576,6 +535,10 @@ def test_set_speed_mode_waits_until_above_slc_target_with_offset():
|
|||||||
controller.update_override(35 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
|
controller.update_override(35 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
assert controller.overridden_speed == pytest.approx(35 * CV.KPH_TO_MS)
|
assert controller.overridden_speed == pytest.approx(35 * CV.KPH_TO_MS)
|
||||||
|
|
||||||
|
controller.update_override(33 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
@@ -583,10 +546,7 @@ def test_set_speed_mode_waits_until_above_slc_target_with_offset():
|
|||||||
def test_set_speed_override_clears_on_new_speed_zone():
|
def test_set_speed_override_clears_on_new_speed_zone():
|
||||||
# Entering a new (lower) posted limit clears the override; a steady high set speed must not
|
# Entering a new (lower) posted limit clears the override; a steady high set speed must not
|
||||||
# re-arm it. Only a fresh +/- press re-arms.
|
# re-arm it. Only a fresh +/- press re-arms.
|
||||||
controller = make_controller(
|
controller = make_controller()
|
||||||
speed_limit_controller_override_manual=False,
|
|
||||||
speed_limit_controller_override_set_speed=True,
|
|
||||||
)
|
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(45)
|
controller.target = mph(45)
|
||||||
@@ -598,26 +558,87 @@ def test_set_speed_override_clears_on_new_speed_zone():
|
|||||||
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
|
||||||
# New lower zone (35): update_limits clears the override for the new segment.
|
# A 1 mph lower zone takes the same-limit fast path but still clears the override.
|
||||||
controller.update_limits(mph(35), datetime.now(timezone.utc), False, mph(60), mph(58), make_sm(gas_pressed=False))
|
controller.update_limits(mph(44), datetime.now(timezone.utc), False, mph(60), mph(58), make_sm(gas_pressed=False))
|
||||||
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.target == pytest.approx(mph(35))
|
assert controller.target == pytest.approx(mph(44))
|
||||||
# Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 35).
|
# Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 44).
|
||||||
assert not controller.override_slc
|
assert not controller.override_slc
|
||||||
assert controller.overridden_speed == 0
|
assert controller.overridden_speed == 0
|
||||||
|
|
||||||
# A fresh + press (60 -> 65) re-arms against the new limit.
|
# A fresh + press (60 -> 65) re-arms against the new limit.
|
||||||
controller.update_override(mph(65), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
controller.update_override(mph(65), 0.0, mph(44), 0.0, make_sm(gas_pressed=False))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
assert controller.overridden_speed == pytest.approx(mph(65))
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_redneck_set_speed_mode_overrides_in_both_directions():
|
def test_confirmation_accel_press_does_not_arm_set_speed_override():
|
||||||
|
controller = make_controller(
|
||||||
|
speed_limit_confirmation_higher=True,
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
controller.source = "Dashboard"
|
||||||
|
controller.target = mph(45)
|
||||||
|
controller.previous_source = "Dashboard"
|
||||||
|
controller.previous_target = mph(45)
|
||||||
|
controller.last_valid_limit = mph(45)
|
||||||
|
|
||||||
|
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||||
|
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), make_sm(gas_pressed=False))
|
||||||
|
assert controller.source == "None"
|
||||||
|
assert controller.unconfirmed_speed_limit == pytest.approx(mph(50))
|
||||||
|
|
||||||
|
# The button arrives before the corresponding cruise-speed update. This + accepts the
|
||||||
|
# pending 50 mph limit, but its delayed 55 mph set-speed update must not arm an override.
|
||||||
|
confirm_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=45 * CV.MPH_TO_KPH)
|
||||||
|
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), confirm_sm)
|
||||||
|
controller.update_override(mph(45), 0.0, mph(45), 0.0, confirm_sm)
|
||||||
|
assert controller.source == "Dashboard"
|
||||||
|
assert controller.target == pytest.approx(mph(50))
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
|
|
||||||
|
delayed_speed_sm = make_sm(gas_pressed=False, v_cruise_kph=55 * CV.MPH_TO_KPH)
|
||||||
|
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(55), mph(45), delayed_speed_sm)
|
||||||
|
controller.update_override(mph(55), 0.0, mph(45), 0.0, delayed_speed_sm)
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
|
|
||||||
|
# A second fresh + is allowed to establish the override.
|
||||||
|
second_press_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=60 * CV.MPH_TO_KPH)
|
||||||
|
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(60), mph(45), second_press_sm)
|
||||||
|
controller.update_override(mph(60), 0.0, mph(45), 0.0, second_press_sm)
|
||||||
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||||
|
finally:
|
||||||
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
def test_adopt_speed_limit_clears_complete_override_state():
|
||||||
|
controller = make_controller()
|
||||||
|
try:
|
||||||
|
controller.source = "Dashboard"
|
||||||
|
controller.target = mph(45)
|
||||||
|
controller.previous_source = "Dashboard"
|
||||||
|
controller.previous_target = mph(45)
|
||||||
|
controller.last_valid_limit = mph(45)
|
||||||
|
controller.override_slc = True
|
||||||
|
controller.overridden_speed = mph(55)
|
||||||
|
controller._slc_adopt_counter = 3
|
||||||
|
controller.starpilot_planner.params_memory.values["SLCAdoptSpeedLimit"] = True
|
||||||
|
|
||||||
|
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||||
|
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
|
assert not controller.override_slc
|
||||||
|
finally:
|
||||||
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
def test_redneck_set_speed_override_is_bidirectional():
|
||||||
controller = make_controller(
|
controller = make_controller(
|
||||||
speed_limit_controller_override_manual=False,
|
|
||||||
speed_limit_controller_override_set_speed=True,
|
|
||||||
redneck_cruise=True,
|
redneck_cruise=True,
|
||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
@@ -638,12 +659,8 @@ def test_redneck_set_speed_mode_overrides_in_both_directions():
|
|||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_gas_pedal_mode_ignores_set_speed_without_gas():
|
def test_manual_override_tracks_current_speed_and_ends_on_release():
|
||||||
# Set With Gas Pedal mode: a high set speed alone must NOT override; gas is still required.
|
controller = make_controller()
|
||||||
controller = make_controller(
|
|
||||||
speed_limit_controller_override_manual=True,
|
|
||||||
speed_limit_controller_override_set_speed=False,
|
|
||||||
)
|
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(45)
|
controller.target = mph(45)
|
||||||
@@ -653,9 +670,17 @@ def test_gas_pedal_mode_ignores_set_speed_without_gas():
|
|||||||
assert not controller.override_slc
|
assert not controller.override_slc
|
||||||
assert controller.overridden_speed == 0
|
assert controller.overridden_speed == 0
|
||||||
|
|
||||||
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True))
|
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
|
|
||||||
|
# The temporary override follows the current speed rather than a historical peak.
|
||||||
|
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True))
|
||||||
assert controller.overridden_speed == pytest.approx(mph(50))
|
assert controller.overridden_speed == pytest.approx(mph(50))
|
||||||
|
|
||||||
|
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||||
|
assert not controller.override_slc
|
||||||
|
assert controller.overridden_speed == 0
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
@@ -667,17 +692,17 @@ def test_manual_override_survives_brief_enabled_flicker():
|
|||||||
controller.target = mph(45)
|
controller.target = mph(45)
|
||||||
controller.previous_source = "Dashboard"
|
controller.previous_source = "Dashboard"
|
||||||
controller.previous_target = mph(45)
|
controller.previous_target = mph(45)
|
||||||
controller.overridden_speed = mph(55)
|
controller.last_valid_limit = mph(45)
|
||||||
controller.override_slc = True
|
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||||
|
|
||||||
disabled_sm = make_sm(gas_pressed=False, enabled=False)
|
disabled_sm = make_sm(gas_pressed=True, enabled=False)
|
||||||
for _ in range(int(0.5 / DT_MDL)):
|
for _ in range(int(0.5 / DT_MDL)):
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, disabled_sm)
|
controller.update_override(mph(60), 0.0, mph(55), 0.0, disabled_sm)
|
||||||
|
|
||||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
|
|
||||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False, enabled=True))
|
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True, enabled=True))
|
||||||
|
|
||||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||||
assert controller.override_slc
|
assert controller.override_slc
|
||||||
@@ -685,16 +710,16 @@ def test_manual_override_survives_brief_enabled_flicker():
|
|||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_manual_override_clears_after_sustained_disengage():
|
def test_override_clears_after_sustained_disengage():
|
||||||
controller = make_controller()
|
controller = make_controller()
|
||||||
try:
|
try:
|
||||||
controller.source = "Dashboard"
|
controller.source = "Dashboard"
|
||||||
controller.target = mph(45)
|
controller.target = mph(45)
|
||||||
controller.previous_source = "Dashboard"
|
controller.previous_source = "Dashboard"
|
||||||
controller.previous_target = mph(45)
|
controller.previous_target = mph(45)
|
||||||
|
controller.last_valid_limit = mph(45)
|
||||||
controller.overridden_speed = mph(55)
|
controller.overridden_speed = mph(55)
|
||||||
controller.override_slc = True
|
controller.override_slc = True
|
||||||
controller.override_requires_gas_release = True
|
|
||||||
|
|
||||||
disabled_sm = make_sm(gas_pressed=False, enabled=False)
|
disabled_sm = make_sm(gas_pressed=False, enabled=False)
|
||||||
for _ in range(int(1.0 / DT_MDL) + 1):
|
for _ in range(int(1.0 / DT_MDL) + 1):
|
||||||
@@ -702,6 +727,5 @@ def test_manual_override_clears_after_sustained_disengage():
|
|||||||
|
|
||||||
assert controller.overridden_speed == 0
|
assert controller.overridden_speed == 0
|
||||||
assert not controller.override_slc
|
assert not controller.override_slc
|
||||||
assert not controller.override_requires_gas_release
|
|
||||||
finally:
|
finally:
|
||||||
controller.shutdown()
|
controller.shutdown()
|
||||||
|
|||||||
@@ -77,13 +77,6 @@ SLC_FALLBACK_OPTIONS = [
|
|||||||
(2, "Previous Limit"),
|
(2, "Previous Limit"),
|
||||||
]
|
]
|
||||||
|
|
||||||
SLC_OVERRIDE_OPTIONS = [
|
|
||||||
(0, "None"),
|
|
||||||
(1, "Set With Gas Pedal"),
|
|
||||||
(2, "Max Set Speed"),
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
# ═══════════════════════════════════════════════════════════════
|
# ═══════════════════════════════════════════════════════════════
|
||||||
# AdaptiveSpeedView — nested panel with two adaptive speed tiles
|
# AdaptiveSpeedView — nested panel with two adaptive speed tiles
|
||||||
# ═══════════════════════════════════════════════════════════════
|
# ═══════════════════════════════════════════════════════════════
|
||||||
@@ -599,11 +592,6 @@ class StarPilotLongitudinalLayout(_SettingsPage):
|
|||||||
get_value=lambda: self._profile_label_for_value(self._params.get_int("SLCFallback"), SLC_FALLBACK_OPTIONS),
|
get_value=lambda: self._profile_label_for_value(self._params.get_int("SLCFallback"), SLC_FALLBACK_OPTIONS),
|
||||||
on_click=lambda: self._show_labeled_select("Fallback Speed", "SLCFallback", SLC_FALLBACK_OPTIONS,
|
on_click=lambda: self._show_labeled_select("Fallback Speed", "SLCFallback", SLC_FALLBACK_OPTIONS,
|
||||||
self._params.get_int("SLCFallback"))),
|
self._params.get_int("SLCFallback"))),
|
||||||
SettingRow("SLCOverride", "value", tr_noop("Override Speed"),
|
|
||||||
subtitle="",
|
|
||||||
get_value=lambda: self._profile_label_for_value(self._params.get_int("SLCOverride"), SLC_OVERRIDE_OPTIONS),
|
|
||||||
on_click=lambda: self._show_labeled_select("Override Speed", "SLCOverride", SLC_OVERRIDE_OPTIONS,
|
|
||||||
self._params.get_int("SLCOverride"))),
|
|
||||||
SettingRow("SLCPriority", "value", tr_noop("Source Priority"),
|
SettingRow("SLCPriority", "value", tr_noop("Source Priority"),
|
||||||
subtitle="",
|
subtitle="",
|
||||||
get_value=self._get_priority_value,
|
get_value=self._get_priority_value,
|
||||||
@@ -889,7 +877,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
|
|||||||
self,
|
self,
|
||||||
[SettingSection(title="", rows=self._slc_rows)],
|
[SettingSection(title="", rows=self._slc_rows)],
|
||||||
header_title=tr_noop("Speed Limit Controller"),
|
header_title=tr_noop("Speed Limit Controller"),
|
||||||
header_subtitle=tr_noop("Manage auto speed matching, confirmation, offsets, and source priority."),
|
header_subtitle=tr_noop("Press + above a limit for a persistent override; hold the gas pedal for a temporary override."),
|
||||||
parent_toggle=pt_slc,
|
parent_toggle=pt_slc,
|
||||||
panel_style=PANEL_STYLE,
|
panel_style=PANEL_STYLE,
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1674,10 +1674,6 @@
|
|||||||
<source>Fallback Speed</source>
|
<source>Fallback Speed</source>
|
||||||
<translation>Резерв. дж. лімітів</translation>
|
<translation>Резерв. дж. лімітів</translation>
|
||||||
</message>
|
</message>
|
||||||
<message>
|
|
||||||
<source>Override Speed</source>
|
|
||||||
<translation>Ручна швидк.</translation>
|
|
||||||
</message>
|
|
||||||
<message>
|
<message>
|
||||||
<source>Confirm New Speed Limits</source>
|
<source>Confirm New Speed Limits</source>
|
||||||
<translation>Підтверд. новий ліміт шв.</translation>
|
<translation>Підтверд. новий ліміт шв.</translation>
|
||||||
@@ -1834,14 +1830,6 @@
|
|||||||
<source>None</source>
|
<source>None</source>
|
||||||
<translation>Нема</translation>
|
<translation>Нема</translation>
|
||||||
</message>
|
</message>
|
||||||
<message>
|
|
||||||
<source>Set With Gas Pedal</source>
|
|
||||||
<translation>Педаль</translation>
|
|
||||||
</message>
|
|
||||||
<message>
|
|
||||||
<source>Max Set Speed</source>
|
|
||||||
<translation>Макс встан. швидк.</translation>
|
|
||||||
</message>
|
|
||||||
<message>
|
<message>
|
||||||
<source>SELECT</source>
|
<source>SELECT</source>
|
||||||
<translation>ОБРАТИ</translation>
|
<translation>ОБРАТИ</translation>
|
||||||
@@ -2346,10 +2334,6 @@
|
|||||||
<source><b>The speed used by "Speed Limit Controller" when no speed limit is found.</b><br><br>- <b>Set Speed</b>: Use the cruise set speed<br>- <b>Experimental Mode</b>: Estimate the limit using the driving model<br>- <b>Previous Limit</b>: Keep using the last confirmed limit</source>
|
<source><b>The speed used by "Speed Limit Controller" when no speed limit is found.</b><br><br>- <b>Set Speed</b>: Use the cruise set speed<br>- <b>Experimental Mode</b>: Estimate the limit using the driving model<br>- <b>Previous Limit</b>: Keep using the last confirmed limit</source>
|
||||||
<translation><b>Швидкість, яка використовується «Контролером обмеження швидкості», коли обмеження швидкості не виявлено.</b><br><br>- <b>Встановити швидкість</b>: Використовувати встановлену швидкість круїз-контролю<br>- <b>Експериментальний режим</b>: Оцінити обмеження за допомогою моделі водіння<br>- <b>Попереднє обмеження</b>: Продовжувати використовувати останнє підтверджене обмеження</translation>
|
<translation><b>Швидкість, яка використовується «Контролером обмеження швидкості», коли обмеження швидкості не виявлено.</b><br><br>- <b>Встановити швидкість</b>: Використовувати встановлену швидкість круїз-контролю<br>- <b>Експериментальний режим</b>: Оцінити обмеження за допомогою моделі водіння<br>- <b>Попереднє обмеження</b>: Продовжувати використовувати останнє підтверджене обмеження</translation>
|
||||||
</message>
|
</message>
|
||||||
<message>
|
|
||||||
<source><b>The speed used by "Speed Limit Controller" after you manually drive faster than the posted limit.</b><br><br>- <b>Set with Gas Pedal</b>: Use the highest speed reached while pressing the gas<br>- <b>Max Set Speed</b>: Use the cruise set speed<br><br>Overrides clear when openpilot disengages.</source>
|
|
||||||
<translation><b>Швидкість, яку використовує «Контролер обмеження швидкості» після того, як ви вручну перевищили встановлене обмеження. </b><br><br>- <b>Встановлюється за допомогою педалі газу</b>: використовується найвища швидкість, досягнута під час натискання на педаль газу<br>- <b>Максимальна встановлена швидкість</b>: використовується встановлена швидкість круїз-контролю<br><br>Перезапис скасовується, коли OpenPilot деактивується.</translation>
|
|
||||||
</message>
|
|
||||||
<message>
|
<message>
|
||||||
<source><b>Miscellaneous "Speed Limit Controller" changes</b> to fine-tune how openpilot drives.</source>
|
<source><b>Miscellaneous "Speed Limit Controller" changes</b> to fine-tune how openpilot drives.</source>
|
||||||
<translation><b>Різні зміни в «Контролері обмеження швидкості»</b> для точного налаштування керуваня openpilot.</translation>
|
<translation><b>Різні зміни в «Контролері обмеження швидкості»</b> для точного налаштування керуваня openpilot.</translation>
|
||||||
|
|||||||
@@ -2106,8 +2106,8 @@
|
|||||||
{
|
{
|
||||||
"key": "SpeedLimitController",
|
"key": "SpeedLimitController",
|
||||||
"label": "Speed Limit Controller",
|
"label": "Speed Limit Controller",
|
||||||
"description": "Limit openpilot's maximum driving speed to the current speed limit from configured map, dashboard, and optional vision sources.",
|
"description": "Limit openpilot's maximum driving speed using configured map, dashboard, and optional vision sources. Press + above a limit for a persistent override; hold the gas pedal for a temporary override.",
|
||||||
"picker_description": "Limits speed using map, dashboard, or vision data.",
|
"picker_description": "Press + above a limit for a persistent override; hold the gas pedal for a temporary override.",
|
||||||
"data_type": "bool",
|
"data_type": "bool",
|
||||||
"ui_type": "toggle",
|
"ui_type": "toggle",
|
||||||
"is_parent_toggle": true,
|
"is_parent_toggle": true,
|
||||||
@@ -2200,30 +2200,6 @@
|
|||||||
"parent_key": "SpeedLimitController",
|
"parent_key": "SpeedLimitController",
|
||||||
"settings_tier": "advanced"
|
"settings_tier": "advanced"
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"key": "SLCOverride",
|
|
||||||
"label": "Override Speed",
|
|
||||||
"description": "Choose how SLC behaves after you manually drive faster than the posted speed limit.",
|
|
||||||
"picker_description": "Chooses how SLC responds after you exceed the limit.",
|
|
||||||
"data_type": "int",
|
|
||||||
"ui_type": "dropdown",
|
|
||||||
"options": [
|
|
||||||
{
|
|
||||||
"value": 0,
|
|
||||||
"label": "None"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"value": 1,
|
|
||||||
"label": "Set With Gas Pedal"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"value": 2,
|
|
||||||
"label": "Max Set Speed"
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"parent_key": "SpeedLimitController",
|
|
||||||
"settings_tier": "advanced"
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"key": "SLCMapboxFiller",
|
"key": "SLCMapboxFiller",
|
||||||
"label": "Use Mapbox as Fallback",
|
"label": "Use Mapbox as Fallback",
|
||||||
|
|||||||
@@ -2,7 +2,6 @@
|
|||||||
# PFEIFER - SLC - Modified by FrogAi
|
# PFEIFER - SLC - Modified by FrogAi
|
||||||
import calendar
|
import calendar
|
||||||
import json
|
import json
|
||||||
import numpy as np
|
|
||||||
import requests
|
import requests
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
from concurrent.futures import ThreadPoolExecutor
|
||||||
@@ -51,9 +50,10 @@ class SpeedLimitController:
|
|||||||
|
|
||||||
self.calling_mapbox = False
|
self.calling_mapbox = False
|
||||||
self.override_slc = False
|
self.override_slc = False
|
||||||
self.override_requires_gas_release = False
|
|
||||||
self.override_disable_timer = 0.0
|
self.override_disable_timer = 0.0
|
||||||
self._prev_v_cruise = None
|
self._prev_v_cruise = None
|
||||||
|
self._persistent_override_speed = 0.0
|
||||||
|
self._set_speed_override_input_consumed = False
|
||||||
|
|
||||||
self.denied_target = 0
|
self.denied_target = 0
|
||||||
self.map_speed_limit = 0
|
self.map_speed_limit = 0
|
||||||
@@ -118,47 +118,29 @@ class SpeedLimitController:
|
|||||||
def offset(self):
|
def offset(self):
|
||||||
return self.get_offset(self.target)
|
return self.get_offset(self.target)
|
||||||
|
|
||||||
@property
|
|
||||||
def override_mode_enabled(self):
|
|
||||||
if self.starpilot_toggles is None:
|
|
||||||
return False
|
|
||||||
return self.starpilot_toggles.speed_limit_controller_override_manual or self.starpilot_toggles.speed_limit_controller_override_set_speed
|
|
||||||
|
|
||||||
def override_active(self, v_ego, gas_pressed):
|
|
||||||
target_to_use = self.target_to_use
|
|
||||||
target_with_offset = target_to_use + self.get_offset(target_to_use)
|
|
||||||
if target_with_offset <= 0 or not self.override_mode_enabled:
|
|
||||||
return False
|
|
||||||
bidirectional_set_speed = (
|
|
||||||
getattr(self.starpilot_toggles, "redneck_cruise", False) and
|
|
||||||
getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False)
|
|
||||||
)
|
|
||||||
return (
|
|
||||||
(bidirectional_set_speed and self.overridden_speed > 0) or
|
|
||||||
self.overridden_speed > target_with_offset or
|
|
||||||
(gas_pressed and v_ego > target_with_offset)
|
|
||||||
)
|
|
||||||
|
|
||||||
def low_vision_limit_filtered(self, limit):
|
def low_vision_limit_filtered(self, limit):
|
||||||
return (
|
return (
|
||||||
getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_filter", False) and
|
getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_filter", False) and
|
||||||
0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0)
|
0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0)
|
||||||
)
|
)
|
||||||
|
|
||||||
def clear_override_for_source_limit(self, desired_source, desired_target, had_override):
|
def clear_override(self):
|
||||||
if desired_source == "None" or desired_target <= 0:
|
|
||||||
return
|
|
||||||
if not had_override and self.overridden_speed <= 0:
|
|
||||||
return
|
|
||||||
if abs(desired_target - self.last_valid_limit) < 0.1:
|
|
||||||
return
|
|
||||||
|
|
||||||
# A new posted limit starts a new segment, so the previous segment's gas override
|
|
||||||
# should not carry through until the driver releases and reapplies the pedal.
|
|
||||||
self.override_slc = False
|
self.override_slc = False
|
||||||
self.overridden_speed = 0
|
self.overridden_speed = 0
|
||||||
if had_override:
|
self._persistent_override_speed = 0.0
|
||||||
self.override_requires_gas_release = True
|
|
||||||
|
def clear_persistent_override(self):
|
||||||
|
self._persistent_override_speed = 0.0
|
||||||
|
|
||||||
|
def clear_persistent_override_for_limit_change(self, previous_limit, new_limit):
|
||||||
|
if self._persistent_override_speed <= 0:
|
||||||
|
return
|
||||||
|
if previous_limit <= 0 or new_limit <= 0 or abs(new_limit - previous_limit) < 0.1:
|
||||||
|
return
|
||||||
|
|
||||||
|
new_target_with_offset = new_limit + self.get_offset(new_limit)
|
||||||
|
if new_limit < previous_limit or self._persistent_override_speed <= new_target_with_offset:
|
||||||
|
self.clear_persistent_override()
|
||||||
|
|
||||||
def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
|
def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
|
||||||
if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
|
if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
|
||||||
@@ -295,10 +277,15 @@ class SpeedLimitController:
|
|||||||
|
|
||||||
def handle_limit_change(self, desired_source, desired_target, current_road_name, v_ego, sm):
|
def handle_limit_change(self, desired_source, desired_target, current_road_name, v_ego, sm):
|
||||||
self.speed_limit_changed_timer += DT_MDL
|
self.speed_limit_changed_timer += DT_MDL
|
||||||
had_override = self.override_active(v_ego, sm["carState"].gasPressed)
|
previous_limit = self.last_valid_limit if self.last_valid_limit > 0 else self.target
|
||||||
|
|
||||||
long_active = sm["carControl"].longActive
|
long_active = sm["carControl"].longActive
|
||||||
speed_limit_accepted = sm["starpilotCarState"].accelPressed and long_active
|
accepted_by_accel_button = sm["starpilotCarState"].accelPressed and long_active
|
||||||
|
confirmation_required = desired_source != "None" and (
|
||||||
|
(desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or
|
||||||
|
(desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher)
|
||||||
|
)
|
||||||
|
speed_limit_accepted = accepted_by_accel_button
|
||||||
if not speed_limit_accepted and self._slc_adopt_counter % 4 == 0:
|
if not speed_limit_accepted and self._slc_adopt_counter % 4 == 0:
|
||||||
speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted")
|
speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted")
|
||||||
speed_limit_denied = sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active)
|
speed_limit_denied = sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active)
|
||||||
@@ -309,7 +296,9 @@ class SpeedLimitController:
|
|||||||
if speed_limit_accepted:
|
if speed_limit_accepted:
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
self.target = desired_target
|
self.target = desired_target
|
||||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||||
|
if accepted_by_accel_button and confirmation_required:
|
||||||
|
self._set_speed_override_input_consumed = True
|
||||||
|
|
||||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||||
|
|
||||||
@@ -323,13 +312,12 @@ class SpeedLimitController:
|
|||||||
elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower):
|
elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower):
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
self.target = desired_target
|
self.target = desired_target
|
||||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||||
|
|
||||||
elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher):
|
elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher):
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
self.target = desired_target
|
self.target = desired_target
|
||||||
if 0 < self.overridden_speed <= self.target + self.get_offset(self.target):
|
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
|
||||||
|
|
||||||
elif desired_target == self.target:
|
elif desired_target == self.target:
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
@@ -431,8 +419,7 @@ class SpeedLimitController:
|
|||||||
if display_only:
|
if display_only:
|
||||||
self.speed_limit_changed_timer = 0
|
self.speed_limit_changed_timer = 0
|
||||||
self.unconfirmed_speed_limit = 0
|
self.unconfirmed_speed_limit = 0
|
||||||
self.overridden_speed = 0
|
self.clear_override()
|
||||||
self.override_requires_gas_release = False
|
|
||||||
|
|
||||||
if desired_target >= 1:
|
if desired_target >= 1:
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
@@ -456,6 +443,8 @@ class SpeedLimitController:
|
|||||||
self.speed_limit_changed_timer = 0
|
self.speed_limit_changed_timer = 0
|
||||||
self.unconfirmed_speed_limit = 0
|
self.unconfirmed_speed_limit = 0
|
||||||
if desired_source != self.source or desired_target != self.target:
|
if desired_source != self.source or desired_target != self.target:
|
||||||
|
if not is_fallback:
|
||||||
|
self.clear_persistent_override_for_limit_change(current_speed, desired_target)
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
self.target = desired_target
|
self.target = desired_target
|
||||||
if desired_source != "None" and desired_target > 0:
|
if desired_source != "None" and desired_target > 0:
|
||||||
@@ -472,7 +461,7 @@ class SpeedLimitController:
|
|||||||
if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
|
if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
|
||||||
self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
|
self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
|
||||||
if desired_target > 0:
|
if desired_target > 0:
|
||||||
self.overridden_speed = 0
|
self.clear_override()
|
||||||
self.denied_target = 0
|
self.denied_target = 0
|
||||||
self.source = desired_source
|
self.source = desired_source
|
||||||
self.target = desired_target
|
self.target = desired_target
|
||||||
@@ -512,55 +501,55 @@ class SpeedLimitController:
|
|||||||
self.map_speed_limit = self.next_speed_limit
|
self.map_speed_limit = self.next_speed_limit
|
||||||
|
|
||||||
def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
|
def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
|
||||||
# Detect +/- changes on the raw set speed (button-driven, no cluster jitter). Requiring a
|
# Detect +/- changes on the raw set speed (button-driven, no cluster jitter). A fresh edge
|
||||||
# fresh edge is what makes the override clear per speed zone — once a new posted limit wipes
|
# keeps a cleared override from re-arming while the selected speed stays high.
|
||||||
# it, a steady set speed will not re-arm.
|
|
||||||
prev_v_cruise = self._prev_v_cruise
|
prev_v_cruise = self._prev_v_cruise
|
||||||
self._prev_v_cruise = v_cruise
|
self._prev_v_cruise = v_cruise
|
||||||
set_speed_changed = prev_v_cruise is not None and abs(v_cruise - prev_v_cruise) > SET_SPEED_RAISE_EPS
|
set_speed_changed = prev_v_cruise is not None and abs(v_cruise - prev_v_cruise) > SET_SPEED_RAISE_EPS
|
||||||
set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS
|
set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS
|
||||||
|
set_speed_input_consumed = self._set_speed_override_input_consumed
|
||||||
|
# The button and its vCruise update can arrive in adjacent frames. Clear a consumed
|
||||||
|
# confirmation only after this frame has seen the speed change or button release.
|
||||||
|
if set_speed_input_consumed and (set_speed_changed or not sm["starpilotCarState"].accelPressed):
|
||||||
|
self._set_speed_override_input_consumed = False
|
||||||
|
|
||||||
if not sm["selfdriveState"].enabled:
|
if not sm["selfdriveState"].enabled:
|
||||||
self.override_disable_timer += DT_MDL
|
self.override_disable_timer += DT_MDL
|
||||||
if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
|
if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
|
||||||
self.override_slc = False
|
self.clear_override()
|
||||||
self.overridden_speed = 0
|
|
||||||
self.override_requires_gas_release = False
|
|
||||||
return
|
return
|
||||||
|
|
||||||
self.override_disable_timer = 0.0
|
self.override_disable_timer = 0.0
|
||||||
|
|
||||||
if not sm["carState"].gasPressed:
|
|
||||||
self.override_requires_gas_release = False
|
|
||||||
|
|
||||||
target_to_use = self.target_to_use
|
target_to_use = self.target_to_use
|
||||||
offset = self.get_offset(target_to_use)
|
target_with_offset = target_to_use + self.get_offset(target_to_use)
|
||||||
set_speed = v_cruise + v_cruise_diff
|
|
||||||
bidirectional_set_speed = (
|
|
||||||
getattr(self.starpilot_toggles, "redneck_cruise", False) and
|
|
||||||
getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False)
|
|
||||||
)
|
|
||||||
self.override_slc = self.override_slc and (
|
|
||||||
(bidirectional_set_speed and self.overridden_speed > 0) or
|
|
||||||
self.overridden_speed > target_to_use + offset > 0
|
|
||||||
)
|
|
||||||
self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0
|
|
||||||
# Redneck Max Set Speed mode uses +/- as a direct, bidirectional SLC override. The normal
|
|
||||||
# mode retains its existing upward-only behavior for full-long cars.
|
|
||||||
self.override_slc |= (
|
|
||||||
self.starpilot_toggles.speed_limit_controller_override_set_speed and
|
|
||||||
target_to_use + offset > 0 and
|
|
||||||
set_speed > 0 and
|
|
||||||
((bidirectional_set_speed and set_speed_changed) or
|
|
||||||
(not bidirectional_set_speed and set_speed_raised and set_speed > target_to_use + offset > 0))
|
|
||||||
)
|
|
||||||
|
|
||||||
if self.override_slc:
|
set_speed = v_cruise + v_cruise_diff
|
||||||
if self.starpilot_toggles.speed_limit_controller_override_manual:
|
bidirectional_set_speed = getattr(self.starpilot_toggles, "redneck_cruise", False)
|
||||||
if sm["carState"].gasPressed:
|
|
||||||
self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
|
if self._persistent_override_speed > 0:
|
||||||
self.overridden_speed = float(np.clip(self.overridden_speed, target_to_use + offset, v_cruise + v_cruise_diff))
|
if bidirectional_set_speed:
|
||||||
elif self.starpilot_toggles.speed_limit_controller_override_set_speed:
|
if set_speed <= 0:
|
||||||
self.overridden_speed = set_speed
|
self.clear_persistent_override()
|
||||||
|
else:
|
||||||
|
self._persistent_override_speed = set_speed
|
||||||
|
elif set_speed <= 0 or (target_with_offset > 0 and set_speed <= target_with_offset and (self.source != "None" or set_speed_changed)):
|
||||||
|
self.clear_persistent_override()
|
||||||
|
else:
|
||||||
|
self._persistent_override_speed = set_speed
|
||||||
|
elif (
|
||||||
|
target_with_offset > 0
|
||||||
|
and set_speed > 0
|
||||||
|
and not set_speed_input_consumed
|
||||||
|
and ((bidirectional_set_speed and set_speed_changed) or (not bidirectional_set_speed and set_speed_raised and set_speed > target_with_offset))
|
||||||
|
):
|
||||||
|
self._persistent_override_speed = set_speed
|
||||||
|
|
||||||
|
if sm["carState"].gasPressed and v_ego > target_with_offset > 0:
|
||||||
|
self.override_slc = True
|
||||||
|
self.overridden_speed = v_ego + v_ego_diff
|
||||||
|
elif self._persistent_override_speed > 0:
|
||||||
|
self.override_slc = True
|
||||||
|
self.overridden_speed = self._persistent_override_speed
|
||||||
else:
|
else:
|
||||||
self.overridden_speed = 0
|
self.clear_override()
|
||||||
|
|||||||
@@ -221,8 +221,7 @@ class StarPilotAcceleration:
|
|||||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
||||||
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
||||||
max(float(getattr(sm["carState"], "vEgoCluster", v_ego) or v_ego), v_ego) - v_ego,
|
max(float(getattr(sm["carState"], "vEgoCluster", v_ego) or v_ego), v_ego) - v_ego,
|
||||||
allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and
|
allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False),
|
||||||
getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)),
|
|
||||||
)
|
)
|
||||||
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
||||||
if effective_slc_target > 0.0:
|
if effective_slc_target > 0.0:
|
||||||
@@ -277,8 +276,7 @@ class StarPilotAcceleration:
|
|||||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
||||||
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
||||||
v_ego_diff,
|
v_ego_diff,
|
||||||
allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and
|
allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False),
|
||||||
getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)),
|
|
||||||
)
|
)
|
||||||
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
||||||
if effective_slc_target > 0.0:
|
if effective_slc_target > 0.0:
|
||||||
@@ -319,8 +317,7 @@ class StarPilotAcceleration:
|
|||||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
|
||||||
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
|
||||||
max(v_ego_cluster, v_ego) - v_ego,
|
max(v_ego_cluster, v_ego) - v_ego,
|
||||||
allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and
|
allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False),
|
||||||
getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)),
|
|
||||||
)
|
)
|
||||||
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
|
||||||
if effective_slc_target > 0.0:
|
if effective_slc_target > 0.0:
|
||||||
|
|||||||
@@ -752,8 +752,7 @@ class StarPilotVCruise:
|
|||||||
self.slc_offset,
|
self.slc_offset,
|
||||||
self.slc.overridden_speed,
|
self.slc.overridden_speed,
|
||||||
v_ego_diff,
|
v_ego_diff,
|
||||||
allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and
|
allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False),
|
||||||
getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)),
|
|
||||||
)
|
)
|
||||||
slc_control_target = get_slc_lead_drop_relaxed_target(
|
slc_control_target = get_slc_lead_drop_relaxed_target(
|
||||||
slc_control_target,
|
slc_control_target,
|
||||||
|
|||||||
@@ -96,7 +96,6 @@ def _toggles(document):
|
|||||||
set_speed_limit=False,
|
set_speed_limit=False,
|
||||||
set_speed_offset=0.0,
|
set_speed_offset=0.0,
|
||||||
speed_limit_controller=False,
|
speed_limit_controller=False,
|
||||||
speed_limit_controller_override_set_speed=False,
|
|
||||||
truck_tuning=False,
|
truck_tuning=False,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -44,6 +44,20 @@ def test_galaxy_layout_removes_obsolete_and_duplicate_controls():
|
|||||||
) == 1
|
) == 1
|
||||||
|
|
||||||
|
|
||||||
|
def test_slc_override_method_is_not_exposed_in_either_settings_ui():
|
||||||
|
layout = _layout()
|
||||||
|
galaxy_keys = {
|
||||||
|
param["key"]
|
||||||
|
for section in layout
|
||||||
|
for param in section.get("params", [])
|
||||||
|
}
|
||||||
|
device_ui = (REPO_ROOT / "selfdrive/ui/layouts/settings/starpilot/longitudinal.py").read_text(encoding="utf-8")
|
||||||
|
|
||||||
|
assert "SLCOverride" not in galaxy_keys
|
||||||
|
assert 'SettingRow("SLCOverride"' not in device_ui
|
||||||
|
assert "SLC_OVERRIDE_OPTIONS" not in device_ui
|
||||||
|
|
||||||
|
|
||||||
def test_galaxy_layout_contains_basic_mode_controls():
|
def test_galaxy_layout_contains_basic_mode_controls():
|
||||||
sections = _params_by_section(_layout())
|
sections = _params_by_section(_layout())
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user