mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-02 04:13:46 +08:00
Compare commits
7 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 52999cb7b2 | |||
| 1e6b221d53 | |||
| 11900616bf | |||
| a60333e513 | |||
| 9d87c4c8cb | |||
| a45ecf73ab | |||
| 452dc42868 |
@@ -237,6 +237,8 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
|
||||
cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin
|
||||
cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m
|
||||
approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off
|
||||
slcPresentedSpeedLimitSource @43 :Text; # source of the shown accepted or pending posted limit
|
||||
slcIsLimitingMaxSet @44 :Bool; # SLC target is below the configured Max Set
|
||||
}
|
||||
|
||||
struct StarPilotRadarState @0xb86e6369214c01c8 {
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.can import CANPacker
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs
|
||||
from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
|
||||
# This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md
|
||||
# (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md.
|
||||
@@ -64,7 +64,9 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
|
||||
return apply_curvature
|
||||
|
||||
|
||||
def apply_creep_compensation(accel: float, v_ego: float) -> float:
|
||||
def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float:
|
||||
if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping):
|
||||
return accel
|
||||
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
|
||||
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
|
||||
accel -= creep_accel
|
||||
@@ -181,12 +183,11 @@ class CarController(CarControllerBase):
|
||||
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
|
||||
accel = actuators.accel
|
||||
gas = accel
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
||||
if CC.longActive:
|
||||
# Compensate for engine creep at low speed.
|
||||
# Either the ABS does not account for engine creep, or the correction is very slow
|
||||
# TODO: verify this applies to EV/hybrid
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo)
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint,
|
||||
standstill=CS.out.standstill, stopping=stopping)
|
||||
|
||||
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
|
||||
# however even 3.5 m/s^3 causes some overshoot with a step response.
|
||||
@@ -210,7 +211,6 @@ class CarController(CarControllerBase):
|
||||
elif accel_pitch_compensated < 0.0:
|
||||
self.brake_request = True
|
||||
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
# TODO: look into using the actuators packet to send the desired speed
|
||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@ import pytest
|
||||
from opendbc.car import Bus, gen_empty_fingerprint
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.carcontroller import FordStockCruiseButton
|
||||
from opendbc.car.ford.carcontroller import FordStockCruiseButton, apply_creep_compensation
|
||||
from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.fw_versions import build_fw_dict
|
||||
@@ -38,6 +38,17 @@ def test_stock_cruise_button_ignores_press_with_cruise_master_off():
|
||||
assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False)
|
||||
|
||||
|
||||
def test_mach_e_does_not_apply_engine_creep_compensation():
|
||||
for accel in (-1.0, -0.1, 0.0, 0.1):
|
||||
assert apply_creep_compensation(accel, 0.5, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
standstill=False, stopping=False) == accel
|
||||
|
||||
assert apply_creep_compensation(0.0, 0.0, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
standstill=True, stopping=True) == -0.6
|
||||
assert apply_creep_compensation(0.0, 0.5, CAR.FORD_F_150_MK14,
|
||||
standstill=False, stopping=False) == -0.6
|
||||
|
||||
|
||||
ECU_ADDRESSES = {
|
||||
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
|
||||
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
|
||||
|
||||
@@ -770,7 +770,8 @@ class CarController(CarControllerBase):
|
||||
lead_data = self._can_lead_data.update(lead_distance, lead_rel_speed, lead_visible)
|
||||
|
||||
# HUD messages
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
stinger_hud_enabled = CC.enabled or (self.CP.carFingerprint == CAR.KIA_STINGER_2022 and CC.latActive)
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(stinger_hud_enabled, self.car_fingerprint,
|
||||
hud_control)
|
||||
|
||||
if blended_hda2:
|
||||
|
||||
@@ -12,7 +12,7 @@ _adrv_0x51_templates: dict[CAR, bytes] = {}
|
||||
|
||||
|
||||
def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
|
||||
if car_fingerprint != CAR.KIA_EV6:
|
||||
if car_fingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN):
|
||||
return
|
||||
|
||||
if dat is None:
|
||||
@@ -26,7 +26,6 @@ def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None
|
||||
if template is None:
|
||||
return packer.make_can_msg("ADRV_0x51", CAN.ACAN, {})
|
||||
|
||||
# EV6 MRR30 tracks stop when the ADAS takeover replaces this platform payload with zeros.
|
||||
dat = bytearray(template)
|
||||
dat[2] = (template[2] + frame + 1) & 0xFF
|
||||
dat[3] = (dat[3] & ~0x1) | int(drive_gear)
|
||||
|
||||
@@ -395,7 +395,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
if not skip_disable_ecu:
|
||||
disable_can_recv = can_recv
|
||||
if CP.carFingerprint == CAR.KIA_EV6 and can_recv is not None:
|
||||
if CP.carFingerprint in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and can_recv is not None:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None)
|
||||
base_can_recv = can_recv
|
||||
adrv_bus = CanBus(CP).ACAN
|
||||
|
||||
@@ -1316,6 +1316,103 @@ class TestHyundaiFingerprint:
|
||||
assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS
|
||||
assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable
|
||||
|
||||
def test_sportage_hev_hda2_redneck_uses_stock_scc(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def get_bool(key):
|
||||
return key == "RedneckCruise"
|
||||
|
||||
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
|
||||
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
assert FPCP.redneckCruiseAvailable
|
||||
assert not FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert not CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
|
||||
controller = CarInterface(CP, FPCP).CC
|
||||
assert not controller.long_active_ecu
|
||||
|
||||
controller.frame = 30
|
||||
CS = SimpleNamespace(redneck_send_button=1, buttons_counter=5)
|
||||
msgs = controller._create_canfd_redneck_button_messages(CS)
|
||||
assert len(msgs) == 20
|
||||
assert all(msg[0] == 0x1CF and msg[2] == can_bus.ECAN for msg in msgs)
|
||||
assert all(msg[1][2] & 0x7 == Buttons.RES_ACCEL for msg in msgs)
|
||||
|
||||
controller.frame = 60
|
||||
CS.redneck_send_button = 2
|
||||
msgs = controller._create_canfd_redneck_button_messages(CS)
|
||||
assert len(msgs) == 20
|
||||
assert all(msg[1][2] & 0x7 == Buttons.SET_DECEL for msg in msgs)
|
||||
|
||||
monkeypatch.setattr(FakeParams, "get_bool", staticmethod(lambda key: False))
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert FPCP.redneckCruiseAvailable
|
||||
assert FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
|
||||
def test_sportage_redneck_rejects_unverified_button_layouts(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def get_bool(key):
|
||||
return key == "RedneckCruise"
|
||||
|
||||
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
|
||||
toggles = get_test_toggles()
|
||||
for button_address, button_bus, button_length, lka_steering in (
|
||||
(0x1AA, 1, 16, True),
|
||||
(0x1CF, 0, 8, True),
|
||||
(0x1CF, 0, 8, False),
|
||||
(0x1CF, 1, 16, True),
|
||||
):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, lka_steering)
|
||||
if lka_steering:
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[button_bus][button_address] = button_length
|
||||
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
assert FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
fingerprint[can_bus.ECAN][0x1AA] = 16
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
|
||||
def test_hyundai_non_scc_without_redneck_keeps_stock_longitudinal_mode(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
|
||||
@@ -248,12 +248,24 @@ class CarInterfaceBase(ABC):
|
||||
(candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6):
|
||||
fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value
|
||||
|
||||
fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED))
|
||||
sportage_stock_scc_buttons = (
|
||||
candidate == HYUNDAI.KIA_SPORTAGE_HEV_2026 and
|
||||
not CP.openpilotLongitudinalControl and
|
||||
bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
fingerprint[CAN.ECAN].get(0x1CF) == 8 and
|
||||
0x1AA not in fingerprint[CAN.ECAN]
|
||||
)
|
||||
fp_ret.redneckCruiseAvailable = (
|
||||
(bool(CP.flags & HyundaiFlags.NON_SCC) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) or
|
||||
sportage_stock_scc_buttons
|
||||
)
|
||||
if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
|
||||
fp_ret.pcmCruiseSpeed = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
if CP.flags & HyundaiFlags.NON_SCC:
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl:
|
||||
fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value
|
||||
|
||||
@@ -992,6 +992,22 @@ class TestSportageNoStockLka(unittest.TestCase):
|
||||
"Damping_Gain": 100,
|
||||
})
|
||||
|
||||
def test_stock_scc_buttons_require_engagement(self):
|
||||
resume = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.RESUME})
|
||||
set_button = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.SET})
|
||||
self.assertFalse(self.safety.safety_tx_hook(resume))
|
||||
self.assertFalse(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
self.safety.safety_rx_hook(set_button)
|
||||
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 1}))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.safety.safety_tx_hook(resume))
|
||||
self.assertTrue(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 0}))
|
||||
self.assertFalse(self.safety.safety_tx_hook(resume))
|
||||
self.assertFalse(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
def test_aol_toggle_keeps_stock_blocked_and_inactive_status_allowed(self):
|
||||
self._speed(30)
|
||||
for expected_aol in (False, True, False, True, False):
|
||||
|
||||
@@ -197,7 +197,9 @@ class RedneckCruise:
|
||||
def _update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
|
||||
update_manual_button_timers(CS, self.cruise_button_timers)
|
||||
button_pressed = any(0 < timer <= int(MANUAL_BUTTON_INACTIVE_TIMER / DT_CTRL) for timer in self.cruise_button_timers.values())
|
||||
self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed
|
||||
stock_cruise_ready = (not self.CP.pcmCruise or self.CP.openpilotLongitudinalControl or CS.cruiseState.enabled)
|
||||
self.is_ready = (CC.enabled and stock_cruise_ready and not CC.cruiseControl.override and
|
||||
not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed)
|
||||
|
||||
def _desired_state(self) -> str:
|
||||
if self.v_target > self.v_cruise_cluster:
|
||||
|
||||
@@ -118,14 +118,17 @@ class TestVCruiseHelper:
|
||||
)
|
||||
assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
|
||||
|
||||
def test_accel_stops_at_slc_target_before_crossing_it(self):
|
||||
@pytest.mark.parametrize(
|
||||
("starting_kph", "waypoint_kph", "expected_speeds"),
|
||||
[(30, 33, (33, 35, 40)), (65, 68, (68, 70, 75))],
|
||||
)
|
||||
def test_accel_stops_at_slc_target_before_crossing_it(self, starting_kph, waypoint_kph, expected_speeds):
|
||||
self.starpilot_toggles.cruise_increase = 5
|
||||
self.v_cruise_helper.v_cruise_kph = 30
|
||||
self.v_cruise_helper.v_cruise_cluster_kph = 30
|
||||
slc_target_with_offset = 33 * CV.KPH_TO_MS
|
||||
self.v_cruise_helper.v_cruise_kph = starting_kph
|
||||
self.v_cruise_helper.v_cruise_cluster_kph = starting_kph
|
||||
slc_target_with_offset = waypoint_kph * CV.KPH_TO_MS
|
||||
|
||||
# A 30 km/h limit with a +3 km/h SLC offset should be an intermediate stop.
|
||||
for expected_kph in (33, 35, 40):
|
||||
for expected_kph in expected_speeds:
|
||||
for pressed in (True, False):
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
|
||||
|
||||
@@ -26,13 +26,13 @@ ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class TestRedneckCruise(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.CP = SimpleNamespace()
|
||||
self.CP = SimpleNamespace(pcmCruise=False, openpilotLongitudinalControl=False)
|
||||
self.FPCP = SimpleNamespace(pcmCruiseSpeed=False, redneckCruiseAvailable=True)
|
||||
self.redneck = RedneckCruise(self.CP, self.FPCP)
|
||||
|
||||
def _new_state(self, speed_cluster_mph=20.0, button_events=None):
|
||||
def _new_state(self, speed_cluster_mph=20.0, button_events=None, cruise_enabled=True):
|
||||
return SimpleNamespace(
|
||||
cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS),
|
||||
cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS, enabled=cruise_enabled),
|
||||
buttonEvents=button_events or [],
|
||||
)
|
||||
|
||||
@@ -143,6 +143,21 @@ class TestRedneckCruise(unittest.TestCase):
|
||||
send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, **kwargs)
|
||||
self.assertEqual(SEND_BUTTON_NONE, send_button)
|
||||
|
||||
def test_stock_scc_only_sends_buttons_while_engaged(self):
|
||||
self.CP.pcmCruise = True
|
||||
frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4
|
||||
for _ in range(frames):
|
||||
send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(),
|
||||
25.0 * CV.MPH_TO_MS, is_metric=False)
|
||||
self.assertEqual(SEND_BUTTON_NONE, send_button)
|
||||
|
||||
send_button, _ = self._run_until_active(target_mph=25.0)
|
||||
self.assertEqual(SEND_BUTTON_INCREASE, send_button)
|
||||
|
||||
send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(),
|
||||
25.0 * CV.MPH_TO_MS, is_metric=False)
|
||||
self.assertEqual(SEND_BUTTON_NONE, send_button)
|
||||
|
||||
def test_resets_when_pcm_cruise_speed_is_enabled(self):
|
||||
self.FPCP.pcmCruiseSpeed = True
|
||||
send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0)
|
||||
|
||||
@@ -35,6 +35,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
is_gm_silverado_early_follow_lead,
|
||||
is_toyota_rav4_tss2_post_departure_tune,
|
||||
get_toyota_rav4_tss2_early_lead_cap,
|
||||
get_toyota_corolla_braking_lead_cap,
|
||||
is_toyota_rav4_tss2_radar_follow_lead,
|
||||
get_toyota_sienna_post_departure_restop_cap,
|
||||
get_untracked_slow_lead_decel_scale,
|
||||
@@ -2580,6 +2581,16 @@ class LongitudinalPlanner:
|
||||
vision_low_speed_stop_active = False
|
||||
vision_brake_cap_active = False
|
||||
if lead_control_active:
|
||||
if (not experimental_mode and
|
||||
not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
|
||||
not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
|
||||
not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False))):
|
||||
corolla_cap = get_toyota_corolla_braking_lead_cap(
|
||||
self.CP, self.lead_one, v_ego,
|
||||
desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow), output_accel_min,
|
||||
)
|
||||
if corolla_cap is not None:
|
||||
close_lead_caps.append(corolla_cap)
|
||||
for lead in (self.lead_one, self.lead_two):
|
||||
rav4_early_lead_cap = get_toyota_rav4_tss2_early_lead_cap(
|
||||
self.CP, lead, v_ego, output_accel_min,
|
||||
|
||||
@@ -24,6 +24,8 @@ HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25
|
||||
HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25
|
||||
GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75
|
||||
KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE = 1.5
|
||||
KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
|
||||
KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
|
||||
GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER = 1.5
|
||||
GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER = 2.0
|
||||
GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER = 5.0
|
||||
@@ -66,6 +68,7 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB = 0.95
|
||||
TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET = 1.75
|
||||
TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
|
||||
TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
|
||||
TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL = 1.5
|
||||
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_MAX_LATERAL_OFFSET = 1.2
|
||||
@@ -168,6 +171,31 @@ def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego):
|
||||
return float(min(bias, max(distance - 0.5, 0.0)))
|
||||
|
||||
|
||||
def get_toyota_corolla_braking_lead_cap(CP, lead, v_ego, desired_gap, accel_min):
|
||||
if (
|
||||
getattr(CP, "brand", "") != "toyota" or
|
||||
str(getattr(CP, "carFingerprint", "")) != "TOYOTA_COROLLA_TSS2" or
|
||||
lead is None or not bool(getattr(lead, "status", False)) or
|
||||
not bool(getattr(lead, "radar", False)) or
|
||||
float(v_ego) < 10.0 or
|
||||
float(getattr(lead, "vLead", 0.0)) < 2.0 or
|
||||
abs(float(getattr(lead, "yRel", 0.0))) > 1.2
|
||||
):
|
||||
return None
|
||||
|
||||
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
|
||||
distance = float(getattr(lead, "dRel", float("inf")))
|
||||
if (
|
||||
lead_brake < 0.6 or
|
||||
float(v_ego) - float(lead.vLead) < 0.75 or
|
||||
distance <= 0.0 or distance > min(60.0, 3.0 * float(v_ego)) or
|
||||
distance > float(desired_gap) + 6.0
|
||||
):
|
||||
return None
|
||||
|
||||
return max(float(accel_min), -min(TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL, 0.65 * lead_brake))
|
||||
|
||||
|
||||
def is_honda_crv_5g(CP):
|
||||
return (
|
||||
getattr(CP, "brand", "") == "honda" and
|
||||
@@ -532,6 +560,11 @@ def allow_radar_standstill_gap_settle(CP):
|
||||
|
||||
|
||||
def get_far_follow_output_slew_rates(CP):
|
||||
if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV":
|
||||
return (
|
||||
KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE,
|
||||
KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE,
|
||||
)
|
||||
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
|
||||
return (
|
||||
HONDA_ACCORD_FAR_FOLLOW_BRAKE_SLEW_RATE,
|
||||
|
||||
@@ -376,6 +376,30 @@ def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow():
|
||||
assert smoothed == pytest.approx(-0.5)
|
||||
|
||||
|
||||
def test_niro_ev_far_follow_slew_is_vehicle_specific_and_preserves_urgent_braking():
|
||||
CP = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV)
|
||||
next_gen = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV_2ND_GEN)
|
||||
planner = LongitudinalPlanner(CP, init_v=16.0)
|
||||
planner.lead_one = make_lead(status=True, d_rel=45.0, v_lead=15.0, model_prob=0.99, y_rel=0.0)
|
||||
planner.lead_two = make_lead(status=False)
|
||||
|
||||
assert get_far_follow_output_slew_rates(CP) == pytest.approx((2.5, 1.75))
|
||||
assert get_far_follow_output_slew_rates(next_gen) == (0.0, 0.0)
|
||||
first = planner.get_vehicle_far_follow_slew_target(16.0, 0.0, -0.4, False, False)
|
||||
release = planner.get_vehicle_far_follow_slew_target(16.0, first, 0.3, False, False)
|
||||
assert first == pytest.approx(-0.4)
|
||||
assert release == pytest.approx(first + 1.75 * planner.dt)
|
||||
|
||||
planner.lead_one.dRel = 18.0
|
||||
assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5)
|
||||
assert not planner.far_follow_output_slew_active
|
||||
|
||||
planner.lead_one.dRel = 45.0
|
||||
planner.lead_one.vLead = 10.0
|
||||
assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5)
|
||||
assert not planner.far_follow_output_slew_active
|
||||
|
||||
|
||||
def test_crv_far_follow_output_slew_damps_nonurgent_lead_transition():
|
||||
v_ego = 24.0
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -64,6 +64,7 @@ def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=Fal
|
||||
"carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill, vEgoCluster=v_ego_cluster),
|
||||
"carControl": SimpleNamespace(orientationNED=[0.0, pitch, 0.0]),
|
||||
"controlsState": SimpleNamespace(forceDecel=force_decel),
|
||||
"selfdriveState": SimpleNamespace(personality=1),
|
||||
"radarState": SimpleNamespace(
|
||||
leadOne=lead_one or make_lead(),
|
||||
leadTwo=lead_two or make_lead(),
|
||||
|
||||
@@ -2,6 +2,7 @@ import datetime
|
||||
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
|
||||
@@ -36,15 +37,26 @@ class FakeParams:
|
||||
def get_float(self, *args, **kwargs):
|
||||
return 0.0
|
||||
|
||||
def get_bool(self, key):
|
||||
return bool(self.values.get(key, False))
|
||||
|
||||
def remove(self, key):
|
||||
self.values.pop(key, None)
|
||||
|
||||
def put_nonblocking(self, key, value):
|
||||
self.values[key] = value
|
||||
self.writes.append((key, value))
|
||||
|
||||
def put_float(self, key, value):
|
||||
self.values[key] = value
|
||||
|
||||
|
||||
def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None, road_curvature=0.0):
|
||||
planner = SimpleNamespace(
|
||||
params=FakeParams(),
|
||||
params_memory=FakeParams({"NavInstructionState": nav_state or {}}),
|
||||
gps_position={},
|
||||
gps_valid=False,
|
||||
lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
|
||||
starpilot_cem=SimpleNamespace(stop_light_detected=red_light),
|
||||
starpilot_following=SimpleNamespace(following_lead=False),
|
||||
@@ -77,9 +89,14 @@ def make_sm(*, standstill=True, min_steer_speed=0.0, car_fingerprint=""):
|
||||
leftBlinker=False,
|
||||
rightBlinker=False,
|
||||
steeringAngleDeg=0.0,
|
||||
vCruise=72.0,
|
||||
),
|
||||
"liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
|
||||
"mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0,
|
||||
waySelectionType=custom.WaySelectionType.fail, roadName=""),
|
||||
"selfdriveState": SimpleNamespace(enabled=True),
|
||||
"carParams": SimpleNamespace(minSteerSpeed=min_steer_speed, carFingerprint=car_fingerprint),
|
||||
"starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0),
|
||||
"starpilotCarState": SimpleNamespace(accelPressed=False, decelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0),
|
||||
"onroadEvents": [],
|
||||
}
|
||||
|
||||
@@ -105,6 +122,27 @@ def make_toggles():
|
||||
nav_longitudinal_allowed=False,
|
||||
speed_limit_controller=False,
|
||||
show_speed_limits=False,
|
||||
is_metric=False,
|
||||
map_speed_lookahead_higher=0.0,
|
||||
map_speed_lookahead_lower=0.0,
|
||||
slc_fallback_previous_speed_limit=False,
|
||||
slc_fallback_set_speed=False,
|
||||
slc_fallback_experimental_mode=False,
|
||||
slc_mapbox_filler=False,
|
||||
speed_limit_confirmation_higher=False,
|
||||
speed_limit_confirmation_lower=False,
|
||||
speed_limit_priority1="Dashboard",
|
||||
speed_limit_priority2="Map Data",
|
||||
speed_limit_priority_highest=False,
|
||||
speed_limit_priority_lowest=False,
|
||||
vision_speed_limit_detection=False,
|
||||
speed_limit_offset1=0.0,
|
||||
speed_limit_offset2=0.0,
|
||||
speed_limit_offset3=0.0,
|
||||
speed_limit_offset4=0.0,
|
||||
speed_limit_offset5=0.0,
|
||||
speed_limit_offset6=0.0,
|
||||
speed_limit_offset7=0.0,
|
||||
force_stop_distance_offset=0,
|
||||
)
|
||||
|
||||
@@ -112,7 +150,6 @@ def make_toggles():
|
||||
def test_active_slc_control_target_does_not_require_set_speed_limit():
|
||||
target = get_active_slc_control_target(
|
||||
speed_limit_controller=True,
|
||||
set_speed_limit=False,
|
||||
slc_target=45.0 * CV.MPH_TO_MS,
|
||||
slc_offset=3.0 * CV.MPH_TO_MS,
|
||||
overridden_speed=0.0,
|
||||
@@ -145,8 +182,7 @@ def test_active_slc_target_constrains_vcruise_below_csc_minimum(slc_target_mph,
|
||||
|
||||
vcruise.slc.target = slc_target_mph * CV.MPH_TO_MS
|
||||
vcruise.slc.source = "Dashboard"
|
||||
vcruise.slc.update_limits = lambda *_args, **_kwargs: None
|
||||
vcruise.slc.update_override = lambda *_args, **_kwargs: None
|
||||
vcruise.slc.update = lambda *_args, **_kwargs: None
|
||||
|
||||
result = update_vcruise(
|
||||
vcruise,
|
||||
@@ -158,6 +194,7 @@ def test_active_slc_target_constrains_vcruise_below_csc_minimum(slc_target_mph,
|
||||
)
|
||||
|
||||
assert result == pytest.approx(expected_v_cruise_mph * CV.MPH_TO_MS)
|
||||
assert vcruise.slc_is_limiting_max_set == (expected_v_cruise_mph < 35.0)
|
||||
|
||||
|
||||
def test_elantra_gets_lead_veto_margin_before_force_stop():
|
||||
@@ -417,6 +454,8 @@ def test_csc_res_press_defers_to_slc_confirmation():
|
||||
sm = make_sm(standstill=False)
|
||||
toggles = make_toggles()
|
||||
toggles.curve_speed_controller = True
|
||||
toggles.speed_limit_controller = True
|
||||
toggles.speed_limit_confirmation_higher = True
|
||||
|
||||
def set_curve_target(_v_ego, _v_cruise):
|
||||
vcruise.csc.target = 14.0
|
||||
@@ -426,10 +465,12 @@ def test_csc_res_press_defers_to_slc_confirmation():
|
||||
assert vcruise.csc_controlling_speed
|
||||
|
||||
|
||||
vcruise.slc.speed_limit_changed_timer = 1.0
|
||||
vcruise.slc.unconfirmed_speed_limit = 25.0
|
||||
# The candidate and accel press arrive together. SLC consumes the press in
|
||||
# this frame, even though accepting immediately clears the pending state.
|
||||
sm["starpilotCarState"].dashboardSpeedLimit = 45.0 * CV.MPH_TO_MS
|
||||
sm["starpilotCarState"].accelPressed = True
|
||||
update_vcruise(vcruise, sm, toggles, now=80.05, v_ego=20.0)
|
||||
assert vcruise.slc.confirmation_button_consumed
|
||||
assert not vcruise.csc_override
|
||||
|
||||
sm["starpilotCarState"].accelPressed = False
|
||||
@@ -622,7 +663,6 @@ def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets()
|
||||
def test_active_slc_control_target_applies_offset_and_cluster_diff():
|
||||
target = get_active_slc_control_target(
|
||||
speed_limit_controller=True,
|
||||
set_speed_limit=True,
|
||||
slc_target=45.0 * CV.MPH_TO_MS,
|
||||
slc_offset=3.0 * CV.MPH_TO_MS,
|
||||
overridden_speed=0.0,
|
||||
@@ -635,7 +675,6 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff():
|
||||
def test_active_slc_control_target_allows_lower_redneck_override():
|
||||
target = get_active_slc_control_target(
|
||||
speed_limit_controller=True,
|
||||
set_speed_limit=False,
|
||||
slc_target=65.0 * CV.MPH_TO_MS,
|
||||
slc_offset=0.0,
|
||||
overridden_speed=35.0 * CV.MPH_TO_MS,
|
||||
|
||||
@@ -628,7 +628,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
|
||||
set_state=lambda s: self._params.put_bool("SLCMapboxFiller", s),
|
||||
visible=self._mapbox_available),
|
||||
SettingRow("ShowSLCOffset", "toggle", tr_noop("Show SLC Offset"),
|
||||
subtitle="",
|
||||
subtitle=tr_noop("Compact display only; the unified card always shows nonzero offsets."),
|
||||
get_state=lambda: self._params.get_bool("ShowSLCOffset"),
|
||||
set_state=lambda s: self._params.put_bool("ShowSLCOffset", s)),
|
||||
SettingRow("SpeedLimitSources", "toggle", tr_noop("Show Sources"),
|
||||
|
||||
@@ -1,16 +1,13 @@
|
||||
import math
|
||||
from typing import Optional
|
||||
|
||||
import pyray as rl
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
|
||||
CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, SLC_HEIGHT,
|
||||
draw_control_card, roundness_for,
|
||||
CONTROL_BORDER, CONTROL_ROUNDNESS, CONTROL_SEGMENTS,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
|
||||
enabled_source_titles, fit_source_label, source_abbreviated_value_text,
|
||||
@@ -23,14 +20,6 @@ _WHITE = rl.Color(255, 255, 255, 255)
|
||||
|
||||
# ── Constants ─────────────────────────────────────────────────────────
|
||||
|
||||
# EU Vienna sign
|
||||
EU_SIGN_SIZE = 176
|
||||
EU_SIGN_WIDTH = 176
|
||||
RED_RING_WIDTH = 20
|
||||
|
||||
# Pending sign blink cadence — 1s period, 50% duty cycle.
|
||||
PENDING_BLINK_MS = 500
|
||||
|
||||
# Source display metadata: source name, main label, value key, bubble label, icon.
|
||||
SOURCE_DEFS = [
|
||||
("Dashboard", "Dash", "dashboard_sl", "Dashboard", "dashboard"),
|
||||
@@ -39,16 +28,12 @@ SOURCE_DEFS = [
|
||||
("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "map"),
|
||||
("Upcoming", "NEXT", "next_sl", "Next", "next"),
|
||||
]
|
||||
_SOURCE_ICON_KEYS = {source: icon for source, _, _, _, icon in SOURCE_DEFS}
|
||||
|
||||
# Fonts
|
||||
FONT_LABEL = 30
|
||||
FONT_SOURCE = 40 # Set Speed MAX label size.
|
||||
FONT_SPEED = 90 # Set Speed value size.
|
||||
FONT_OFFSET = 29 # Compact offset text.
|
||||
OFFSET_CHIP_SEGMENTS = 8 # Capsule curve segments.
|
||||
FONT_EU_LARGE = 70
|
||||
FONT_EU_SMALL = 60
|
||||
FONT_EU_OFFSET = 40
|
||||
|
||||
def source_icon_key(source: str) -> str | None:
|
||||
"""Use the same source glyph as the detailed source diagnostics."""
|
||||
return _SOURCE_ICON_KEYS.get(source)
|
||||
|
||||
# Vision speed-limit pulse — one-shot purple highlight when the active source
|
||||
# is "Vision" and the resolved value just changed.
|
||||
@@ -90,8 +75,21 @@ def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color:
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────────
|
||||
|
||||
def _is_slc_enabled() -> bool:
|
||||
toggles = getattr(ui_state, "starpilot_toggles", {})
|
||||
if "speed_limit_controller" in toggles:
|
||||
return bool(toggles["speed_limit_controller"])
|
||||
return ui_state.ui_params.get_bool("SpeedLimitController")
|
||||
|
||||
|
||||
def _get_slc_state():
|
||||
"""Extract SLC state from SubMaster. Returns dict or None if stale/hidden."""
|
||||
slc_enabled = _is_slc_enabled()
|
||||
params = ui_state.ui_params
|
||||
if not (slc_enabled or params.get_bool("ShowSpeedLimits")):
|
||||
_pulse.clear()
|
||||
return None
|
||||
|
||||
sm = ui_state.sm
|
||||
if sm.recv_frame["starpilotPlan"] < ui_state.started_frame:
|
||||
_pulse.clear()
|
||||
@@ -99,18 +97,11 @@ def _get_slc_state():
|
||||
|
||||
plan = sm["starpilotPlan"]
|
||||
speed_limit_changed = plan.speedLimitChanged
|
||||
presented_source = getattr(plan, 'slcPresentedSpeedLimitSource', '')
|
||||
|
||||
params = ui_state.ui_params
|
||||
show_slc = params.get_bool("ShowSpeedLimits")
|
||||
unconfirmed_valid = plan.unconfirmedSlcSpeedLimit > 1
|
||||
|
||||
if not show_slc:
|
||||
_pulse.clear()
|
||||
return None
|
||||
|
||||
speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
show_offset = params.get_bool("ShowSLCOffset")
|
||||
|
||||
dashboard_sl = sm["starpilotCarState"].dashboardSpeedLimit if sm.valid.get("starpilotCarState", False) else 0.0
|
||||
vision_enabled = params.get_bool("VisionSpeedLimitDetection")
|
||||
vision_sl = ui_state.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0.0
|
||||
@@ -120,41 +111,24 @@ def _get_slc_state():
|
||||
params.get("MapboxSecretKey", encoding="utf-8")
|
||||
)
|
||||
|
||||
slc_overridden_speed = plan.slcOverriddenSpeed
|
||||
# Keep the source limit visible when overridden.
|
||||
speed_limit = plan.slcSpeedLimit
|
||||
|
||||
# Resolved limit in m/s (pre-conversion, pre-offset) — feeds the vision pulse
|
||||
# change detector so the comparison is unit-stable across km/h ↔ mph flips.
|
||||
resolved_ms = speed_limit
|
||||
|
||||
# Add the per-limit offset to the displayed value only when NOT overridden
|
||||
# AND ShowSLCOffset is off (when the offset toggle is on, it's rendered as
|
||||
# a separate field below the speed number instead).
|
||||
if slc_overridden_speed == 0 and not show_offset:
|
||||
speed_limit += plan.slcSpeedLimitOffset
|
||||
speed_limit *= speed_conversion
|
||||
|
||||
speed_limit_offset = plan.slcSpeedLimitOffset * speed_conversion
|
||||
offset_str = f"{'+' if speed_limit_offset > 0 else '-'}{abs(int(round(speed_limit_offset)))}" if speed_limit_offset != 0 else "\u2013"
|
||||
|
||||
# Update the vision-source pulse once per frame, after resolved_ms is known
|
||||
# and before any sign colors are computed downstream.
|
||||
_tick_pulse(plan.slcSpeedLimitSource, resolved_ms)
|
||||
|
||||
# The pulse uses the accepted raw limit, so unit changes cannot retrigger it.
|
||||
_tick_pulse(plan.slcSpeedLimitSource, plan.slcSpeedLimit)
|
||||
return {
|
||||
'speed_limit': speed_limit,
|
||||
'speed_limit_str': "\u2013" if speed_limit <= 1 else str(int(round(speed_limit))),
|
||||
'slc_overridden_speed': slc_overridden_speed,
|
||||
'accepted_speed_limit_ms': plan.slcSpeedLimit,
|
||||
# Match the control target's non-negative base before cluster compensation.
|
||||
'effective_target_ms': max(0.0, plan.slcSpeedLimit + plan.slcSpeedLimitOffset),
|
||||
'offset_ms': plan.slcSpeedLimitOffset,
|
||||
'slc_overridden_speed': plan.slcOverriddenSpeed,
|
||||
'speed_limit_source': plan.slcSpeedLimitSource,
|
||||
# Older publishers/replays decode the new Text field as "", rather than omitting the attribute.
|
||||
'presented_source': presented_source or plan.slcSpeedLimitSource,
|
||||
'slc_enabled': slc_enabled,
|
||||
# Both UI fields were added together; older plans have no published limiting state.
|
||||
'slc_is_limiting_max_set': bool(getattr(plan, 'slcIsLimitingMaxSet', False)) if presented_source else None,
|
||||
'unconfirmed_speed_limit': max(0.0, plan.unconfirmedSlcSpeedLimit * speed_conversion),
|
||||
'unconfirmed_valid': unconfirmed_valid,
|
||||
'speed_limit_changed': speed_limit_changed,
|
||||
'show_offset': show_offset,
|
||||
'use_vienna': params.get_bool("UseVienna"),
|
||||
'offset_str': offset_str,
|
||||
'speed_conversion': speed_conversion,
|
||||
'speed_unit': " km/h" if ui_state.is_metric else " mph",
|
||||
'slc_abbreviated_sources': params.get_bool("SLCAbbreviatedSources"),
|
||||
'slc_active_sources_only': params.get_bool("SLCActiveSourcesOnly"),
|
||||
'slc_enabled_sources': enabled_source_titles(
|
||||
@@ -191,203 +165,6 @@ def _get_semi_bold():
|
||||
return _font_semi_bold
|
||||
|
||||
|
||||
_ACTIVE_SOURCE_LABELS = {title: abbrev.upper() for title, abbrev, *_ in SOURCE_DEFS}
|
||||
|
||||
|
||||
def _active_source_label(state: dict) -> str:
|
||||
source = state.get("speed_limit_source")
|
||||
if not source or source == "None":
|
||||
return tr("LIMIT")
|
||||
return _ACTIVE_SOURCE_LABELS.get(source, source.upper())
|
||||
|
||||
|
||||
def _source_label_color(alpha: int, is_overridden: bool = False) -> rl.Color:
|
||||
"""Match Set Speed's MAX label color."""
|
||||
if is_overridden or ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE):
|
||||
base = COLORS.DISENGAGED
|
||||
elif ui_state.status == UIStatus.ENGAGED:
|
||||
base = COLORS.ENGAGED
|
||||
else:
|
||||
base = COLORS.GREY
|
||||
return _speed_limit_pulse_color(base, alpha)
|
||||
|
||||
|
||||
# ── US MUTCD Sign ─────────────────────────────────────────────────────
|
||||
|
||||
def _draw_offset_chip(rect: rl.Rectangle, offset_str: str, color: rl.Color) -> None:
|
||||
"""Draw the optional SLC offset as a compact accent chip."""
|
||||
font = _get_semi_bold()
|
||||
text_size = measure_text_cached(font, offset_str, FONT_OFFSET)
|
||||
chip_w = max(64.0, text_size.x + 24.0)
|
||||
chip_h = 36.0
|
||||
chip_rect = rl.Rectangle(
|
||||
rect.x + (rect.width - chip_w) / 2,
|
||||
rect.y + rect.height - chip_h - 10,
|
||||
chip_w,
|
||||
chip_h,
|
||||
)
|
||||
chip_fill = rl.Color(0, 0, 0, min(120, color.a))
|
||||
roundness = roundness_for(chip_rect, 18)
|
||||
rl.draw_rectangle_rounded(chip_rect, roundness, OFFSET_CHIP_SEGMENTS, chip_fill)
|
||||
rl.draw_rectangle_rounded_lines_ex(chip_rect, roundness, OFFSET_CHIP_SEGMENTS, 2, color)
|
||||
rl.draw_text_ex(
|
||||
font,
|
||||
offset_str,
|
||||
rl.Vector2(chip_rect.x + (chip_w - text_size.x) / 2, chip_rect.y + (chip_h - text_size.y) / 2),
|
||||
FONT_OFFSET,
|
||||
0,
|
||||
color,
|
||||
)
|
||||
|
||||
|
||||
def _draw_us_sign(x: float, y: float, sign_width: float, sign_height: float,
|
||||
speed_text: str, offset_str: str,
|
||||
source_label: str, alpha: int, show_offset: bool, *,
|
||||
pending: bool = False, is_overridden: bool = False):
|
||||
"""Draw the NA control card at (x, y).
|
||||
|
||||
The card keeps the SLC's label/value hierarchy while sharing the exact
|
||||
visible frame geometry with Set Speed. Border and text colors continue to
|
||||
use the existing Vision pulse and pending blink behavior.
|
||||
"""
|
||||
# Pending: blink white/red. Active: shared blue-grey.
|
||||
if pending:
|
||||
blink_on = int(rl.get_time() * 1000) % 1000 < PENDING_BLINK_MS
|
||||
base_border = rl.Color(255, 255, 255, alpha) if blink_on else rl.Color(201, 34, 49, alpha)
|
||||
else:
|
||||
base_border = rl.Color(CONTROL_BORDER.r, CONTROL_BORDER.g, CONTROL_BORDER.b,
|
||||
min(alpha, CONTROL_BORDER.a))
|
||||
|
||||
# Compose the blink base with the active vision pulse (no-op outside window).
|
||||
border_color = _speed_limit_pulse_color(base_border, base_border.a)
|
||||
# White value text reads on the translucent road background.
|
||||
text_color = _speed_limit_pulse_color(rl.Color(255, 255, 255, 255), alpha)
|
||||
|
||||
card_rect = rl.Rectangle(x, y, sign_width, sign_height)
|
||||
card_fill = rl.Color(CONTROL_BG.r, CONTROL_BG.g, CONTROL_BG.b, min(CONTROL_BG.a, alpha))
|
||||
draw_control_card(card_rect, fill=card_fill, border=border_color,
|
||||
border_width=CONTROL_BORDER_WIDTH)
|
||||
|
||||
font_bold = _get_bold()
|
||||
font_semi = _get_semi_bold()
|
||||
cx = x + sign_width / 2
|
||||
|
||||
# Pending layout: "PENDING" + "LIMIT" + speed (no offset shown when pending).
|
||||
if pending:
|
||||
pending_size = measure_text_cached(font_semi, tr("PENDING"), FONT_LABEL - 2)
|
||||
rl.draw_text_ex(font_semi, tr("PENDING"), rl.Vector2(cx - pending_size.x / 2, y + 20), FONT_LABEL - 2, 0, text_color)
|
||||
limit_size = measure_text_cached(font_semi, tr("LIMIT"), FONT_LABEL)
|
||||
rl.draw_text_ex(font_semi, tr("LIMIT"), rl.Vector2(cx - limit_size.x / 2, y + 48), FONT_LABEL, 0, text_color)
|
||||
speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED - 6)
|
||||
rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 85), FONT_SPEED - 6, 0, text_color)
|
||||
elif show_offset:
|
||||
# Offset ON: source at the top, speed below it, and the offset in a chip.
|
||||
source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE)
|
||||
source_color = _source_label_color(alpha, is_overridden=is_overridden)
|
||||
rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 8), FONT_SOURCE, 0, source_color)
|
||||
|
||||
speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED)
|
||||
rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 44), FONT_SPEED, 0, text_color)
|
||||
_draw_offset_chip(card_rect, offset_str, text_color)
|
||||
else:
|
||||
# Offset OFF: match Set Speed typography.
|
||||
source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE)
|
||||
source_color = _source_label_color(alpha, is_overridden=is_overridden)
|
||||
rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 27), FONT_SOURCE, 0, source_color)
|
||||
|
||||
speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED)
|
||||
rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 77), FONT_SPEED, 0, text_color)
|
||||
|
||||
|
||||
# ── EU Vienna Sign ────────────────────────────────────────────────────
|
||||
|
||||
def _draw_eu_sign(x: float, y: float, speed_text: str, offset_str: str,
|
||||
source_label: str, text_alpha: int, show_offset: bool, *, pending: bool = False):
|
||||
"""Draw EU-style (Vienna) speed limit sign at (x, y).
|
||||
|
||||
White disk with a pulsable red ring and pulsable black text. The pre-existing
|
||||
pending-text blink (black <-> red) composes with the vision pulse: outside the
|
||||
pulse window the blink is unchanged, inside it both colors are eased toward
|
||||
VISION_SPEED_LIMIT_PULSE_COLOR.
|
||||
"""
|
||||
center_x = x + EU_SIGN_SIZE / 2
|
||||
center_y = y + EU_SIGN_SIZE / 2
|
||||
radius = EU_SIGN_SIZE / 2
|
||||
|
||||
# White disk fill.
|
||||
rl.draw_circle(int(center_x), int(center_y), radius, rl.Color(255, 255, 255, text_alpha))
|
||||
# Red ring; eased toward VISION_SPEED_LIMIT_PULSE_COLOR when a Vision-sourced
|
||||
# limit just changed.
|
||||
ring_color = _speed_limit_pulse_color(rl.Color(201, 34, 49, 255), text_alpha)
|
||||
rl.draw_ring(rl.Vector2(center_x, center_y), radius - RED_RING_WIDTH, radius,
|
||||
0, 360, 64, ring_color)
|
||||
|
||||
font_bold = _get_bold()
|
||||
|
||||
eu_font = FONT_EU_LARGE if len(speed_text) <= 2 else FONT_EU_SMALL
|
||||
|
||||
# EU pending: text blinks black/red, composed with the vision pulse.
|
||||
if pending:
|
||||
blink_on = int(rl.get_time() * 1000) % 1000 < PENDING_BLINK_MS
|
||||
base_text = rl.Color(0, 0, 0, 255) if blink_on else rl.Color(201, 34, 49, 255)
|
||||
else:
|
||||
base_text = rl.Color(0, 0, 0, 255)
|
||||
text_color = _speed_limit_pulse_color(base_text, text_alpha)
|
||||
|
||||
# Pending: text centered (no offset display)
|
||||
if pending:
|
||||
speed_size = measure_text_cached(font_bold, speed_text, eu_font)
|
||||
speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2)
|
||||
rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color)
|
||||
elif not show_offset:
|
||||
font_semi = _get_semi_bold()
|
||||
source_size = measure_text_cached(font_semi, source_label, FONT_LABEL - 4)
|
||||
source_pos = rl.Vector2(center_x - source_size.x / 2, y + 16)
|
||||
rl.draw_text_ex(font_semi, source_label, source_pos, FONT_LABEL - 4, 0, text_color)
|
||||
|
||||
speed_size = measure_text_cached(font_bold, speed_text, eu_font)
|
||||
speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2)
|
||||
rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color)
|
||||
else:
|
||||
# Offset ON: source at the top, speed below it, offset at the bottom.
|
||||
font_semi = _get_semi_bold()
|
||||
source_size = measure_text_cached(font_semi, source_label, FONT_LABEL - 4)
|
||||
source_pos = rl.Vector2(center_x - source_size.x / 2, y + 16)
|
||||
rl.draw_text_ex(font_semi, source_label, source_pos, FONT_LABEL - 4, 0, text_color)
|
||||
|
||||
speed_size = measure_text_cached(font_bold, speed_text, eu_font)
|
||||
speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2 - 5)
|
||||
rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color)
|
||||
|
||||
offset_size = measure_text_cached(font_semi, offset_str, FONT_EU_OFFSET)
|
||||
offset_pos = rl.Vector2(center_x - offset_size.x / 2, y + 122)
|
||||
rl.draw_text_ex(font_semi, offset_str, offset_pos, FONT_EU_OFFSET, 0, text_color)
|
||||
|
||||
|
||||
# ── Dispatcher (pending and active sign share the same rect) ─────────
|
||||
|
||||
def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False):
|
||||
"""Draw either the pending or active sign in the given rect."""
|
||||
if pending:
|
||||
# Pending shows the unconfirmed value, full opacity
|
||||
speed_text = ("\u2013" if state['unconfirmed_speed_limit'] <= 1
|
||||
else str(int(round(state['unconfirmed_speed_limit']))))
|
||||
else:
|
||||
speed_text = state['speed_limit_str']
|
||||
|
||||
text_alpha = 255
|
||||
is_overridden = not pending and state['slc_overridden_speed'] != 0
|
||||
source_label = _active_source_label(state)
|
||||
|
||||
if state['use_vienna']:
|
||||
_draw_eu_sign(rect.x, rect.y, speed_text, state['offset_str'], source_label, text_alpha,
|
||||
state['show_offset'], pending=pending)
|
||||
else:
|
||||
_draw_us_sign(rect.x, rect.y, rect.width, rect.height, speed_text, state['offset_str'],
|
||||
source_label, text_alpha, state['show_offset'], pending=pending,
|
||||
is_overridden=is_overridden)
|
||||
|
||||
|
||||
# ── Sources Bubble (expandable overlay) ────────────────────────────────
|
||||
|
||||
# Fixed outer footprint; the content scale adapts to the visible row count.
|
||||
@@ -417,7 +194,7 @@ _SOURCE_COMPACT_LABELS = {
|
||||
|
||||
|
||||
def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.Color) -> None:
|
||||
"""Draw the small, intentionally simple source glyphs used by the panel."""
|
||||
"""Draw the existing source glyph for both the header and diagnostics."""
|
||||
cx = x + size / 2
|
||||
cy = y + size / 2
|
||||
stroke = max(2.5, size / 12.0)
|
||||
@@ -480,11 +257,20 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
|
||||
color,
|
||||
)
|
||||
rl.draw_circle_v(pin_center, size * 0.09, _SOURCE_PANEL_BG)
|
||||
else: # Dashboard / fallback
|
||||
dashboard_scale = 1.22
|
||||
elif icon_key == "dashboard":
|
||||
# The Dashboard speed-limit source is a vehicle glyph, distinct from Max Set's gauge.
|
||||
body = rl.Rectangle(x + size * 0.10, y + size * 0.43, size * 0.80, size * 0.29)
|
||||
rl.draw_rectangle_rounded_lines_ex(body, 0.30, 8, stroke, color)
|
||||
rl.draw_line_ex(rl.Vector2(x + size * 0.25, body.y), rl.Vector2(x + size * 0.36, y + size * 0.27), stroke, color)
|
||||
rl.draw_line_ex(rl.Vector2(x + size * 0.36, y + size * 0.27), rl.Vector2(x + size * 0.68, y + size * 0.27), stroke, color)
|
||||
rl.draw_line_ex(rl.Vector2(x + size * 0.68, y + size * 0.27), rl.Vector2(x + size * 0.79, body.y), stroke, color)
|
||||
for wheel_x in (x + size * 0.27, x + size * 0.73):
|
||||
rl.draw_circle_v(rl.Vector2(wheel_x, y + size * 0.75), size * 0.07, color)
|
||||
elif icon_key == "speedometer":
|
||||
gauge_scale = 1.22
|
||||
pivot = rl.Vector2(cx, cy + size * 0.17)
|
||||
inner_radius = size * 0.27 * dashboard_scale
|
||||
outer_radius = size * 0.34 * dashboard_scale
|
||||
inner_radius = size * 0.27 * gauge_scale
|
||||
outer_radius = size * 0.34 * gauge_scale
|
||||
ring_segments = max(24, int(size * 0.25))
|
||||
rl.draw_ring(pivot, inner_radius, outer_radius, 190, 350, ring_segments, color)
|
||||
cap_radius = (outer_radius - inner_radius) / 2
|
||||
@@ -509,7 +295,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
|
||||
stroke,
|
||||
color,
|
||||
)
|
||||
rl.draw_circle_v(pivot, max(2.0, size * 0.06 * dashboard_scale), color)
|
||||
rl.draw_circle_v(pivot, max(2.0, size * 0.06 * gauge_scale), color)
|
||||
|
||||
|
||||
def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None:
|
||||
@@ -523,7 +309,7 @@ def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None:
|
||||
total_h = sum(sz.y for sz in line_sizes) + line_gap * (len(lines) - 1)
|
||||
curr_y = round(panel_rect.y + (panel_rect.height - total_h) / 2)
|
||||
|
||||
for line, sz in zip(lines, line_sizes):
|
||||
for line, sz in zip(lines, line_sizes, strict=True):
|
||||
pos_x = round(panel_rect.x + (panel_rect.width - sz.x) / 2)
|
||||
rl.draw_text_ex(font, line, rl.Vector2(pos_x, curr_y), font_size, 0, _WHITE)
|
||||
curr_y += round(sz.y + line_gap)
|
||||
@@ -608,7 +394,7 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
|
||||
f"{tr(compact_label)}-{source_abbreviated_value_text(value)}",
|
||||
"",
|
||||
content_right - label_left,
|
||||
lambda text: measure_text_cached(text_font, text, font_size).x,
|
||||
lambda text, font=text_font: measure_text_cached(font, text, font_size).x,
|
||||
)
|
||||
label_size = measure_text_cached(text_font, label_text, font_size)
|
||||
text_y = round(row_y + (row_h - label_size.y) / 2)
|
||||
@@ -647,24 +433,3 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
|
||||
value_pos = rl.Vector2(round(content_right - value_size.x), text_y)
|
||||
rl.draw_text_ex(font_semi, label_text, label_pos, font_size, 0, text_color)
|
||||
rl.draw_text_ex(font_bold, value_text, value_pos, font_size, 0, text_color)
|
||||
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────
|
||||
|
||||
def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = False) -> Optional[rl.Rectangle]:
|
||||
"""Render the SLC sign and optional source bubble at a layout rect."""
|
||||
flashing_pending = state['speed_limit_changed'] and state['unconfirmed_valid']
|
||||
|
||||
if flashing_pending:
|
||||
_draw_sign(state, rect, pending=True)
|
||||
return None
|
||||
|
||||
_draw_sign(state, rect, pending=False)
|
||||
|
||||
use_vienna = state['use_vienna']
|
||||
visual_rect = rl.Rectangle(rect.x, rect.y, EU_SIGN_SIZE, EU_SIGN_SIZE) if use_vienna else rect
|
||||
|
||||
if expanded:
|
||||
_draw_sources_bubble(state, visual_rect)
|
||||
|
||||
return visual_rect
|
||||
|
||||
@@ -8,7 +8,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager import WidgetLayoutManager
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets import (
|
||||
SetSpeedWidget, SpeedLimitWidget, PedalIconsWidget,
|
||||
UnifiedSpeedWidget, PedalIconsWidget,
|
||||
AetherGaugeWidget, PersonalityButtonWidget, DriverMonitorWidget,
|
||||
SteeringWheelWidget, StoppedTimerWidget, ModelSourceWidget
|
||||
)
|
||||
@@ -25,7 +25,6 @@ from openpilot.starpilot.common.favorite_slots import (
|
||||
build_favorite_slot_options,
|
||||
filter_favorite_slot_options,
|
||||
favorite_key_is_valid,
|
||||
is_bool_param,
|
||||
)
|
||||
|
||||
from openpilot.system.ui.lib.application import MousePos, gui_app, FontWeight
|
||||
@@ -64,8 +63,7 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
self._hud_renderer.draw_exp_button = False
|
||||
|
||||
# Initialize layout widgets
|
||||
self._set_speed_widget = SetSpeedWidget(self._hud_renderer)
|
||||
self._speed_limit_widget = SpeedLimitWidget()
|
||||
self._unified_speed_widget = UnifiedSpeedWidget(self._hud_renderer)
|
||||
self._aethergauge_widget = AetherGaugeWidget(self._hud_renderer)
|
||||
self._steering_wheel_widget = SteeringWheelWidget(self._hud_renderer._exp_button)
|
||||
self._pedals_widget = PedalIconsWidget()
|
||||
@@ -75,8 +73,7 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
self._stopped_timer_widget = StoppedTimerWidget(self.is_in_reverse)
|
||||
|
||||
# Register to layout zones
|
||||
self.layout_manager.register_widget("left", self._set_speed_widget)
|
||||
self.layout_manager.register_widget("left", self._speed_limit_widget)
|
||||
self.layout_manager.register_widget("left", self._unified_speed_widget)
|
||||
self.layout_manager.register_widget("left", self._aethergauge_widget)
|
||||
self.layout_manager.register_widget("right", self._steering_wheel_widget)
|
||||
self.layout_manager.register_widget("right", self._pedals_widget)
|
||||
@@ -85,8 +82,7 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
self.layout_manager.register_widget("bottom", self._driver_monitor_widget)
|
||||
|
||||
# Register as child widgets for click propagation
|
||||
self._child(self._set_speed_widget)
|
||||
self._child(self._speed_limit_widget)
|
||||
self._child(self._unified_speed_widget)
|
||||
self._child(self._aethergauge_widget)
|
||||
self._child(self._steering_wheel_widget)
|
||||
self._child(self._pedals_widget)
|
||||
@@ -133,7 +129,7 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
if self._draw_hud_controls:
|
||||
dm = self.driver_state_renderer
|
||||
self.layout_manager.update_layout(self._content_rect, is_rhd=dm.is_rhd if dm else False)
|
||||
self._render_slc()
|
||||
self._render_speed_card()
|
||||
self._render_overlays()
|
||||
self._render_road_name()
|
||||
|
||||
@@ -167,13 +163,11 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
render_background_effects(rect, border_width)
|
||||
render_overlay(border_rect, border_width)
|
||||
|
||||
def _render_slc(self):
|
||||
def _render_speed_card(self):
|
||||
if self._full_alert_showing():
|
||||
return
|
||||
if self._speed_limit_widget.is_visible:
|
||||
self._speed_limit_widget.render(self._speed_limit_widget.rect)
|
||||
if self._set_speed_widget.is_visible:
|
||||
self._set_speed_widget.render(self._set_speed_widget.rect)
|
||||
if self._unified_speed_widget.is_visible:
|
||||
self._unified_speed_widget.render(self._unified_speed_widget.rect)
|
||||
|
||||
def _render_overlays(self):
|
||||
alert_showing, _ = self.alert_renderer.will_render()
|
||||
@@ -186,7 +180,7 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
|
||||
self._render_developer_metrics()
|
||||
|
||||
self.layout_manager.render_widgets(exclude={"speed_limit", "set_speed"})
|
||||
self.layout_manager.render_widgets(exclude={"unified_speed"})
|
||||
|
||||
self._render_torque_bar()
|
||||
self._render_bottom_row_widgets()
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
"""Displayed Max Set and posted-limit values for the Big UI speed card."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UnifiedSpeedPresentation:
|
||||
mode: str
|
||||
max_speed_text: str
|
||||
posted_speed_text: str
|
||||
effective_speed_text: str
|
||||
offset_text: str | None
|
||||
unit_text: str
|
||||
source: str
|
||||
confirmation_pending: bool
|
||||
active_side: str
|
||||
|
||||
|
||||
def resolve_unified_speed(show_max: bool, cruise_set: bool, max_speed: float,
|
||||
slc_state: dict | None, slc_enabled: bool, is_metric: bool) -> UnifiedSpeedPresentation:
|
||||
"""Compare the rounded values the driver sees; ignore override speed for layout."""
|
||||
unit = "km/h" if is_metric else "mph"
|
||||
max_text = str(round(max_speed)) if cruise_set else "–"
|
||||
posted_text = effective_text = "–"
|
||||
offset_text = None
|
||||
source = "None"
|
||||
pending = has_limit = slc_is_limiting = False
|
||||
if slc_state is not None:
|
||||
conversion = slc_state['speed_conversion']
|
||||
accepted = slc_state['accepted_speed_limit_ms']
|
||||
pending = bool(slc_state['speed_limit_changed'] and slc_state['unconfirmed_valid'])
|
||||
source = slc_state['presented_source']
|
||||
has_limit = (source not in ("", "None") and accepted > 1) or pending
|
||||
if has_limit:
|
||||
posted_text = str(round(slc_state['unconfirmed_speed_limit'])) if pending else str(round(accepted * conversion))
|
||||
effective = slc_state['effective_target_ms']
|
||||
slc_is_limiting = slc_state['slc_is_limiting_max_set']
|
||||
if slc_is_limiting is None:
|
||||
slc_is_limiting = cruise_set and accepted > 1 and 0 < effective * conversion < max_speed
|
||||
effective_text = str(round(effective * conversion)) if effective > 0 else "–"
|
||||
offset_display = round(slc_state['offset_ms'] * conversion)
|
||||
offset_text = f"{offset_display:+d}" if offset_display else None
|
||||
else:
|
||||
source = "None"
|
||||
|
||||
# Max-only is valid only when SLC is disabled.
|
||||
if pending:
|
||||
mode = "split"
|
||||
elif slc_enabled:
|
||||
mode = "merged" if show_max and cruise_set and has_limit and max_text == effective_text else "split" if show_max else "limit_only"
|
||||
elif has_limit:
|
||||
mode = "split" if show_max else "limit_only"
|
||||
else:
|
||||
mode = "max_only"
|
||||
|
||||
active_side = "none" if slc_state is not None and slc_state['slc_overridden_speed'] else "shared" if mode == "merged" else (
|
||||
"slc" if slc_enabled and cruise_set and slc_is_limiting else
|
||||
"max" if (show_max or pending) and cruise_set else "none"
|
||||
)
|
||||
return UnifiedSpeedPresentation(mode, max_text, posted_text, effective_text, offset_text, unit, source, pending, active_side)
|
||||
@@ -30,12 +30,12 @@ class WidgetLayoutManager:
|
||||
active_widgets = [w for w in self.zones["left"] if w.is_visible]
|
||||
|
||||
# Left zone stacks vertically from the top-left offset
|
||||
# X anchor is the shared left-control center (content x + 146).
|
||||
center_x = self.content_rect.x + WIDGET_ANCHOR_OFFSET
|
||||
# Keep wide cards inside the content rect without moving compact widgets.
|
||||
current_y = self.content_rect.y + 45
|
||||
|
||||
for widget in active_widgets:
|
||||
w, h = widget.get_size()
|
||||
center_x = self.content_rect.x + max(float(WIDGET_ANCHOR_OFFSET), w / 2 + 30)
|
||||
widget.set_rect(rl.Rectangle(center_x - w / 2, current_y, w, h))
|
||||
current_y += h + self.spacing
|
||||
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.set_speed import SetSpeedWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.speed_limit import SpeedLimitWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.unified_speed import UnifiedSpeedWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.pedal_icons import PedalIconsWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.aethergauge import AetherGaugeWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.personality_button import PersonalityButtonWidget
|
||||
@@ -11,8 +10,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets.model_source import ModelSo
|
||||
|
||||
__all__ = [
|
||||
"LayoutWidget",
|
||||
"SetSpeedWidget",
|
||||
"SpeedLimitWidget",
|
||||
"UnifiedSpeedWidget",
|
||||
"PedalIconsWidget",
|
||||
"AetherGaugeWidget",
|
||||
"PersonalityButtonWidget",
|
||||
|
||||
@@ -1,72 +0,0 @@
|
||||
import pyray as rl
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import (
|
||||
UI_CONFIG, FONT_SIZES, COLORS, CRUISE_DISABLED_CHAR
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import draw_control_card
|
||||
|
||||
class SetSpeedWidget(LayoutWidget):
|
||||
def __init__(self, hud_renderer):
|
||||
super().__init__("set_speed", priority=1)
|
||||
self.hud_renderer = hud_renderer
|
||||
self._font_semi_bold = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
self._font_bold = gui_app.font(FontWeight.BOLD)
|
||||
|
||||
@property
|
||||
def is_visible(self) -> bool:
|
||||
return (
|
||||
self.hud_renderer.is_cruise_available
|
||||
and not ui_state.starpilot_toggles.get("hide_max_speed", False)
|
||||
)
|
||||
|
||||
def get_size(self) -> tuple[float, float]:
|
||||
set_speed_width = (
|
||||
UI_CONFIG.set_speed_width_metric
|
||||
if ui_state.is_metric
|
||||
else UI_CONFIG.set_speed_width_imperial
|
||||
)
|
||||
return float(set_speed_width), float(UI_CONFIG.set_speed_height)
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
draw_control_card(rect)
|
||||
|
||||
max_color = COLORS.GREY
|
||||
set_speed_color = COLORS.DARK_GREY
|
||||
if self.hud_renderer.is_cruise_set:
|
||||
set_speed_color = COLORS.WHITE
|
||||
if ui_state.status == UIStatus.ENGAGED:
|
||||
max_color = COLORS.ENGAGED
|
||||
elif ui_state.status == UIStatus.DISENGAGED:
|
||||
max_color = COLORS.DISENGAGED
|
||||
elif ui_state.status == UIStatus.OVERRIDE:
|
||||
max_color = COLORS.OVERRIDE
|
||||
|
||||
max_text = tr("MAX")
|
||||
max_text_width = measure_text_cached(self._font_semi_bold, max_text, FONT_SIZES.max_speed).x
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
max_text,
|
||||
rl.Vector2(rect.x + (rect.width - max_text_width) / 2, rect.y + 27),
|
||||
FONT_SIZES.max_speed,
|
||||
0,
|
||||
max_color,
|
||||
)
|
||||
|
||||
set_speed_text = (
|
||||
CRUISE_DISABLED_CHAR
|
||||
if not self.hud_renderer.is_cruise_set
|
||||
else str(round(self.hud_renderer.set_speed))
|
||||
)
|
||||
speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x
|
||||
rl.draw_text_ex(
|
||||
self._font_bold,
|
||||
set_speed_text,
|
||||
rl.Vector2(rect.x + (rect.width - speed_text_width) / 2, rect.y + 77),
|
||||
FONT_SIZES.set_speed,
|
||||
0,
|
||||
set_speed_color,
|
||||
)
|
||||
@@ -1,67 +0,0 @@
|
||||
import pyray as rl
|
||||
from typing import Optional
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import (
|
||||
_get_slc_state, render_speed_limit_at, EU_SIGN_SIZE,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH, SLC_HEIGHT
|
||||
|
||||
|
||||
class SpeedLimitWidget(LayoutWidget):
|
||||
TOUCH_SLOP = 20
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("speed_limit", priority=2)
|
||||
self._slc_state: dict | None = None
|
||||
self._sign_rect: Optional[rl.Rectangle] = None
|
||||
|
||||
@property
|
||||
def _hit_rect(self) -> rl.Rectangle:
|
||||
rect = self._sign_rect or self.rect
|
||||
slop = self.TOUCH_SLOP
|
||||
return rl.Rectangle(
|
||||
rect.x - slop,
|
||||
rect.y - slop,
|
||||
rect.width + 2 * slop,
|
||||
rect.height + 2 * slop,
|
||||
)
|
||||
|
||||
@property
|
||||
def is_visible(self) -> bool:
|
||||
self._slc_state = _get_slc_state()
|
||||
if self._slc_state is None:
|
||||
self._sign_rect = None
|
||||
return False
|
||||
return True
|
||||
|
||||
def get_size(self) -> tuple[float, float]:
|
||||
if self._slc_state is None:
|
||||
return 0.0, 0.0
|
||||
|
||||
use_vienna = self._slc_state['use_vienna']
|
||||
w = float(EU_SIGN_SIZE if use_vienna else CONTROL_WIDTH)
|
||||
h = float(EU_SIGN_SIZE if use_vienna else SLC_HEIGHT)
|
||||
|
||||
return w, h
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
if self._slc_state is None:
|
||||
return
|
||||
params = ui_state.ui_params
|
||||
expanded = params.get_bool("SpeedLimitSources")
|
||||
self._sign_rect = render_speed_limit_at(self._slc_state, rect, expanded)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos) -> None:
|
||||
state = self._slc_state
|
||||
if state is None or not rl.check_collision_point_rec(mouse_pos, self._hit_rect):
|
||||
return
|
||||
|
||||
if state['speed_limit_changed'] and state['unconfirmed_valid']:
|
||||
Params(memory=True).put_bool("SpeedLimitAccepted", True)
|
||||
return
|
||||
|
||||
params = ui_state.ui_params
|
||||
current = params.get_bool("SpeedLimitSources")
|
||||
params.put_bool("SpeedLimitSources", not current)
|
||||
@@ -0,0 +1,324 @@
|
||||
"""One Big UI card for Max Set and the accepted speed limit."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pyray as rl
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import (
|
||||
_draw_source_icon, _draw_sources_bubble, _get_slc_state, _is_slc_enabled, _speed_limit_pulse_color, source_icon_key,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import (
|
||||
UnifiedSpeedPresentation, resolve_unified_speed,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
|
||||
CONTROL_BG, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, draw_control_card, roundness_for,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
|
||||
|
||||
UNIFIED_WIDTH = 520
|
||||
UNIFIED_HEIGHT = 250
|
||||
SINGLE_WIDTH = 250
|
||||
MERGED_SEPARATOR_Y = 76
|
||||
HEADER_ICON_SIZE = 34
|
||||
HEADER_FONT_SIZE = 28
|
||||
VALUE_FONT_SIZE = 96
|
||||
UNIT_FONT_SIZE = 28
|
||||
PAUSE_ICON_WIDTH = 12
|
||||
PAUSE_ICON_HEIGHT = 14
|
||||
PAUSE_ICON_GAP = 8
|
||||
OFFSET_FONT_SIZE = 22
|
||||
OFFSET_PILL_HEIGHT = 30
|
||||
CONFIRMATION_COLOR = rl.Color(188, 132, 255, 255)
|
||||
UNIFIED_ACCENT = rl.Color(160, 96, 230, 230)
|
||||
OFFSET_COLOR = rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 255)
|
||||
|
||||
|
||||
def _draw_header_icon(icon_key: str, x: float, y: float) -> None:
|
||||
scale = max(1.0, gui_app._scale * max(gui_app._pixel_scale_x, gui_app._pixel_scale_y))
|
||||
# Supersample for smooth edges.
|
||||
texture_size = math.ceil(2 * HEADER_ICON_SIZE * scale)
|
||||
|
||||
def render() -> None:
|
||||
rl.rl_push_matrix()
|
||||
try:
|
||||
texture_scale = texture_size / HEADER_ICON_SIZE
|
||||
rl.rl_scalef(texture_scale, texture_scale, 1.0)
|
||||
_draw_source_icon(icon_key, 0, 0, HEADER_ICON_SIZE, rl.WHITE)
|
||||
finally:
|
||||
rl.rl_pop_matrix()
|
||||
|
||||
texture = gui_app.cached_render_texture(
|
||||
f"unified-speed-header:{icon_key}:{texture_size}", texture_size, texture_size, render,
|
||||
)
|
||||
if texture is None:
|
||||
_draw_source_icon(icon_key, x, y, HEADER_ICON_SIZE, rl.WHITE)
|
||||
return
|
||||
|
||||
rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY)
|
||||
try:
|
||||
rl.draw_texture_pro(
|
||||
texture, rl.Rectangle(0, 0, texture_size, -texture_size),
|
||||
rl.Rectangle(x, y, HEADER_ICON_SIZE, HEADER_ICON_SIZE), rl.Vector2(0, 0), 0.0, rl.WHITE,
|
||||
)
|
||||
finally:
|
||||
rl.end_blend_mode()
|
||||
|
||||
|
||||
class UnifiedSpeedWidget(LayoutWidget):
|
||||
TOUCH_SLOP = 20
|
||||
|
||||
def __init__(self, hud_renderer):
|
||||
super().__init__("unified_speed", priority=1)
|
||||
self.hud_renderer = hud_renderer
|
||||
self._font_semi_bold = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
self._font_bold = gui_app.font(FontWeight.BOLD)
|
||||
self._slc_state: dict | None = None
|
||||
self._slc_enabled = False
|
||||
self._presentation: UnifiedSpeedPresentation | None = None
|
||||
self._show_max = False
|
||||
self._pedal_override = False
|
||||
self._snapshot_frame: int | None = None
|
||||
|
||||
def _refresh_snapshot(self) -> None:
|
||||
frame = getattr(ui_state.sm, "frame", None)
|
||||
if frame is not None and frame == self._snapshot_frame:
|
||||
return
|
||||
self._snapshot_frame = frame
|
||||
self._slc_enabled = _is_slc_enabled()
|
||||
self._slc_state = _get_slc_state()
|
||||
self._show_max = (
|
||||
self.hud_renderer.is_cruise_available and
|
||||
not ui_state.starpilot_toggles.get("hide_max_speed", False)
|
||||
)
|
||||
self._pedal_override = (
|
||||
self.hud_renderer.is_cruise_set and ui_state.engaged and
|
||||
ui_state.sm.valid.get("carState", False) and ui_state.sm.alive.get("carState", False) and
|
||||
ui_state.sm.recv_frame["carState"] >= ui_state.started_frame and ui_state.sm["carState"].gasPressed
|
||||
)
|
||||
self._presentation = resolve_unified_speed(
|
||||
self._show_max, self.hud_renderer.is_cruise_set, self.hud_renderer.set_speed,
|
||||
self._slc_state, self._slc_enabled, ui_state.is_metric,
|
||||
)
|
||||
|
||||
@property
|
||||
def is_visible(self) -> bool:
|
||||
self._refresh_snapshot()
|
||||
return self._show_max or self._presentation.mode != "max_only"
|
||||
|
||||
def get_size(self) -> tuple[float, float]:
|
||||
self._refresh_snapshot()
|
||||
width = UNIFIED_WIDTH if self._presentation.mode in ("split", "merged") else SINGLE_WIDTH
|
||||
return float(width), float(UNIFIED_HEIGHT)
|
||||
|
||||
@property
|
||||
def _hit_rect(self) -> rl.Rectangle:
|
||||
rect = self.rect
|
||||
return rl.Rectangle(
|
||||
rect.x, rect.y - self.TOUCH_SLOP,
|
||||
rect.width + self.TOUCH_SLOP, rect.height + 2 * self.TOUCH_SLOP,
|
||||
)
|
||||
|
||||
def _speed_limit_bounds(self, rect: rl.Rectangle) -> rl.Rectangle | None:
|
||||
mode = self._presentation.mode
|
||||
if mode in ("split", "merged"):
|
||||
return rl.Rectangle(rect.x + rect.width / 2, rect.y, rect.width / 2, rect.height)
|
||||
if mode == "limit_only":
|
||||
return rect
|
||||
return None
|
||||
|
||||
def _draw_centered_text(self, text: str, bounds: rl.Rectangle, y: float,
|
||||
font_size: int, color: rl.Color, *, bold: bool = False) -> None:
|
||||
font = self._font_bold if bold else self._font_semi_bold
|
||||
text_size = measure_text_cached(font, text, font_size)
|
||||
text_x = bounds.x + (bounds.width - text_size.x) / 2
|
||||
rl.draw_text_ex(font, text, rl.Vector2(text_x, y), font_size, 0, color)
|
||||
|
||||
def _draw_header(self, bounds: rl.Rectangle, text: str, icon_key: str | None, label_color: rl.Color) -> None:
|
||||
text = tr(text)
|
||||
font_size = HEADER_FONT_SIZE
|
||||
icon_width = HEADER_ICON_SIZE + 9 if icon_key else 0
|
||||
while font_size > 16 and measure_text_cached(self._font_semi_bold, text, font_size).x + icon_width > bounds.width - 24:
|
||||
font_size -= 1
|
||||
text_size = measure_text_cached(self._font_semi_bold, text, font_size)
|
||||
group_width = icon_width + text_size.x
|
||||
group_x = bounds.x + (bounds.width - group_width) / 2
|
||||
icon_y = bounds.y + 20
|
||||
if icon_key:
|
||||
_draw_header_icon(icon_key, group_x, icon_y)
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold, text,
|
||||
rl.Vector2(group_x + icon_width, icon_y + (HEADER_ICON_SIZE - text_size.y) / 2),
|
||||
font_size, 0, label_color,
|
||||
)
|
||||
|
||||
def _draw_offset_pill(self, bounds: rl.Rectangle, text: str, y: float) -> None:
|
||||
text_size = measure_text_cached(self._font_semi_bold, text, OFFSET_FONT_SIZE)
|
||||
width = max(56.0, text_size.x + 20.0)
|
||||
pill = rl.Rectangle(bounds.x + (bounds.width - width) / 2, y, width, OFFSET_PILL_HEIGHT)
|
||||
rl.draw_rectangle_rounded(pill, roundness_for(pill, 17), 8, rl.Color(32, 20, 45, 255))
|
||||
rl.draw_rectangle_rounded_lines_ex(pill, roundness_for(pill, 17), 8, 2, OFFSET_COLOR)
|
||||
self._draw_centered_text(text, pill, y + (pill.height - text_size.y) / 2, OFFSET_FONT_SIZE, OFFSET_COLOR)
|
||||
|
||||
def _draw_unit(self, bounds: rl.Rectangle, y: float) -> None:
|
||||
text = tr(self._presentation.unit_text)
|
||||
color = COLORS.WHITE_TRANSLUCENT
|
||||
if self._pedal_override:
|
||||
text_size = measure_text_cached(self._font_semi_bold, text, UNIT_FONT_SIZE)
|
||||
text_shift = (PAUSE_ICON_WIDTH + PAUSE_ICON_GAP) / 2
|
||||
icon_x = bounds.x + (bounds.width - text_size.x) / 2 - text_shift
|
||||
icon_y = y + (text_size.y - PAUSE_ICON_HEIGHT) / 2
|
||||
bar_width = PAUSE_ICON_WIDTH / 3
|
||||
for x in (icon_x, icon_x + 2 * bar_width):
|
||||
rl.draw_rectangle_rec(rl.Rectangle(x, icon_y, bar_width, PAUSE_ICON_HEIGHT), OFFSET_COLOR)
|
||||
bounds = rl.Rectangle(bounds.x + text_shift, bounds.y, bounds.width, bounds.height)
|
||||
color = COLORS.DISENGAGED
|
||||
self._draw_centered_text(text, bounds, y, UNIT_FONT_SIZE, color)
|
||||
|
||||
def _max_header_color(self, active_side: str, cruise_set: bool) -> rl.Color:
|
||||
if self._pedal_override:
|
||||
return COLORS.DISENGAGED
|
||||
if cruise_set and ui_state.status == UIStatus.ENGAGED and active_side in ("max", "shared"):
|
||||
return COLORS.ENGAGED
|
||||
if cruise_set and ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE):
|
||||
return COLORS.DISENGAGED
|
||||
return COLORS.GREY
|
||||
|
||||
def _limit_header_color(self, active_side: str, overridden: bool) -> rl.Color:
|
||||
if self._pedal_override or overridden or ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE):
|
||||
return COLORS.DISENGAGED
|
||||
if ui_state.status == UIStatus.ENGAGED and active_side in ("slc", "shared"):
|
||||
return COLORS.ENGAGED
|
||||
return COLORS.GREY
|
||||
|
||||
def _draw_active_emphasis(self, rect: rl.Rectangle) -> None:
|
||||
presentation = self._presentation
|
||||
if self._pedal_override or presentation.mode == "merged" or ui_state.status != UIStatus.ENGAGED or presentation.active_side == "none":
|
||||
return
|
||||
if presentation.mode in ("max_only", "limit_only"):
|
||||
bounds = rect
|
||||
elif presentation.active_side == "slc":
|
||||
bounds = self._speed_limit_bounds(rect)
|
||||
elif presentation.active_side == "max":
|
||||
bounds = rl.Rectangle(rect.x, rect.y, rect.width / 2, rect.height)
|
||||
else:
|
||||
bounds = rect
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(bounds.x + 18, rect.y + 65),
|
||||
rl.Vector2(bounds.x + bounds.width - 18, rect.y + 65),
|
||||
3, UNIFIED_ACCENT,
|
||||
)
|
||||
|
||||
def _draw_merged_separator(self, rect: rl.Rectangle) -> None:
|
||||
center = rect.x + rect.width / 2
|
||||
shelf_y = rect.y + MERGED_SEPARATOR_Y
|
||||
valley_y = shelf_y + 12
|
||||
color = rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 170)
|
||||
rl.draw_line_ex(rl.Vector2(rect.x + 18, shelf_y), rl.Vector2(center - 34, shelf_y), 2, color)
|
||||
rl.draw_spline_segment_bezier_cubic(
|
||||
rl.Vector2(center - 34, shelf_y), rl.Vector2(center - 19, shelf_y),
|
||||
rl.Vector2(center - 23, valley_y), rl.Vector2(center - 7, valley_y), 2, color,
|
||||
)
|
||||
rl.draw_line_ex(rl.Vector2(center - 7, valley_y), rl.Vector2(center + 7, valley_y), 2, color)
|
||||
rl.draw_spline_segment_bezier_cubic(
|
||||
rl.Vector2(center + 7, valley_y), rl.Vector2(center + 23, valley_y),
|
||||
rl.Vector2(center + 19, shelf_y), rl.Vector2(center + 34, shelf_y), 2, color,
|
||||
)
|
||||
rl.draw_line_ex(rl.Vector2(center + 34, shelf_y), rl.Vector2(rect.x + rect.width - 18, shelf_y), 2, color)
|
||||
|
||||
def _draw_speed_limit_border(self, rect: rl.Rectangle, right: rl.Rectangle, color: rl.Color) -> None:
|
||||
# Clip the shared rounded outline so only the Speed Limit side changes.
|
||||
rl.begin_scissor_mode(int(right.x), int(rect.y), int(right.width + 1), int(rect.height + 1))
|
||||
try:
|
||||
rl.draw_rectangle_rounded_lines_ex(rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 3, color)
|
||||
finally:
|
||||
rl.end_scissor_mode()
|
||||
if self._presentation.mode == "split":
|
||||
rl.draw_line_ex(rl.Vector2(right.x, rect.y + 8), rl.Vector2(right.x, rect.y + rect.height - 8), 3, color)
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
presentation = self._presentation
|
||||
state = self._slc_state
|
||||
speed_color = COLORS.DISENGAGED if self._pedal_override else COLORS.WHITE
|
||||
rl.draw_rectangle_rounded_lines_ex(
|
||||
rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 7,
|
||||
rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 55),
|
||||
)
|
||||
draw_control_card(rect, fill=CONTROL_BG, border=UNIFIED_ACCENT, border_width=2)
|
||||
if presentation.mode == "split":
|
||||
divider_x = rect.x + rect.width / 2
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(divider_x, rect.y + 8), rl.Vector2(divider_x, rect.y + rect.height - 8),
|
||||
2, rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 110),
|
||||
)
|
||||
elif presentation.mode == "merged":
|
||||
self._draw_merged_separator(rect)
|
||||
|
||||
self._draw_active_emphasis(rect)
|
||||
max_bounds = rl.Rectangle(rect.x, rect.y, rect.width / 2, rect.height) if presentation.mode in ("split", "merged") else rect
|
||||
limit_bounds = self._speed_limit_bounds(rect)
|
||||
if self._show_max or presentation.confirmation_pending:
|
||||
max_color = COLORS.DARK_GREY if not self.hud_renderer.is_cruise_set else speed_color
|
||||
max_label_color = self._max_header_color(presentation.active_side, self.hud_renderer.is_cruise_set)
|
||||
self._draw_header(max_bounds, "MAX SET", "speedometer", max_label_color)
|
||||
if presentation.mode != "merged":
|
||||
self._draw_centered_text(presentation.max_speed_text, max_bounds, rect.y + 75, VALUE_FONT_SIZE, max_color, bold=True)
|
||||
self._draw_unit(max_bounds, rect.y + 204)
|
||||
|
||||
if limit_bounds is not None:
|
||||
icon_key = source_icon_key(presentation.source)
|
||||
overridden = bool(state and state['slc_overridden_speed'])
|
||||
label_color = self._limit_header_color(presentation.active_side, overridden)
|
||||
self._draw_header(limit_bounds, "SPEED LIMIT", icon_key, label_color)
|
||||
if presentation.mode != "merged":
|
||||
self._draw_centered_text(presentation.posted_speed_text, limit_bounds, rect.y + 75, VALUE_FONT_SIZE, speed_color, bold=True)
|
||||
if presentation.confirmation_pending:
|
||||
self._draw_centered_text(tr("PENDING"), limit_bounds, rect.y + 175, 25, CONFIRMATION_COLOR)
|
||||
elif presentation.offset_text is not None:
|
||||
self._draw_offset_pill(limit_bounds, presentation.offset_text, rect.y + 175)
|
||||
self._draw_unit(limit_bounds, rect.y + 204)
|
||||
|
||||
if presentation.mode == "merged":
|
||||
self._draw_centered_text(presentation.effective_speed_text, rect, rect.y + 98, VALUE_FONT_SIZE, speed_color, bold=True)
|
||||
self._draw_unit(rect, rect.y + 204)
|
||||
if presentation.offset_text is not None:
|
||||
self._draw_offset_pill(
|
||||
limit_bounds, presentation.offset_text, rect.y + MERGED_SEPARATOR_Y - OFFSET_PILL_HEIGHT / 2,
|
||||
)
|
||||
|
||||
if presentation.confirmation_pending and limit_bounds is not None:
|
||||
intensity = (1.0 + math.sin(2.0 * math.pi * rl.get_time())) / 2.0
|
||||
alpha = round(100 + 155 * intensity)
|
||||
pulse = rl.Color(CONFIRMATION_COLOR.r, CONFIRMATION_COLOR.g, CONFIRMATION_COLOR.b, alpha)
|
||||
self._draw_speed_limit_border(rect, limit_bounds, pulse)
|
||||
else:
|
||||
if limit_bounds is not None and state is not None:
|
||||
vision_color = _speed_limit_pulse_color(UNIFIED_ACCENT, UNIFIED_ACCENT.a)
|
||||
if (vision_color.r, vision_color.g, vision_color.b) != (UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b):
|
||||
self._draw_speed_limit_border(rect, limit_bounds, vision_color)
|
||||
if state is not None and ui_state.ui_params.get_bool("SpeedLimitSources"):
|
||||
_draw_sources_bubble(state, rect)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos) -> None:
|
||||
right = self._speed_limit_bounds(self.rect)
|
||||
if right is None and self._slc_state is not None:
|
||||
# The detailed source panel remains dismissible when no limit is valid.
|
||||
right = self.rect
|
||||
if right is None:
|
||||
return
|
||||
target = rl.Rectangle(right.x, right.y - self.TOUCH_SLOP,
|
||||
right.width + self.TOUCH_SLOP, right.height + 2 * self.TOUCH_SLOP)
|
||||
if not rl.check_collision_point_rec(mouse_pos, target):
|
||||
return
|
||||
if self._presentation.confirmation_pending:
|
||||
Params(memory=True).put_bool("SpeedLimitAccepted", True)
|
||||
return
|
||||
params = ui_state.ui_params
|
||||
params.put_bool("SpeedLimitSources", not params.get_bool("SpeedLimitSources"))
|
||||
@@ -69,8 +69,7 @@ def _load_starpilot_onroad_view(monkeypatch):
|
||||
stub_module("openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager", WidgetLayoutManager=dummy_widget)
|
||||
stub_module(
|
||||
"openpilot.selfdrive.ui.onroad.starpilot.widgets",
|
||||
SetSpeedWidget=dummy_widget,
|
||||
SpeedLimitWidget=dummy_widget,
|
||||
UnifiedSpeedWidget=dummy_widget,
|
||||
PedalIconsWidget=dummy_widget,
|
||||
AetherGaugeWidget=dummy_widget,
|
||||
PersonalityButtonWidget=dummy_widget,
|
||||
|
||||
@@ -80,39 +80,19 @@ def test_visible_source_rows_honor_active_only_and_source_order():
|
||||
]
|
||||
# When no sources have a valid speed reading (> 0), returns empty list (triggers empty state)
|
||||
assert visible_source_rows(
|
||||
source_defs, {key: 0.0 for key in values}, "Map Data", ("Map Data",),
|
||||
source_defs, dict.fromkeys(values, 0.0), "Map Data", ("Map Data",),
|
||||
) == []
|
||||
|
||||
|
||||
def test_source_label_color_override_and_engagement_states():
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import _source_label_color
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
def test_header_reuses_diagnostic_source_icons_without_unknown_fallback():
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import source_icon_key
|
||||
|
||||
# Engaged and not overridden -> Active green
|
||||
ui_state.status = UIStatus.ENGAGED
|
||||
color = _source_label_color(255, is_overridden=False)
|
||||
assert (color.r, color.g, color.b, color.a) == (COLORS.ENGAGED.r, COLORS.ENGAGED.g, COLORS.ENGAGED.b, 255)
|
||||
|
||||
# Engaged but overridden -> Disengaged/override gray
|
||||
color_overridden = _source_label_color(255, is_overridden=True)
|
||||
assert (color_overridden.r, color_overridden.g, color_overridden.b, color_overridden.a) == (
|
||||
COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255
|
||||
)
|
||||
|
||||
# Disengaged -> Disengaged/override gray
|
||||
ui_state.status = UIStatus.DISENGAGED
|
||||
color_disengaged = _source_label_color(255, is_overridden=False)
|
||||
assert (color_disengaged.r, color_disengaged.g, color_disengaged.b, color_disengaged.a) == (
|
||||
COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255
|
||||
)
|
||||
|
||||
# Override UI status -> Disengaged/override gray
|
||||
ui_state.status = UIStatus.OVERRIDE
|
||||
color_ui_override = _source_label_color(255, is_overridden=False)
|
||||
assert (color_ui_override.r, color_ui_override.g, color_ui_override.b, color_ui_override.a) == (
|
||||
COLORS.OVERRIDE.r, COLORS.OVERRIDE.g, COLORS.OVERRIDE.b, 255
|
||||
)
|
||||
assert source_icon_key("Vision") == "camera"
|
||||
assert source_icon_key("Dashboard") == "dashboard"
|
||||
assert source_icon_key("Map Data") == "map"
|
||||
assert source_icon_key("Mapbox") == "map"
|
||||
assert source_icon_key("None") is None
|
||||
assert source_icon_key("Unexpected") is None
|
||||
|
||||
|
||||
def test_vision_pulse_ignores_same_limit_source_flapping(monkeypatch):
|
||||
|
||||
@@ -0,0 +1,186 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import resolve_unified_speed
|
||||
|
||||
|
||||
def slc_state(posted_mph=65, offset_mph=0, source="Map Data", *, pending_mph=0,
|
||||
enabled=True, limiting=False, overridden=False, metric=False):
|
||||
conversion = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
|
||||
return {
|
||||
"accepted_speed_limit_ms": posted_mph / conversion,
|
||||
"effective_target_ms": max(0, posted_mph + offset_mph) / conversion,
|
||||
"offset_ms": offset_mph / conversion,
|
||||
"speed_conversion": conversion,
|
||||
"unconfirmed_speed_limit": pending_mph,
|
||||
"unconfirmed_valid": pending_mph > 0,
|
||||
"speed_limit_changed": pending_mph > 0,
|
||||
"presented_source": source,
|
||||
"slc_enabled": enabled,
|
||||
"slc_is_limiting_max_set": limiting,
|
||||
"slc_overridden_speed": 1.0 if overridden else 0.0,
|
||||
}
|
||||
|
||||
|
||||
@pytest.mark.parametrize("max_speed,posted,offset,expected_mode", [
|
||||
(80, 65, 5, "split"),
|
||||
(70, 70, 0, "merged"),
|
||||
(70, 65, 5, "merged"),
|
||||
(70, 65, 4, "split"),
|
||||
(65, 70, -5, "merged"),
|
||||
])
|
||||
def test_split_and_merge_use_effective_accepted_limit(max_speed, posted, offset, expected_mode):
|
||||
result = resolve_unified_speed(True, True, max_speed, slc_state(posted, offset), True, False)
|
||||
assert result.mode == expected_mode
|
||||
assert result.posted_speed_text == str(posted)
|
||||
assert result.effective_speed_text == str(posted + offset)
|
||||
assert result.offset_text == (f"{offset:+d}" if offset else None)
|
||||
|
||||
|
||||
def test_pending_candidate_forces_split_without_replacing_accepted_target():
|
||||
state = slc_state(65, 5, source="Vision", pending_mph=75)
|
||||
result = resolve_unified_speed(True, True, 70, state, True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.confirmation_pending
|
||||
assert result.posted_speed_text == "75"
|
||||
assert result.effective_speed_text == "70"
|
||||
assert result.source == "Vision"
|
||||
|
||||
|
||||
def test_resolution_after_confirmation_uses_same_equality_rule():
|
||||
state = slc_state(65, 5, pending_mph=75)
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split"
|
||||
state["speed_limit_changed"] = state["unconfirmed_valid"] = False
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
assert resolve_unified_speed(True, True, 80, state, True, False).mode == "split"
|
||||
|
||||
|
||||
def test_source_change_and_override_do_not_change_layout():
|
||||
state = slc_state(65, 5)
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
state["presented_source"] = "Vision"
|
||||
result = resolve_unified_speed(True, True, 70, state, True, False)
|
||||
assert result.mode == "merged"
|
||||
assert result.source == "Vision"
|
||||
state["presented_source"] = "Dashboard"
|
||||
state["slc_overridden_speed"] = 40.0
|
||||
result = resolve_unified_speed(True, True, 70, state, True, False)
|
||||
assert result.mode == "merged"
|
||||
assert result.source == "Dashboard"
|
||||
|
||||
|
||||
def test_source_target_change_recomputes_layout_independently():
|
||||
state = slc_state(65, 5, source="Map Data")
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
state.update(slc_state(55, 5, source="Vision"))
|
||||
result = resolve_unified_speed(True, True, 70, state, True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.source == "Vision"
|
||||
|
||||
|
||||
def test_active_side_uses_published_control_semantic():
|
||||
state = slc_state(65, 5, limiting=True)
|
||||
assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "slc"
|
||||
state["slc_is_limiting_max_set"] = False
|
||||
assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "max"
|
||||
state["slc_overridden_speed"] = 40.0
|
||||
assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "none"
|
||||
|
||||
|
||||
def test_display_only_speed_limit_stays_split():
|
||||
result = resolve_unified_speed(True, True, 70, slc_state(70, enabled=False), False, False)
|
||||
assert result.mode == "split"
|
||||
|
||||
|
||||
def test_disabled_confirmation_does_not_force_split():
|
||||
state = slc_state(65, 5, pending_mph=75)
|
||||
state["speed_limit_changed"] = False
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
|
||||
|
||||
def test_missing_limit_never_renders_zero_or_a_stale_source():
|
||||
result = resolve_unified_speed(True, True, 70, slc_state(0, source="None"), True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.posted_speed_text == "–"
|
||||
assert result.effective_speed_text == "–"
|
||||
assert result.source == "None"
|
||||
|
||||
|
||||
def test_missing_limit_with_slc_disabled_allows_max_only():
|
||||
result = resolve_unified_speed(True, True, 70, slc_state(0, source="None", enabled=False), False, False)
|
||||
assert result.mode == "max_only"
|
||||
|
||||
|
||||
@pytest.mark.parametrize("show_max,expected_mode", [(True, "split"), (False, "limit_only")])
|
||||
def test_stale_slc_data_keeps_speed_limit_region(show_max, expected_mode):
|
||||
result = resolve_unified_speed(show_max, True, 70, None, True, False)
|
||||
assert result.mode == expected_mode
|
||||
assert result.posted_speed_text == "–"
|
||||
assert result.effective_speed_text == "–"
|
||||
assert result.source == "None"
|
||||
|
||||
|
||||
def test_missing_data_with_slc_disabled_uses_max_only():
|
||||
assert resolve_unified_speed(True, True, 70, None, False, False).mode == "max_only"
|
||||
|
||||
|
||||
def test_persisted_previous_limit_without_source_remains_visible():
|
||||
result = resolve_unified_speed(True, True, 70, slc_state(45, source="Previous Limit"), True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.posted_speed_text == "45"
|
||||
assert result.source == "Previous Limit"
|
||||
|
||||
|
||||
def test_low_limit_with_large_negative_offset_preserves_configured_offset():
|
||||
result = resolve_unified_speed(True, True, 70, slc_state(5, -99), True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.posted_speed_text == "5"
|
||||
assert result.effective_speed_text == "–"
|
||||
assert result.offset_text == "-99"
|
||||
|
||||
|
||||
def test_metric_and_rounding_follow_the_displayed_value():
|
||||
state = slc_state(65.4, 4.4, metric=True)
|
||||
result = resolve_unified_speed(True, True, 70, state, True, True)
|
||||
assert result.mode == "merged"
|
||||
assert result.posted_speed_text == "65"
|
||||
assert result.offset_text == "+4"
|
||||
assert result.unit_text == "km/h"
|
||||
|
||||
|
||||
def test_invisible_fraction_does_not_keep_card_split():
|
||||
state = slc_state(65.1, 5.2)
|
||||
result = resolve_unified_speed(True, True, 70.4, state, True, False)
|
||||
assert result.mode == "merged"
|
||||
|
||||
|
||||
def test_hidden_max_still_shows_posted_limit():
|
||||
result = resolve_unified_speed(False, True, 70, slc_state(65), True, False)
|
||||
assert result.mode == "limit_only"
|
||||
result = resolve_unified_speed(False, True, 70, slc_state(65, enabled=False), False, False)
|
||||
assert result.mode == "limit_only"
|
||||
|
||||
|
||||
def test_confirmation_forces_split_even_when_max_is_hidden():
|
||||
result = resolve_unified_speed(False, True, 70, slc_state(65, pending_mph=75), True, False)
|
||||
assert result.mode == "split"
|
||||
assert result.max_speed_text == "70"
|
||||
assert result.confirmation_pending
|
||||
|
||||
|
||||
def test_offset_max_pending_and_source_transitions_recompute_mode():
|
||||
state = slc_state(65)
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split"
|
||||
state.update(slc_state(65, 5))
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
assert resolve_unified_speed(True, True, 75, state, True, False).mode == "split"
|
||||
state.update(slc_state(65, 5, pending_mph=75))
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split"
|
||||
state.update(slc_state(65, 5))
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
state.update(slc_state(0, source="None"))
|
||||
missing = resolve_unified_speed(True, True, 70, state, True, False)
|
||||
assert missing.mode == "split"
|
||||
assert missing.posted_speed_text == "–"
|
||||
state.update(slc_state(65, 5))
|
||||
assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged"
|
||||
@@ -0,0 +1,495 @@
|
||||
from types import SimpleNamespace
|
||||
from dataclasses import replace
|
||||
|
||||
import pyray as rl
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import UnifiedSpeedPresentation, resolve_unified_speed
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets import unified_speed
|
||||
|
||||
|
||||
def make_widget(mode="split", pending=False):
|
||||
widget = object.__new__(unified_speed.UnifiedSpeedWidget)
|
||||
widget._rect = rl.Rectangle(30, 75, 520, 250)
|
||||
widget._presentation = UnifiedSpeedPresentation(mode, "70", "65", "70", "+5", "mph", "Map Data", pending, "slc")
|
||||
widget._show_max = True
|
||||
widget._slc_state = None
|
||||
widget._pedal_override = False
|
||||
widget.hud_renderer = SimpleNamespace(is_cruise_set=True)
|
||||
return widget
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def header_icon_cache(monkeypatch):
|
||||
app = object.__new__(type(unified_speed.gui_app))
|
||||
app._scale = app._pixel_scale_x = app._pixel_scale_y = 1.0
|
||||
app._cached_render_textures = {}
|
||||
app._pending_render_textures = {}
|
||||
geometry, draws, allocations, scales = [], [], [], []
|
||||
monkeypatch.setattr(unified_speed, "gui_app", app)
|
||||
monkeypatch.setattr(unified_speed, "_draw_source_icon", lambda *args: geometry.append(args))
|
||||
monkeypatch.setattr(unified_speed, "measure_text_cached", lambda *args: rl.Vector2(100, 28))
|
||||
monkeypatch.setattr(rl, "draw_text_ex", lambda *args: None)
|
||||
monkeypatch.setattr(rl, "draw_texture_pro", lambda *args: draws.append(args))
|
||||
monkeypatch.setattr(rl, "rl_scalef", lambda *args: scales.append(args))
|
||||
for name in ("rl_push_matrix", "rl_pop_matrix", "begin_texture_mode", "end_texture_mode", "clear_background",
|
||||
"rl_set_blend_factors_separate", "begin_blend_mode", "end_blend_mode", "set_texture_filter", "set_texture_wrap"):
|
||||
monkeypatch.setattr(rl, name, lambda *args: None)
|
||||
|
||||
def allocate(width, height):
|
||||
allocations.append((width, height))
|
||||
return SimpleNamespace(texture=SimpleNamespace(width=width, height=height))
|
||||
|
||||
monkeypatch.setattr(rl, "load_render_texture", allocate)
|
||||
return app, geometry, draws, allocations, scales
|
||||
|
||||
|
||||
def test_header_glyph_cache_is_shared_and_skips_geometry_after_first_frame(header_icon_cache):
|
||||
app, geometry, draws, allocations, _scales = header_icon_cache
|
||||
widgets = [make_widget(), make_widget()]
|
||||
for widget in widgets:
|
||||
widget._font_semi_bold = None
|
||||
widget = widgets[0]
|
||||
for label, icon in (("MAX SET", "speedometer"), ("SPEED LIMIT", "map")):
|
||||
widget._draw_header(widget.rect, label, icon, rl.WHITE)
|
||||
assert len(geometry) == 2
|
||||
assert allocations == []
|
||||
app._populate_render_texture_cache()
|
||||
assert len(geometry) == 4
|
||||
|
||||
for frame in range(60):
|
||||
widget = widgets[frame % 2]
|
||||
bounds = rl.Rectangle(frame, frame, 260, 250)
|
||||
widget._draw_header(bounds, "MAX SET", "speedometer", rl.WHITE)
|
||||
widget._draw_header(bounds, f"LIMIT {frame}", "map", rl.GRAY)
|
||||
assert len(geometry) == 4
|
||||
assert len(draws) == 120
|
||||
assert len(allocations) == len(app._cached_render_textures) == 2
|
||||
assert app._pending_render_textures == {}
|
||||
|
||||
|
||||
@pytest.mark.parametrize("scale,dpi,texture_size", [(0.5, 1.0, 68), (1.0, 2.0, 136), (1.25, 1.5, 128)])
|
||||
def test_header_cache_resolution_preserves_logical_geometry(header_icon_cache, scale, dpi, texture_size):
|
||||
app, geometry, draws, allocations, scales = header_icon_cache
|
||||
app._scale, app._pixel_scale_x = scale, dpi
|
||||
for icon in ("speedometer", "map", "camera", "dashboard", "next"):
|
||||
unified_speed._draw_header_icon(icon, 10, 20)
|
||||
app._populate_render_texture_cache()
|
||||
assert len(app._cached_render_textures) == 5
|
||||
assert allocations == [(texture_size, texture_size)] * 5
|
||||
assert all(args[1:4] == (0, 0, 34) for args in geometry[5:])
|
||||
assert scales == [(texture_size / 34, texture_size / 34, 1.0)] * 5
|
||||
|
||||
unified_speed._draw_header_icon("map", 200, 300)
|
||||
assert len(geometry) == 10
|
||||
source, destination = draws[-1][1:3]
|
||||
assert (source.width, source.height) == (texture_size, -texture_size)
|
||||
assert (destination.x, destination.y, destination.width, destination.height) == (200, 300, 34, 34)
|
||||
|
||||
|
||||
def test_speed_limit_hit_target_is_right_half_in_both_layouts():
|
||||
for mode in ("split", "merged"):
|
||||
right = make_widget(mode)._speed_limit_bounds(rl.Rectangle(30, 75, 520, 250))
|
||||
assert (right.x, right.width) == (290, 260)
|
||||
|
||||
|
||||
def test_confirmation_touch_only_accepts_on_speed_limit_side(monkeypatch):
|
||||
widget = make_widget(pending=True)
|
||||
writes = []
|
||||
monkeypatch.setattr(unified_speed, "Params", lambda memory: SimpleNamespace(put_bool=lambda key, value: writes.append((key, value))))
|
||||
widget._handle_mouse_press(rl.Vector2(100, 150))
|
||||
assert writes == []
|
||||
widget._handle_mouse_press(rl.Vector2(400, 150))
|
||||
assert writes == [("SpeedLimitAccepted", True)]
|
||||
|
||||
|
||||
def test_merged_speed_limit_side_toggles_sources(monkeypatch):
|
||||
widget = make_widget("merged")
|
||||
writes = []
|
||||
params = SimpleNamespace(get_bool=lambda _key: False, put_bool=lambda key, value: writes.append((key, value)))
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(ui_params=params))
|
||||
widget._handle_mouse_press(rl.Vector2(100, 150))
|
||||
assert writes == []
|
||||
widget._handle_mouse_press(rl.Vector2(400, 150))
|
||||
assert writes == [("SpeedLimitSources", True)]
|
||||
|
||||
|
||||
def test_diagnostic_sources_can_be_dismissed_from_max_only_card(monkeypatch):
|
||||
widget = make_widget("max_only")
|
||||
widget._slc_state = {}
|
||||
params = SimpleNamespace(get_bool=lambda _key: True, put_bool=lambda key, value: writes.append((key, value)))
|
||||
writes = []
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(ui_params=params))
|
||||
widget._handle_mouse_press(rl.Vector2(100, 150))
|
||||
assert writes == [("SpeedLimitSources", False)]
|
||||
|
||||
|
||||
def test_right_border_overlay_is_clipped_to_speed_limit_side(monkeypatch):
|
||||
events = []
|
||||
monkeypatch.setattr(unified_speed.rl, "begin_scissor_mode", lambda *args: events.append(("begin", args)))
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: events.append(("outline", args)))
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: events.append(("divider", args)))
|
||||
monkeypatch.setattr(unified_speed.rl, "end_scissor_mode", lambda: events.append(("end",)))
|
||||
for mode, expected in (("split", ["begin", "outline", "end", "divider"]),
|
||||
("merged", ["begin", "outline", "end"])):
|
||||
events.clear()
|
||||
widget = make_widget(mode)
|
||||
rect = widget.rect
|
||||
right = widget._speed_limit_bounds(rect)
|
||||
widget._draw_speed_limit_border(rect, right, rl.Color(188, 132, 255, 200))
|
||||
assert events[0] == ("begin", (290, 75, 261, 251))
|
||||
assert [event[0] for event in events] == expected
|
||||
|
||||
|
||||
def test_split_and_merged_draw_one_card_with_both_headers(monkeypatch):
|
||||
cards = []
|
||||
lines = []
|
||||
monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: cards.append(args[0]))
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.DISENGAGED,
|
||||
ui_params=SimpleNamespace(get_bool=lambda _key: False)))
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args))
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
for mode in ("split", "merged"):
|
||||
lines.clear()
|
||||
widget = make_widget(mode)
|
||||
headers = []
|
||||
values = []
|
||||
separators = []
|
||||
offsets = []
|
||||
monkeypatch.setattr(widget, "_draw_header", lambda _bounds, text, icon, _color, rows=headers: rows.append((text, icon)))
|
||||
monkeypatch.setattr(widget, "_draw_centered_text", lambda text, *args, rows=values, **kwargs: rows.append(text))
|
||||
monkeypatch.setattr(widget, "_draw_offset_pill", lambda bounds, text, y, rows=offsets: rows.append((bounds, text, y)))
|
||||
monkeypatch.setattr(widget, "_draw_merged_separator", lambda _rect, rows=separators: rows.append(True))
|
||||
monkeypatch.setattr(widget, "_draw_active_emphasis", lambda *args: None)
|
||||
widget._render(widget.rect)
|
||||
assert headers == [("MAX SET", "speedometer"), ("SPEED LIMIT", "map")]
|
||||
assert separators == ([True] if mode == "merged" else [])
|
||||
assert sum(line[0].x == line[1].x == 290 for line in lines) == (1 if mode == "split" else 0)
|
||||
assert values == (["70", "mph"] if mode == "merged" else ["70", "mph", "65", "mph"])
|
||||
assert offsets[0][0].x == 290
|
||||
assert offsets[0][2] == (136 if mode == "merged" else 250)
|
||||
assert len(cards) == 2
|
||||
|
||||
|
||||
def test_merged_draws_effective_speed_once_and_skips_active_line(monkeypatch):
|
||||
widget = make_widget("merged")
|
||||
widget._presentation = replace(widget._presentation, max_speed_text="71", effective_speed_text="70", active_side="shared")
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED,
|
||||
ui_params=SimpleNamespace(get_bool=lambda _key: False)))
|
||||
monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: None)
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
lines = []
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args))
|
||||
monkeypatch.setattr(widget, "_draw_merged_separator", lambda _rect: None)
|
||||
monkeypatch.setattr(widget, "_draw_header", lambda *args: None)
|
||||
monkeypatch.setattr(widget, "_draw_offset_pill", lambda *args: None)
|
||||
values = []
|
||||
monkeypatch.setattr(widget, "_draw_centered_text", lambda text, *args, **kwargs: values.append(text))
|
||||
widget._render(widget.rect)
|
||||
assert values == ["70", "mph"]
|
||||
assert lines == []
|
||||
|
||||
|
||||
def test_merged_separator_has_shallow_center_dip(monkeypatch):
|
||||
widget = make_widget("merged")
|
||||
segments = []
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: segments.append(("line", args)))
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_spline_segment_bezier_cubic", lambda *args: segments.append(("curve", args)))
|
||||
widget._draw_merged_separator(widget.rect)
|
||||
assert [segment[0] for segment in segments] == ["line", "curve", "line", "curve", "line"]
|
||||
assert segments[0][1][0].y == widget.rect.y + 76
|
||||
assert segments[2][1][0].y == widget.rect.y + 88
|
||||
|
||||
|
||||
def test_enabled_slc_stays_full_width_when_plan_is_stale(monkeypatch):
|
||||
widget = make_widget("split")
|
||||
widget._snapshot_frame = None
|
||||
widget.hud_renderer = SimpleNamespace(is_cruise_available=True, is_cruise_set=True, set_speed=70)
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(
|
||||
sm=SimpleNamespace(frame=1), starpilot_toggles={}, is_metric=False, engaged=False,
|
||||
))
|
||||
monkeypatch.setattr(unified_speed, "_is_slc_enabled", lambda: True)
|
||||
monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: None)
|
||||
assert widget.get_size() == (520.0, 250.0)
|
||||
assert widget.is_visible
|
||||
assert widget._presentation.posted_speed_text == "–"
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def pedal_snapshot(monkeypatch):
|
||||
class SubMaster(dict):
|
||||
pass
|
||||
|
||||
sm = SubMaster(carState=SimpleNamespace(gasPressed=True))
|
||||
sm.frame = 20
|
||||
sm.valid = {"carState": True}
|
||||
sm.alive = {"carState": True}
|
||||
sm.recv_frame = {"carState": 20}
|
||||
ui = SimpleNamespace(sm=sm, started_frame=10, engaged=True, starpilot_toggles={}, is_metric=False)
|
||||
widget = make_widget()
|
||||
widget._snapshot_frame = None
|
||||
widget.hud_renderer = SimpleNamespace(is_cruise_available=True, is_cruise_set=True, set_speed=70)
|
||||
monkeypatch.setattr(unified_speed, "ui_state", ui)
|
||||
monkeypatch.setattr(unified_speed, "_is_slc_enabled", lambda: True)
|
||||
monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: None)
|
||||
return widget, ui
|
||||
|
||||
|
||||
@pytest.mark.parametrize("gas,engaged,cruise_set,valid,alive,received,expected", [
|
||||
(True, True, True, True, True, 20, True),
|
||||
(False, True, True, True, True, 20, False),
|
||||
(True, False, True, True, True, 20, False),
|
||||
(True, True, False, True, True, 20, False),
|
||||
(True, True, True, False, True, 20, False),
|
||||
(True, True, True, True, False, 20, False),
|
||||
(True, True, True, True, True, 9, False),
|
||||
])
|
||||
def test_pedal_override_requires_fresh_gas_and_engaged_cruise(pedal_snapshot, gas, engaged, cruise_set, valid, alive, received, expected):
|
||||
widget, ui = pedal_snapshot
|
||||
ui.sm["carState"].gasPressed = gas
|
||||
ui.engaged = engaged
|
||||
widget.hud_renderer.is_cruise_set = cruise_set
|
||||
ui.sm.valid["carState"] = valid
|
||||
ui.sm.alive["carState"] = alive
|
||||
ui.sm.recv_frame["carState"] = received
|
||||
widget._refresh_snapshot()
|
||||
assert widget._pedal_override == expected
|
||||
|
||||
|
||||
def test_pedal_cue_clears_on_release_with_a_persistent_slc_override(pedal_snapshot, monkeypatch):
|
||||
widget, ui = pedal_snapshot
|
||||
sm = ui.sm
|
||||
state = {
|
||||
"speed_conversion": CV.MS_TO_MPH, "accepted_speed_limit_ms": 65 * CV.MPH_TO_MS,
|
||||
"effective_target_ms": 70 * CV.MPH_TO_MS, "offset_ms": 5 * CV.MPH_TO_MS,
|
||||
"speed_limit_changed": False, "unconfirmed_valid": False, "presented_source": "Map Data",
|
||||
"slc_is_limiting_max_set": False, "slc_overridden_speed": 80 * CV.MPH_TO_MS,
|
||||
}
|
||||
monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: state)
|
||||
widget._refresh_snapshot()
|
||||
assert widget._pedal_override
|
||||
presentation = widget._presentation
|
||||
|
||||
sm["carState"].gasPressed = False
|
||||
widget._refresh_snapshot()
|
||||
assert widget._pedal_override
|
||||
sm.frame += 1
|
||||
sm.recv_frame["carState"] = sm.frame
|
||||
widget._refresh_snapshot()
|
||||
assert not widget._pedal_override
|
||||
assert widget._presentation == presentation
|
||||
assert widget._slc_state["slc_overridden_speed"] > 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode", ["split", "merged", "max_only", "limit_only"])
|
||||
@pytest.mark.parametrize("unit", ["mph", "km/h"])
|
||||
def test_pedal_cue_mutes_targets_and_preserves_units_offsets_and_layout(monkeypatch, mode, unit):
|
||||
widget = make_widget(mode)
|
||||
widget._pedal_override = True
|
||||
widget._font_semi_bold = None
|
||||
widget._show_max = mode != "limit_only"
|
||||
widget._presentation = replace(widget._presentation, unit_text=unit)
|
||||
if mode in ("max_only", "limit_only"):
|
||||
widget._rect.width = 250
|
||||
values, headers, pauses, offsets, lines = [], [], [], [], []
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED))
|
||||
monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: None)
|
||||
monkeypatch.setattr(unified_speed, "measure_text_cached", lambda *args: rl.Vector2(60, 28))
|
||||
monkeypatch.setattr(rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
monkeypatch.setattr(rl, "draw_rectangle_rec", lambda *args: pauses.append(args))
|
||||
monkeypatch.setattr(rl, "draw_line_ex", lambda *args: lines.append(args))
|
||||
monkeypatch.setattr(widget, "_draw_merged_separator", lambda *args: None)
|
||||
monkeypatch.setattr(widget, "_draw_header", lambda bounds, text, icon, color: headers.append(color))
|
||||
monkeypatch.setattr(widget, "_draw_centered_text", lambda text, bounds, y, size, color, **kwargs: values.append((text, bounds, size, color)))
|
||||
monkeypatch.setattr(widget, "_draw_offset_pill", lambda bounds, text, y: offsets.append(text))
|
||||
|
||||
widget._render(widget.rect)
|
||||
speed_values = [value for value in values if value[2] == unified_speed.VALUE_FONT_SIZE]
|
||||
unit_values = [value for value in values if value[2] == unified_speed.UNIT_FONT_SIZE]
|
||||
expected_speeds = {"split": ["70", "65"], "merged": ["70"], "max_only": ["70"], "limit_only": ["65"]}
|
||||
assert [value[0] for value in speed_values] == expected_speeds[mode]
|
||||
assert all(value[3] == unified_speed.COLORS.DISENGAGED for value in speed_values + unit_values)
|
||||
assert all(color == unified_speed.COLORS.DISENGAGED for color in headers)
|
||||
assert [value[0] for value in unit_values] == [unit] * (2 if mode == "split" else 1)
|
||||
assert len(pauses) == 2 * len(unit_values)
|
||||
assert all(color == unified_speed.OFFSET_COLOR for _bounds, color in pauses)
|
||||
assert offsets == ([] if mode == "max_only" else ["+5"])
|
||||
assert not any(line[2] == 3 for line in lines)
|
||||
for index, value in enumerate(unit_values):
|
||||
pause = pauses[index * 2][0]
|
||||
assert pause.x == pytest.approx(value[1].x + (value[1].width - 60) / 2 - 20)
|
||||
|
||||
values.clear()
|
||||
pauses.clear()
|
||||
widget._pedal_override = False
|
||||
widget._render(widget.rect)
|
||||
assert not pauses
|
||||
assert all(value[3] == unified_speed.COLORS.WHITE for value in values if value[2] == unified_speed.VALUE_FONT_SIZE)
|
||||
assert all(value[3] == unified_speed.COLORS.WHITE_TRANSLUCENT for value in values if value[2] == unified_speed.UNIT_FONT_SIZE)
|
||||
|
||||
|
||||
def test_split_merged_transitions_keep_the_same_footprint(monkeypatch):
|
||||
widget = make_widget("split")
|
||||
monkeypatch.setattr(widget, "_refresh_snapshot", lambda: None)
|
||||
sizes = []
|
||||
for mode in ("split", "merged", "split", "merged"):
|
||||
widget._presentation = replace(widget._presentation, mode=mode)
|
||||
sizes.append(widget.get_size())
|
||||
assert sizes == [(520.0, 250.0)] * 4
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def slc_ui(monkeypatch):
|
||||
class Params(dict):
|
||||
def get_bool(self, key):
|
||||
return bool(self.get(key))
|
||||
|
||||
def get(self, key, encoding=None):
|
||||
return super().get(key)
|
||||
|
||||
class SubMaster(dict):
|
||||
recv_frame = {"starpilotPlan": 10}
|
||||
valid = {"starpilotCarState": True}
|
||||
|
||||
plan = custom.StarPilotPlan.new_message(
|
||||
slcSpeedLimit=30 * CV.MPH_TO_MS, slcSpeedLimitOffset=0.0, slcSpeedLimitSource="Map Data",
|
||||
slcOverriddenSpeed=0.0, slcMapSpeedLimit=30 * CV.MPH_TO_MS, slcMapboxSpeedLimit=0.0,
|
||||
slcNextSpeedLimit=0.0, unconfirmedSlcSpeedLimit=0.0, speedLimitChanged=False,
|
||||
)
|
||||
sm = SubMaster(starpilotPlan=plan, starpilotCarState=SimpleNamespace(dashboardSpeedLimit=0.0))
|
||||
sm.recv_frame = sm.recv_frame.copy()
|
||||
params = Params(SpeedLimitController=True, ShowSpeedLimits=False)
|
||||
ui = SimpleNamespace(
|
||||
sm=sm, started_frame=10, is_metric=False, ui_params=params, starpilot_toggles={},
|
||||
params_memory=SimpleNamespace(get_float=lambda _key: 0.0),
|
||||
)
|
||||
monkeypatch.setattr(slc_speed_limit, "ui_state", ui)
|
||||
monkeypatch.setattr(slc_speed_limit, "starpilot_state", SimpleNamespace(car_state=SimpleNamespace(hasDashSpeedLimits=True)))
|
||||
monkeypatch.setattr(slc_speed_limit, "_tick_pulse", lambda *args: None)
|
||||
return ui
|
||||
|
||||
|
||||
def test_slc_state_extraction_respects_feature_and_display_toggles(slc_ui):
|
||||
assert slc_speed_limit._is_slc_enabled()
|
||||
assert slc_speed_limit._get_slc_state()["slc_enabled"]
|
||||
slc_ui.starpilot_toggles["speed_limit_controller"] = False
|
||||
assert not slc_speed_limit._is_slc_enabled()
|
||||
assert slc_speed_limit._get_slc_state() is None
|
||||
slc_ui.ui_params["ShowSpeedLimits"] = True
|
||||
assert not slc_speed_limit._get_slc_state()["slc_enabled"]
|
||||
slc_ui.starpilot_toggles["speed_limit_controller"] = True
|
||||
slc_ui.sm.recv_frame["starpilotPlan"] = 9
|
||||
assert slc_speed_limit._get_slc_state() is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("presented_source,expected_source,expected_speed", [
|
||||
("", "Map Data", "30"),
|
||||
("Map Data", "Map Data", "30"),
|
||||
("None", "None", "–"),
|
||||
("Previous Limit", "Previous Limit", "30"),
|
||||
("Vision", "Vision", "30"),
|
||||
])
|
||||
def test_serialized_plan_source_defaults_and_explicit_values(slc_ui, presented_source, expected_source, expected_speed):
|
||||
message = slc_ui.sm["starpilotPlan"]
|
||||
if presented_source:
|
||||
message.slcPresentedSpeedLimitSource = presented_source
|
||||
# Replay decodes older plans with a present but empty Text attribute.
|
||||
with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan:
|
||||
slc_ui.sm["starpilotPlan"] = plan
|
||||
state = slc_speed_limit._get_slc_state()
|
||||
result = resolve_unified_speed(True, True, 35, state, True, False)
|
||||
assert result.source == expected_source
|
||||
assert result.posted_speed_text == expected_speed
|
||||
assert result.mode == "split"
|
||||
|
||||
|
||||
def test_legacy_replay_limit_and_offset_merge_with_max_set(slc_ui):
|
||||
message = slc_ui.sm["starpilotPlan"]
|
||||
message.slcSpeedLimitOffset = 5 * CV.MPH_TO_MS
|
||||
with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan:
|
||||
slc_ui.sm["starpilotPlan"] = plan
|
||||
result = resolve_unified_speed(True, True, 35, slc_speed_limit._get_slc_state(), True, False)
|
||||
assert (result.source, result.posted_speed_text, result.effective_speed_text) == ("Map Data", "30", "35")
|
||||
assert (result.mode, result.offset_text) == ("merged", "+5")
|
||||
|
||||
|
||||
@pytest.mark.parametrize("presented_source,limiting,max_speed,enabled,overridden,expected_side,line_x", [
|
||||
("", False, 40, True, False, "slc", 308),
|
||||
("", False, 34, True, False, "max", 48),
|
||||
("", False, 35, True, False, "shared", None),
|
||||
("", False, 40, False, False, "max", 48),
|
||||
("", False, 40, True, True, "none", None),
|
||||
("Map Data", False, 40, True, False, "max", 48),
|
||||
("Map Data", True, 40, True, False, "slc", 308),
|
||||
])
|
||||
def test_active_underline_with_legacy_and_current_plans(slc_ui, monkeypatch, presented_source, limiting,
|
||||
max_speed, enabled, overridden, expected_side, line_x):
|
||||
message = slc_ui.sm["starpilotPlan"]
|
||||
message.slcSpeedLimitOffset = 5 * CV.MPH_TO_MS
|
||||
message.slcPresentedSpeedLimitSource = presented_source
|
||||
message.slcIsLimitingMaxSet = limiting
|
||||
message.slcOverriddenSpeed = 40 * CV.MPH_TO_MS if overridden else 0.0
|
||||
slc_ui.starpilot_toggles["speed_limit_controller"] = enabled
|
||||
slc_ui.ui_params["ShowSpeedLimits"] = True
|
||||
with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan:
|
||||
slc_ui.sm["starpilotPlan"] = plan
|
||||
presentation = resolve_unified_speed(True, True, max_speed, slc_speed_limit._get_slc_state(), enabled, False)
|
||||
assert presentation.active_side == expected_side
|
||||
|
||||
widget = make_widget(presentation.mode)
|
||||
widget._presentation = presentation
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED))
|
||||
lines = []
|
||||
monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args))
|
||||
widget._draw_active_emphasis(widget.rect)
|
||||
if line_x is None:
|
||||
assert lines == []
|
||||
else:
|
||||
assert len(lines) == 1
|
||||
assert (lines[0][0].x, lines[0][0].y, lines[0][1].x) == (line_x, 140, line_x + 224)
|
||||
assert lines[0][3] == unified_speed.UNIFIED_ACCENT
|
||||
|
||||
|
||||
def test_legacy_plan_without_active_source_does_not_use_diagnostic_map_limit(slc_ui):
|
||||
message = slc_ui.sm["starpilotPlan"]
|
||||
message.slcSpeedLimitSource = "None"
|
||||
with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan:
|
||||
slc_ui.sm["starpilotPlan"] = plan
|
||||
state = slc_speed_limit._get_slc_state()
|
||||
assert round(state["map_sl"]) == 30
|
||||
result = resolve_unified_speed(True, True, 35, state, True, False)
|
||||
assert (result.source, result.posted_speed_text, result.mode) == ("None", "–", "split")
|
||||
|
||||
|
||||
def test_legacy_pending_candidate_remains_visible_without_active_source(slc_ui):
|
||||
message = slc_ui.sm["starpilotPlan"]
|
||||
message.slcSpeedLimitSource = "None"
|
||||
message.unconfirmedSlcSpeedLimit = 45 * CV.MPH_TO_MS
|
||||
message.speedLimitChanged = True
|
||||
with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan:
|
||||
slc_ui.sm["starpilotPlan"] = plan
|
||||
result = resolve_unified_speed(True, True, 35, slc_speed_limit._get_slc_state(), True, False)
|
||||
assert (result.posted_speed_text, result.mode, result.confirmation_pending) == ("45", "split", True)
|
||||
|
||||
|
||||
def test_header_colors_preserve_engaged_disengaged_and_override_semantics(monkeypatch):
|
||||
widget = make_widget()
|
||||
colors = unified_speed.COLORS
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED))
|
||||
assert widget._max_header_color("max", True) == colors.ENGAGED
|
||||
assert widget._max_header_color("slc", True) == colors.GREY
|
||||
assert widget._limit_header_color("slc", False) == colors.ENGAGED
|
||||
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.DISENGAGED))
|
||||
assert widget._max_header_color("max", True) == colors.DISENGAGED
|
||||
assert widget._limit_header_color("slc", False) == colors.DISENGAGED
|
||||
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.OVERRIDE))
|
||||
assert widget._max_header_color("max", True) == colors.DISENGAGED
|
||||
assert widget._limit_header_color("slc", False) == colors.DISENGAGED
|
||||
|
||||
monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED))
|
||||
assert widget._limit_header_color("none", True) == colors.DISENGAGED
|
||||
@@ -137,6 +137,19 @@ class TestWidgetLayoutManager(unittest.TestCase):
|
||||
# w3: should stack directly below w1: y = 75 + 100 + 15 = 190
|
||||
self.assertEqual(w3.rect.y, 190)
|
||||
|
||||
def test_wide_unified_card_stays_inside_the_left_edge(self):
|
||||
card = DummyLayoutWidget("unified_speed", priority=1, width=520, height=250)
|
||||
gauge = DummyLayoutWidget("aethergauge", priority=3, width=176, height=260)
|
||||
self.layout_manager.register_widget("left", card)
|
||||
self.layout_manager.register_widget("left", gauge)
|
||||
|
||||
self.layout_manager.update_layout(self.content_rect)
|
||||
|
||||
self.assertEqual(card.rect.x, self.content_rect.x + 30)
|
||||
self.assertEqual(card.rect.y, self.content_rect.y + 45)
|
||||
self.assertEqual(gauge.rect.x + gauge.rect.width / 2, self.content_rect.x + 146)
|
||||
self.assertEqual(gauge.rect.y, card.rect.y + card.rect.height + self.layout_manager.spacing)
|
||||
|
||||
def test_dynamic_repositioning_on_rect_change(self):
|
||||
# Register a widget
|
||||
w1 = DummyLayoutWidget("w1", priority=1, width=100, height=100)
|
||||
|
||||
@@ -85,6 +85,8 @@ MACH_E_PATH_ANGLE_MAX = 0.16
|
||||
MACH_E_PATH_ANGLE_STEP = 0.055
|
||||
MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0
|
||||
MACH_E_PATH_ANGLE_MAX_SPEED = 8.8
|
||||
MACH_E_PATH_ANGLE_TRACKING_FACTOR = 0.75
|
||||
MACH_E_PATH_ANGLE_DRIVER_COOLDOWN = 0.75
|
||||
FORD_CURVATURE_LOOKAHEAD = {
|
||||
CAR.FORD_EXPLORER_MK6: 0.20,
|
||||
}
|
||||
@@ -171,6 +173,7 @@ class FordLateralController:
|
||||
self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.path_angle_driver_cooldown = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
self._frame = 0
|
||||
self._update_params()
|
||||
@@ -232,18 +235,28 @@ class FordLateralController:
|
||||
deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0]))
|
||||
return base + (MACH_E_UNDERSTEER_ERROR_MAX - base) * speed_weight * deficit_weight
|
||||
|
||||
def _path_angle_assist(self, requested: float, desired: float, applied: float, v_ego: float,
|
||||
def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float,
|
||||
steering_pressed: bool, lane_change: bool) -> float:
|
||||
if steering_pressed:
|
||||
self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN
|
||||
else:
|
||||
self.path_angle_driver_cooldown = max(0.0, self.path_angle_driver_cooldown - STEER_DT)
|
||||
target = 0.0
|
||||
if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and
|
||||
not steering_pressed and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
|
||||
not steering_pressed and self.path_angle_driver_cooldown == 0.0 and not lane_change and
|
||||
3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
|
||||
requested * desired > 0.0 and requested * applied > 0.0 and
|
||||
abs(requested) > 0.021 and abs(desired) > 0.021 and abs(applied) >= 0.0195):
|
||||
abs(requested) > 0.0198 and abs(desired) > 0.016 and abs(applied) >= 0.0195 and
|
||||
np.sign(desired) * (desired - current) > 0.002):
|
||||
max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2
|
||||
residual = max(0.0, min(abs(requested), abs(desired), max_curvature) - abs(applied))
|
||||
residual = max(0.0, min(max(abs(requested), abs(desired)), max_curvature) - abs(applied))
|
||||
tracking_deficit = max(0.0, np.sign(applied) * (applied - current))
|
||||
acceleration_headroom = max(0.0, (max_curvature - abs(applied)) * v_ego)
|
||||
speed_weight = float(np.interp(
|
||||
v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0]))
|
||||
target = float(np.sign(applied) * min(residual * v_ego * speed_weight, MACH_E_PATH_ANGLE_MAX))
|
||||
target = float(np.sign(applied) * min(
|
||||
(residual + MACH_E_PATH_ANGLE_TRACKING_FACTOR * tracking_deficit) * v_ego,
|
||||
acceleration_headroom, MACH_E_PATH_ANGLE_MAX) * speed_weight)
|
||||
if target == 0.0 or target * self.path_angle_last < 0.0:
|
||||
self.path_angle_last = 0.0
|
||||
else:
|
||||
@@ -477,11 +490,14 @@ class FordLateralController:
|
||||
self.curvature_samples.clear()
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.path_angle_driver_cooldown = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
return FordLateralResult()
|
||||
|
||||
manual_turn = self._manual_turn(CC, CS, float(actuators.curvature))
|
||||
if manual_turn or CS.out.vEgoRaw < 0.1:
|
||||
if CS.out.steeringPressed:
|
||||
self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN
|
||||
self.curvature_samples.clear()
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
@@ -557,7 +573,7 @@ class FordLateralController:
|
||||
max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2
|
||||
applied = float(np.clip(applied, -max_curvature, max_curvature))
|
||||
path_angle = self._path_angle_assist(
|
||||
requested, desired, applied, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
|
||||
requested, desired, applied, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
|
||||
|
||||
self.curvature_samples.append(predicted)
|
||||
curvature_rate = 0.0
|
||||
|
||||
@@ -120,17 +120,31 @@ def test_understeer_error_preserves_other_fords(controller):
|
||||
def test_mach_e_path_angle_assist_at_curvature_limit(controller):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) for _ in range(3)]
|
||||
outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False) for _ in range(3)]
|
||||
assert outputs == pytest.approx([0.055, 0.110, 0.150])
|
||||
assert controller._path_angle_assist(0.018, 0.018, 0.018, 7.5, False, False) == 0.0
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, False, False) == pytest.approx(-0.055)
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, True, False) == 0.0
|
||||
assert controller._path_angle_assist(0.018, 0.018, 0.018, 0.018, 7.5, False, False) == 0.0
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, False, False) == pytest.approx(-0.055)
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, True, False) == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
def test_mach_e_path_angle_assist_starts_at_saturation_and_releases_after_driver(controller, sign):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
request = (sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.007, 7.0)
|
||||
assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055)
|
||||
assert controller._path_angle_assist(*request, True, False) == 0.0
|
||||
for _ in range(round(0.75 / STEER_DT) - 1):
|
||||
assert controller._path_angle_assist(*request, False, False) == 0.0
|
||||
assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055)
|
||||
assert controller._path_angle_assist(
|
||||
sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.021, 7.0, False, False) == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", (
|
||||
(9.0, 0.04, 0.04, 0.02, False, False),
|
||||
(7.5, 0.020, 0.04, 0.02, False, False),
|
||||
(7.5, 0.04, 0.018, 0.02, False, False),
|
||||
(7.5, 0.0197, 0.04, 0.02, False, False),
|
||||
(7.5, 0.04, 0.015, 0.02, False, False),
|
||||
(7.5, 0.04, 0.04, 0.018, False, False),
|
||||
(7.5, 0.04, 0.04, 0.02, True, False),
|
||||
(7.5, 0.04, 0.04, 0.02, False, True),
|
||||
@@ -138,18 +152,18 @@ def test_mach_e_path_angle_assist_at_curvature_limit(controller):
|
||||
def test_mach_e_path_angle_assist_is_scoped(controller, speed, requested, desired, applied, driver, lane_change):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._path_angle_assist(requested, desired, applied, speed, driver, lane_change) == 0.0
|
||||
assert controller._path_angle_assist(requested, desired, applied, 0.01, speed, driver, lane_change) == 0.0
|
||||
|
||||
|
||||
def test_path_angle_assist_preserves_other_fords(controller):
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) == 0.0
|
||||
assert controller._path_angle_assist(0.04, 0.04, 0.02, 0.01, 7.5, False, False) == 0.0
|
||||
|
||||
|
||||
def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assist = controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False)
|
||||
assist = controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False)
|
||||
packer = CANPacker("ford_lincoln_base_pt")
|
||||
can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()]))
|
||||
_, data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, -0.02, 0.0, 0, -assist)
|
||||
|
||||
@@ -2370,8 +2370,8 @@
|
||||
{
|
||||
"key": "ShowSLCOffset",
|
||||
"label": "Show Speed Limit Offset",
|
||||
"description": "Show the current offset from the posted limit on the driving screen.",
|
||||
"picker_description": "Shows the current offset from the posted limit.",
|
||||
"description": "Show the current offset on the compact driving display. The unified Max Set / Speed Limit card always shows nonzero offsets.",
|
||||
"picker_description": "Shows the offset on the compact display; the unified card always shows nonzero offsets.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle",
|
||||
"parent_key": "SpeedLimitController",
|
||||
@@ -2990,8 +2990,8 @@
|
||||
{
|
||||
"key": "UseVienna",
|
||||
"label": "Use Vienna-Style Speed Signs",
|
||||
"description": "Show Vienna-style (EU) speed-limit signs instead of MUTCD (US).",
|
||||
"picker_description": "Uses Vienna-style speed-limit signs.",
|
||||
"description": "Use Vienna-style (EU) speed-limit signs on the compact driving display. The unified Max Set / Speed Limit card uses its own layout.",
|
||||
"picker_description": "Uses Vienna-style signs on the compact display.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle",
|
||||
"parent_key": "NavigationUI",
|
||||
|
||||
@@ -1461,6 +1461,8 @@ class StarPilotVariables:
|
||||
speed_limit_confirmation = self.get_value("SLCConfirmation", condition=toggle.speed_limit_controller)
|
||||
toggle.speed_limit_confirmation_higher = self.get_value("SLCConfirmationHigher", condition=speed_limit_confirmation)
|
||||
toggle.speed_limit_confirmation_lower = self.get_value("SLCConfirmationLower", condition=speed_limit_confirmation)
|
||||
# Legacy setting is hidden in the current UI. SLC's pedal and +/- overrides
|
||||
# remain available regardless of its saved value; keep loading it for compatibility.
|
||||
slc_override_method = self.get_value("SLCOverride", cast=float, condition=toggle.speed_limit_controller)
|
||||
toggle.speed_limit_controller_override_manual = slc_override_method == 1
|
||||
toggle.speed_limit_controller_override_set_speed = slc_override_method == 2
|
||||
|
||||
@@ -0,0 +1,130 @@
|
||||
import calendar
|
||||
import json
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
import requests
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.starpilot.common.starpilot_utilities import calculate_bearing_offset, is_url_pingable
|
||||
|
||||
|
||||
FREE_MAPBOX_REQUESTS = 100_000
|
||||
|
||||
|
||||
class MapboxSpeedLimit:
|
||||
def __init__(self, params):
|
||||
self.params = params
|
||||
try:
|
||||
self.requests = json.loads(params.get("MapBoxRequests", encoding="utf-8") or "{}")
|
||||
except (TypeError, ValueError):
|
||||
self.requests = {}
|
||||
self.requests.setdefault("total_requests", 0)
|
||||
self.requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - 28 * 100)
|
||||
|
||||
self.host = "https://api.mapbox.com"
|
||||
self.token = params.get("MapboxSecretKey", encoding="utf-8")
|
||||
self.limit = 0.0
|
||||
self.segment_distance = 0.0
|
||||
self.future = None
|
||||
self.executor = ThreadPoolExecutor(max_workers=1)
|
||||
self.session = requests.Session()
|
||||
self.session.headers.update({"Accept-Language": "en"})
|
||||
self.session.headers.update({"User-Agent": "starpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/StarPilot)"})
|
||||
|
||||
def reset(self):
|
||||
# A discarded future may still finish, but only update() can publish its result.
|
||||
if self.future is not None:
|
||||
self.future.cancel()
|
||||
self.future = None
|
||||
self.limit = 0.0
|
||||
self.segment_distance = 0.0
|
||||
|
||||
def shutdown(self):
|
||||
self.reset()
|
||||
self.executor.shutdown(wait=False, cancel_futures=True)
|
||||
self.session.close()
|
||||
|
||||
def _request(self, position, v_ego):
|
||||
if not is_url_pingable(self.host):
|
||||
return 0.0, v_ego
|
||||
|
||||
self.requests["total_requests"] += 1
|
||||
self.params.put_nonblocking("MapBoxRequests", json.dumps(self.requests))
|
||||
|
||||
bearing = position.get("bearing")
|
||||
latitude = position.get("latitude")
|
||||
longitude = position.get("longitude")
|
||||
future_latitude, future_longitude = calculate_bearing_offset(latitude, longitude, bearing, v_ego)
|
||||
url = f"{self.host}/matching/v5/mapbox/driving/{longitude},{latitude};{future_longitude},{future_latitude}.json"
|
||||
params = {
|
||||
"access_token": self.token,
|
||||
"annotations": "maxspeed,distance",
|
||||
"geometries": "polyline6",
|
||||
"overview": "full",
|
||||
"steps": "false",
|
||||
"radiuses": "10;10",
|
||||
"tidy": "true",
|
||||
}
|
||||
response = self.session.get(url, params=params, timeout=10)
|
||||
response.raise_for_status()
|
||||
matchings = response.json().get("matchings") or []
|
||||
if not matchings:
|
||||
return 0.0, v_ego
|
||||
legs = (matchings[0] or {}).get("legs") or []
|
||||
if not legs:
|
||||
return 0.0, v_ego
|
||||
|
||||
annotation = legs[0].get("annotation") or {}
|
||||
distances = annotation.get("distance") or [v_ego]
|
||||
speeds = annotation.get("maxspeed") or []
|
||||
if not speeds:
|
||||
return 0.0, v_ego
|
||||
first = speeds[0]
|
||||
try:
|
||||
speed = float(first.get("speed")) if first.get("speed") != "none" else 0.0
|
||||
except (TypeError, ValueError):
|
||||
speed = 0.0
|
||||
if speed <= 0:
|
||||
return 0.0, v_ego
|
||||
conversion = CV.MPH_TO_MS if first.get("unit", "km/h") == "mph" else CV.KPH_TO_MS
|
||||
return speed * conversion, distances[0]
|
||||
|
||||
def update(self, now, time_validated, v_ego, gps_valid, position, steering_angle, angle_offset):
|
||||
if not gps_valid or not self.token or abs(steering_angle - angle_offset) >= 45:
|
||||
self.reset()
|
||||
return
|
||||
|
||||
if time_validated and now.month != self.requests.get("month"):
|
||||
self.requests.update({
|
||||
"month": now.month,
|
||||
"total_requests": 0,
|
||||
"max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, now.month)[1] * 100,
|
||||
})
|
||||
if self.requests["total_requests"] >= self.requests["max_requests"]:
|
||||
self.reset()
|
||||
return
|
||||
|
||||
if self.future is not None:
|
||||
if not self.future.done():
|
||||
return
|
||||
future = self.future
|
||||
self.future = None
|
||||
try:
|
||||
self.limit, self.segment_distance = future.result()
|
||||
except Exception as exception:
|
||||
print(f"Unexpected error in Mapbox request: {exception}")
|
||||
self.limit, self.segment_distance = 0.0, v_ego
|
||||
return
|
||||
|
||||
if v_ego < 1:
|
||||
return
|
||||
if self.segment_distance > 0:
|
||||
self.segment_distance -= v_ego * DT_MDL
|
||||
return
|
||||
|
||||
try:
|
||||
self.future = self.executor.submit(self._request, dict(position), v_ego)
|
||||
except RuntimeError:
|
||||
self.segment_distance = v_ego
|
||||
return
|
||||
@@ -1,19 +1,20 @@
|
||||
#!/usr/bin/env python3
|
||||
# PFEIFER - SLC - Modified by FrogAi
|
||||
import calendar
|
||||
import json
|
||||
import requests
|
||||
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.starpilot.common.starpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable
|
||||
from openpilot.starpilot.controls.lib.mapbox_speed_limit import MapboxSpeedLimit
|
||||
|
||||
FREE_MAPBOX_REQUESTS = 100_000
|
||||
|
||||
SOURCE_NONE = "None"
|
||||
SOURCE_DASHBOARD = "Dashboard"
|
||||
SOURCE_MAP = "Map Data"
|
||||
SOURCE_VISION = "Vision"
|
||||
SOURCE_MAPBOX = "Mapbox"
|
||||
SOURCE_PREVIOUS_LIMIT = "Previous Limit"
|
||||
REAL_SOURCES = (SOURCE_DASHBOARD, SOURCE_MAP, SOURCE_VISION, SOURCE_MAPBOX)
|
||||
|
||||
OFFSET_MAP_IMPERIAL = [
|
||||
(0, 11.2, "speed_limit_offset1"), # 0–24 mph
|
||||
@@ -36,83 +37,90 @@ OFFSET_MAP_METRIC = [
|
||||
]
|
||||
|
||||
SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75
|
||||
# Minimum set-speed increase (m/s) counted as a deliberate +/- press. Below the smallest
|
||||
# real step (1 km/h ≈ 0.28 m/s), above cluster/float jitter.
|
||||
SET_SPEED_RAISE_EPS = 0.1
|
||||
SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND = 0.1
|
||||
SAME_LIMIT_TOLERANCE = 1.0
|
||||
VISION_LARGE_REFERENCE_SPEED_DELTA = 30 * CV.MPH_TO_MS
|
||||
VISION_LARGE_SET_SPEED_MIN_SUPPORT = 3
|
||||
VISION_SUPPORT_SPEED_TOLERANCE = 0.5 * CV.MPH_TO_MS
|
||||
|
||||
|
||||
class SpeedLimitController:
|
||||
def __init__(self, StarPilotVCruise):
|
||||
self.starpilot_planner = StarPilotVCruise.starpilot_planner
|
||||
self.starpilot_toggles = None
|
||||
self.mapbox = MapboxSpeedLimit(self.starpilot_planner.params)
|
||||
|
||||
self.calling_mapbox = False
|
||||
self.override_slc = False
|
||||
self.override_disable_timer = 0.0
|
||||
self._prev_v_cruise = None
|
||||
self._persistent_override_speed = 0.0
|
||||
self._set_speed_override_input_consumed = False
|
||||
self.source = SOURCE_NONE
|
||||
self.target = 0.0
|
||||
self.map_speed_limit = 0.0
|
||||
self.next_speed_limit = 0.0
|
||||
self.vision_limit = 0.0
|
||||
self.overridden_speed = 0.0
|
||||
|
||||
self.denied_target = 0
|
||||
self.map_speed_limit = 0
|
||||
self.mapbox_limit = 0
|
||||
self.next_speed_limit = 0
|
||||
self.overridden_speed = 0
|
||||
self.segment_distance = 0
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.target = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
self.vision_limit = 0
|
||||
|
||||
self.previous_source = "None"
|
||||
self.source = "None"
|
||||
self.last_valid_limit = max(self.starpilot_planner.params.get_float("PreviousSpeedLimit"), 0.0)
|
||||
self.last_valid_source = SOURCE_NONE # The persisted number has no known live source.
|
||||
self.pending_limit = 0.0
|
||||
self.pending_source = SOURCE_NONE
|
||||
self.confirmation_time = 0.0
|
||||
self.denied_limit = 0.0
|
||||
self.previous_road_name = ""
|
||||
|
||||
self._slc_adopt_counter = 0
|
||||
|
||||
mapbox_requests_raw = self.starpilot_planner.params.get("MapBoxRequests", encoding="utf-8")
|
||||
try:
|
||||
self.mapbox_requests = json.loads(mapbox_requests_raw or "{}")
|
||||
except (TypeError, ValueError):
|
||||
self.mapbox_requests = {}
|
||||
self.mapbox_requests.setdefault("total_requests", 0)
|
||||
self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100))
|
||||
|
||||
self.mapbox_host = "https://api.mapbox.com"
|
||||
self.mapbox_token = self.starpilot_planner.params.get("MapboxSecretKey", encoding="utf-8")
|
||||
|
||||
self.previous_target = self.starpilot_planner.params.get_float("PreviousSpeedLimit")
|
||||
self.last_valid_limit = self.previous_target if self.previous_target > 0 else 0
|
||||
|
||||
self.executor = ThreadPoolExecutor(max_workers=1)
|
||||
self.mapbox_future = None
|
||||
|
||||
self.session = requests.Session()
|
||||
self.session.headers.update({"Accept-Language": "en"})
|
||||
self.session.headers.update({"User-Agent": "starpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/StarPilot)"})
|
||||
self.set_speed_override = 0.0
|
||||
self.pedal_override = 0.0
|
||||
self.previous_set_speed = None
|
||||
self.consume_set_speed_change = False
|
||||
self.override_disable_time = 0.0
|
||||
self.limit_change_started = False
|
||||
self.confirmation_button_consumed = False
|
||||
self._active_control = False
|
||||
self._using_experimental_fallback = False
|
||||
self._using_previous_limit_fallback = False
|
||||
self._mode = "off"
|
||||
|
||||
def shutdown(self):
|
||||
self.executor.shutdown(wait=False, cancel_futures=True)
|
||||
self.session.close()
|
||||
self.mapbox.shutdown()
|
||||
|
||||
@property
|
||||
def mapbox_limit(self):
|
||||
return self.mapbox.limit
|
||||
|
||||
@property
|
||||
def confirmation_pending(self):
|
||||
return self.pending_limit >= 1
|
||||
|
||||
@property
|
||||
def unconfirmed_speed_limit(self):
|
||||
return self.pending_limit
|
||||
|
||||
@property
|
||||
def presented_source(self):
|
||||
if self.confirmation_pending:
|
||||
return self.pending_source
|
||||
if self.source in REAL_SOURCES:
|
||||
return self.source
|
||||
if self._using_previous_limit_fallback and self.target >= 1:
|
||||
return self.last_valid_source if self.last_valid_source in REAL_SOURCES else SOURCE_PREVIOUS_LIMIT
|
||||
if (self.denied_limit > 0 and self.last_valid_limit > 0 and self.target >= 1 and
|
||||
abs(self.target - self.last_valid_limit) < SAME_LIMIT_TOLERANCE):
|
||||
return self.last_valid_source if self.last_valid_source in REAL_SOURCES else SOURCE_PREVIOUS_LIMIT
|
||||
return SOURCE_NONE
|
||||
|
||||
@property
|
||||
def experimental_mode(self):
|
||||
return self.target == 0 and bool(getattr(self.starpilot_toggles, "slc_fallback_experimental_mode", False))
|
||||
return self._active_control and self._using_experimental_fallback
|
||||
|
||||
@property
|
||||
def target_to_use(self):
|
||||
if self.source == "None" and self.target > 0 and self.last_valid_limit > 0:
|
||||
if self.target >= self.last_valid_limit:
|
||||
return self.last_valid_limit
|
||||
# Keep Set Speed fallback from arming an override against a higher fake limit.
|
||||
if self.source == SOURCE_NONE and self.target > 0 and self.last_valid_limit > 0:
|
||||
return min(self.target, self.last_valid_limit)
|
||||
return self.target
|
||||
|
||||
def get_offset(self, target_speed):
|
||||
def get_offset(self, limit):
|
||||
if self.starpilot_toggles is None:
|
||||
return 0
|
||||
return 0.0
|
||||
offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL
|
||||
return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low <= target_speed < high), 0)
|
||||
return next((getattr(self.starpilot_toggles, name) for low, high, name in offset_map if low <= limit < high), 0.0)
|
||||
|
||||
@property
|
||||
def offset(self):
|
||||
@@ -124,458 +132,303 @@ class SpeedLimitController:
|
||||
0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0)
|
||||
)
|
||||
|
||||
def _confirmation_required(self, desired_source, desired_target):
|
||||
return 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)
|
||||
)
|
||||
def reset_control_state(self):
|
||||
self._clear_pending()
|
||||
self.clear_override()
|
||||
self.previous_set_speed = None
|
||||
self.consume_set_speed_change = False
|
||||
self.override_disable_time = 0.0
|
||||
self.limit_change_started = False
|
||||
self.confirmation_button_consumed = False
|
||||
self._active_control = False
|
||||
self._using_experimental_fallback = False
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
|
||||
|
||||
def clear_override(self):
|
||||
self.override_slc = False
|
||||
self.overridden_speed = 0
|
||||
self._persistent_override_speed = 0.0
|
||||
def _get_vision_limit(self, v_ego, sm, display_only):
|
||||
enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False)
|
||||
self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if enabled else 0.0
|
||||
limit = self.vision_limit
|
||||
if not display_only and self.low_vision_limit_filtered(limit):
|
||||
return 0.0
|
||||
|
||||
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):
|
||||
if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = 0
|
||||
return
|
||||
|
||||
if v_ego < 1:
|
||||
return
|
||||
|
||||
if self.segment_distance > 0:
|
||||
self.segment_distance -= v_ego * DT_MDL
|
||||
return
|
||||
|
||||
if self.calling_mapbox:
|
||||
self.segment_distance = v_ego
|
||||
return
|
||||
|
||||
def make_request():
|
||||
successful = False
|
||||
response_data = None
|
||||
try:
|
||||
if not is_url_pingable(self.mapbox_host):
|
||||
self.segment_distance = 1000
|
||||
successful = True
|
||||
return None
|
||||
|
||||
if time_validated:
|
||||
current_month = now.month
|
||||
if current_month != self.mapbox_requests.get("month"):
|
||||
self.mapbox_requests.update({
|
||||
"month": current_month,
|
||||
"total_requests": 0,
|
||||
"max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100,
|
||||
})
|
||||
|
||||
self.mapbox_requests["total_requests"] += 1
|
||||
self.starpilot_planner.params.put_nonblocking("MapBoxRequests", json.dumps(self.mapbox_requests))
|
||||
|
||||
current_bearing = self.starpilot_planner.gps_position.get("bearing")
|
||||
current_latitude = self.starpilot_planner.gps_position.get("latitude")
|
||||
current_longitude = self.starpilot_planner.gps_position.get("longitude")
|
||||
|
||||
future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego)
|
||||
|
||||
url = (
|
||||
f"{self.mapbox_host}/matching/v5/mapbox/driving/"
|
||||
f"{current_longitude},{current_latitude};"
|
||||
f"{future_longitude},{future_latitude}.json"
|
||||
)
|
||||
|
||||
mapbox_params = {
|
||||
"access_token": self.mapbox_token,
|
||||
"annotations": "maxspeed,distance",
|
||||
"geometries": "polyline6",
|
||||
"overview": "full",
|
||||
"steps": "false",
|
||||
"radiuses": "10;10",
|
||||
"tidy": "true",
|
||||
}
|
||||
|
||||
response = self.session.get(url, params=mapbox_params, timeout=10)
|
||||
response.raise_for_status()
|
||||
|
||||
successful = True
|
||||
response_data = response.json()
|
||||
except Exception as exception:
|
||||
print(f"Unexpected error in Mapbox request: {exception}")
|
||||
finally:
|
||||
self.calling_mapbox = False
|
||||
|
||||
if not successful:
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = v_ego
|
||||
return response_data
|
||||
|
||||
def complete_request(future):
|
||||
try:
|
||||
data = future.result()
|
||||
if data:
|
||||
matchings = data.get("matchings") or []
|
||||
if not matchings:
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = v_ego
|
||||
return
|
||||
|
||||
legs = (matchings[0] or {}).get("legs") or []
|
||||
if not legs:
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = v_ego
|
||||
return
|
||||
|
||||
annotation = legs[0].get("annotation") or {}
|
||||
|
||||
distances = annotation.get("distance") or [v_ego]
|
||||
segment_distance = distances[0]
|
||||
|
||||
speed_data = annotation.get("maxspeed", [])
|
||||
if speed_data:
|
||||
first_segment_speed = speed_data[0]
|
||||
try:
|
||||
raw_speed = float(first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0.0)
|
||||
except (ValueError, TypeError):
|
||||
raw_speed = 0.0
|
||||
unit = first_segment_speed.get("unit", "km/h")
|
||||
if raw_speed > 0:
|
||||
if unit == "mph":
|
||||
self.mapbox_limit = raw_speed * CV.MPH_TO_MS
|
||||
else:
|
||||
self.mapbox_limit = raw_speed * CV.KPH_TO_MS
|
||||
self.segment_distance = segment_distance
|
||||
return
|
||||
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = v_ego
|
||||
|
||||
except Exception as exception:
|
||||
print(f"Mapbox Callback Error: {exception}")
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = v_ego
|
||||
finally:
|
||||
self.mapbox_future = None
|
||||
|
||||
self.calling_mapbox = True
|
||||
try:
|
||||
future = self.executor.submit(make_request)
|
||||
except RuntimeError:
|
||||
self.calling_mapbox = False
|
||||
self.segment_distance = v_ego
|
||||
return
|
||||
|
||||
self.mapbox_future = future
|
||||
future.add_done_callback(complete_request)
|
||||
|
||||
def handle_limit_change(self, desired_source, desired_target, current_road_name, v_ego, sm):
|
||||
self.speed_limit_changed_timer += DT_MDL
|
||||
previous_limit = self.last_valid_limit if self.last_valid_limit > 0 else self.target
|
||||
|
||||
long_active = sm["carControl"].longActive
|
||||
accepted_by_accel_button = sm["starpilotCarState"].accelPressed and long_active
|
||||
confirmation_required = self._confirmation_required(desired_source, desired_target)
|
||||
higher_confirmation = confirmation_required and desired_target > self.target
|
||||
speed_limit_accepted = confirmation_required and accepted_by_accel_button
|
||||
if confirmation_required and not speed_limit_accepted and self._slc_adopt_counter % 4 == 0:
|
||||
speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted")
|
||||
if not confirmation_required:
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
self.unconfirmed_speed_limit = 0
|
||||
speed_limit_denied = confirmation_required and (
|
||||
sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active)
|
||||
)
|
||||
|
||||
if not long_active and not sm["selfdriveState"].enabled:
|
||||
speed_limit_accepted = True
|
||||
|
||||
if speed_limit_accepted:
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||
set_speed_kph = float(sm["carState"].vCruise)
|
||||
target_with_offset = self.target + self.offset
|
||||
if (
|
||||
higher_confirmation
|
||||
and long_active
|
||||
and 0 < set_speed_kph < V_CRUISE_UNSET
|
||||
and set_speed_kph * CV.KPH_TO_MS < target_with_offset
|
||||
):
|
||||
self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", target_with_offset)
|
||||
if accepted_by_accel_button and confirmation_required:
|
||||
self._set_speed_override_input_consumed = True
|
||||
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
|
||||
elif speed_limit_denied:
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
self.denied_target = desired_target
|
||||
|
||||
self.previous_source = desired_source
|
||||
self.previous_target = desired_target
|
||||
self.previous_road_name = current_road_name
|
||||
|
||||
elif desired_target != self.target and not confirmation_required:
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||
|
||||
elif desired_target == self.target:
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
|
||||
else:
|
||||
self.source = "None"
|
||||
self.unconfirmed_speed_limit = desired_target
|
||||
|
||||
if (self.target != self.previous_target or self.previous_road_name != current_road_name) and self.target > 0 and not speed_limit_denied:
|
||||
self.denied_target = 0
|
||||
|
||||
self.previous_source = self.source
|
||||
self.previous_target = self.target
|
||||
self.previous_road_name = current_road_name
|
||||
|
||||
self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
|
||||
|
||||
def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False):
|
||||
self.update_map_speed_limit(v_ego, sm)
|
||||
vision_enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False)
|
||||
self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0
|
||||
usable_vision_limit = self.vision_limit
|
||||
if not display_only and self.low_vision_limit_filtered(usable_vision_limit):
|
||||
usable_vision_limit = 0
|
||||
# The planner clamps V_CRUISE_UNSET to V_CRUISE_MAX, so plausibility must use the raw selected speed.
|
||||
raw_set_speed_kph = float(sm["carState"].vCruise)
|
||||
selected_set_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0
|
||||
reference_speed = selected_set_speed if selected_set_speed > 0 else max(float(v_ego), 0)
|
||||
# vEgo jitters around zero at standstill; do not let that switch the active source.
|
||||
if (
|
||||
usable_vision_limit > 0 and reference_speed > 0 and not sm["carState"].standstill and
|
||||
abs(usable_vision_limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA
|
||||
):
|
||||
support_count = self.starpilot_planner.params_memory.get_int("VisionSpeedLimitSupportCount")
|
||||
support_speed = self.starpilot_planner.params_memory.get_float("VisionSpeedLimitSupportSpeed")
|
||||
if support_count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - usable_vision_limit) > VISION_SUPPORT_SPEED_TOLERANCE:
|
||||
usable_vision_limit = 0
|
||||
selected_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0.0
|
||||
reference_speed = selected_speed if selected_speed > 0 else max(float(v_ego), 0.0)
|
||||
if (limit > 0 and reference_speed > 0 and not sm["carState"].standstill and
|
||||
abs(limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA):
|
||||
memory = self.starpilot_planner.params_memory
|
||||
count = memory.get_int("VisionSpeedLimitSupportCount")
|
||||
support_speed = memory.get_float("VisionSpeedLimitSupportSpeed")
|
||||
if count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - limit) > VISION_SUPPORT_SPEED_TOLERANCE:
|
||||
return 0.0
|
||||
return limit
|
||||
|
||||
configured_priorities = {
|
||||
self.starpilot_toggles.speed_limit_priority1,
|
||||
self.starpilot_toggles.speed_limit_priority2,
|
||||
}
|
||||
limits = {
|
||||
"Dashboard": dashboard_speed_limit,
|
||||
"Map Data": self.map_speed_limit,
|
||||
}
|
||||
if "Vision" in configured_priorities:
|
||||
limits["Vision"] = usable_vision_limit
|
||||
filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1}
|
||||
|
||||
if self.starpilot_toggles.speed_limit_priority_highest:
|
||||
desired_source = max(filtered_limits, key=filtered_limits.get, default="None")
|
||||
desired_target = filtered_limits.get(desired_source, 0)
|
||||
|
||||
elif self.starpilot_toggles.speed_limit_priority_lowest:
|
||||
desired_source = min(filtered_limits, key=filtered_limits.get, default="None")
|
||||
desired_target = filtered_limits.get(desired_source, 0)
|
||||
|
||||
elif filtered_limits:
|
||||
for priority in [
|
||||
self.starpilot_toggles.speed_limit_priority1,
|
||||
self.starpilot_toggles.speed_limit_priority2
|
||||
]:
|
||||
if priority in filtered_limits:
|
||||
desired_source = priority
|
||||
desired_target = filtered_limits[desired_source]
|
||||
break
|
||||
else:
|
||||
desired_source = "None"
|
||||
desired_target = 0
|
||||
|
||||
else:
|
||||
desired_source = "None"
|
||||
desired_target = 0
|
||||
|
||||
if desired_target == 0:
|
||||
if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.starpilot_toggles.slc_mapbox_filler:
|
||||
self.get_mapbox_speed_limit(now, time_validated, v_ego, sm)
|
||||
|
||||
if self.mapbox_limit >= 1:
|
||||
desired_source = "Mapbox"
|
||||
desired_target = self.mapbox_limit
|
||||
|
||||
if not display_only and desired_target == 0:
|
||||
previous_vision_limit_filtered = self.previous_source == "Vision" and self.low_vision_limit_filtered(self.previous_target)
|
||||
if self.previous_target > 0 and self.starpilot_toggles.slc_fallback_previous_speed_limit and not previous_vision_limit_filtered:
|
||||
desired_source = self.previous_source
|
||||
desired_target = self.previous_target
|
||||
|
||||
self.target = desired_target
|
||||
|
||||
elif sm["selfdriveState"].enabled and self.starpilot_toggles.slc_fallback_set_speed:
|
||||
desired_source = "None"
|
||||
desired_target = v_cruise
|
||||
else:
|
||||
self.mapbox_limit = 0
|
||||
self.segment_distance = 0
|
||||
|
||||
if display_only:
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
self.clear_override()
|
||||
|
||||
if desired_target >= 1:
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
else:
|
||||
self.source = "None"
|
||||
self.target = 0
|
||||
|
||||
return
|
||||
|
||||
current_road_name = sm["mapdOut"].roadName if desired_source == "Map Data" else ""
|
||||
current_speed = self.target if (self.source != "None" and self.target > 0) else self.last_valid_limit
|
||||
|
||||
# Do not trigger alerts when shifting to fallback or when re-obtaining the same speed limit
|
||||
is_fallback = desired_source == "None" or desired_target == 0
|
||||
same_speed = desired_target > 0 and current_speed > 0 and abs(desired_target - current_speed) < 1
|
||||
confirmation_required = self._confirmation_required(desired_source, desired_target)
|
||||
denied_same_limit = (
|
||||
confirmation_required and self.denied_target > 0 and
|
||||
abs(desired_target - self.denied_target) < 1
|
||||
)
|
||||
|
||||
if not denied_same_limit:
|
||||
self.denied_target = 0
|
||||
|
||||
if denied_same_limit:
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
elif not is_fallback and not same_speed and (abs(desired_target - self.previous_target) >= 1 or current_speed == 0):
|
||||
self.handle_limit_change(desired_source, desired_target, current_road_name, v_ego, sm)
|
||||
else:
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
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.target = desired_target
|
||||
if desired_source != "None" and desired_target > 0:
|
||||
self.previous_source = desired_source
|
||||
self.previous_target = desired_target
|
||||
if current_road_name != self.previous_road_name and current_road_name != "":
|
||||
self.previous_road_name = current_road_name
|
||||
self.denied_target = 0
|
||||
|
||||
if self.source != "None" and self.target > 0:
|
||||
self.last_valid_limit = self.target
|
||||
|
||||
self._slc_adopt_counter += 1
|
||||
if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
|
||||
self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
|
||||
if desired_target > 0:
|
||||
self.clear_override()
|
||||
self.denied_target = 0
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
self.previous_source = desired_source
|
||||
self.previous_target = desired_target
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
|
||||
self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset)
|
||||
|
||||
def update_map_speed_limit(self, v_ego, sm):
|
||||
next_speed_limit_distance = sm["mapdOut"].nextSpeedLimitDistance
|
||||
|
||||
way_sel = sm["mapdOut"].waySelectionType
|
||||
if way_sel in (custom.WaySelectionType.current,
|
||||
custom.WaySelectionType.extended):
|
||||
self.map_speed_limit = sm["mapdOut"].speedLimit
|
||||
self.next_speed_limit = sm["mapdOut"].nextSpeedLimit
|
||||
elif way_sel in (custom.WaySelectionType.predicted,
|
||||
custom.WaySelectionType.possible):
|
||||
speed = sm["mapdOut"].speedLimit
|
||||
def _update_map_speed_limit(self, v_ego, sm):
|
||||
map_data = sm["mapdOut"]
|
||||
way_sel = map_data.waySelectionType
|
||||
if way_sel in (custom.WaySelectionType.current, custom.WaySelectionType.extended):
|
||||
self.map_speed_limit = map_data.speedLimit
|
||||
self.next_speed_limit = map_data.nextSpeedLimit
|
||||
elif way_sel in (custom.WaySelectionType.predicted, custom.WaySelectionType.possible):
|
||||
speed = map_data.speedLimit
|
||||
if speed > 0 and (self.map_speed_limit == 0 or speed < self.map_speed_limit):
|
||||
self.map_speed_limit = speed
|
||||
self.next_speed_limit = 0
|
||||
self.next_speed_limit = 0.0
|
||||
else:
|
||||
self.next_speed_limit = 0
|
||||
# Explicit selection failure means the old current limit is no longer live.
|
||||
self.map_speed_limit = 0.0
|
||||
self.next_speed_limit = 0.0
|
||||
|
||||
if self.next_speed_limit > 0:
|
||||
if self.map_speed_limit < self.next_speed_limit:
|
||||
max_lookahead = self.starpilot_toggles.map_speed_lookahead_higher * v_ego
|
||||
lookahead = self.starpilot_toggles.map_speed_lookahead_higher * v_ego
|
||||
elif self.map_speed_limit > self.next_speed_limit:
|
||||
max_lookahead = self.starpilot_toggles.map_speed_lookahead_lower * v_ego
|
||||
lookahead = self.starpilot_toggles.map_speed_lookahead_lower * v_ego
|
||||
else:
|
||||
max_lookahead = 0
|
||||
|
||||
if next_speed_limit_distance < max_lookahead:
|
||||
lookahead = 0.0
|
||||
if map_data.nextSpeedLimitDistance < lookahead:
|
||||
self.map_speed_limit = self.next_speed_limit
|
||||
|
||||
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). A fresh edge
|
||||
# keeps a cleared override from re-arming while the selected speed stays high.
|
||||
prev_v_cruise = self._prev_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_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
|
||||
def _select_limit(self, dashboard, map_limit, vision_limit):
|
||||
priorities = (self.starpilot_toggles.speed_limit_priority1, self.starpilot_toggles.speed_limit_priority2)
|
||||
limits = {SOURCE_DASHBOARD: dashboard, SOURCE_MAP: map_limit}
|
||||
if SOURCE_VISION in priorities:
|
||||
limits[SOURCE_VISION] = vision_limit
|
||||
valid = {source: limit for source, limit in limits.items() if limit >= 1}
|
||||
if not valid:
|
||||
return SOURCE_NONE, 0.0
|
||||
if self.starpilot_toggles.speed_limit_priority_highest:
|
||||
source = max(valid, key=valid.get)
|
||||
elif self.starpilot_toggles.speed_limit_priority_lowest:
|
||||
source = min(valid, key=valid.get)
|
||||
else:
|
||||
source = next((name for name in priorities if name in valid), SOURCE_NONE)
|
||||
return source, valid.get(source, 0.0)
|
||||
|
||||
if not sm["selfdriveState"].enabled:
|
||||
self.override_disable_timer += DT_MDL
|
||||
if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
|
||||
self.clear_override()
|
||||
def _apply_mapbox_filler(self, source, limit, now, time_validated, v_ego, sm):
|
||||
if source != SOURCE_NONE or not self.starpilot_toggles.slc_mapbox_filler:
|
||||
self.mapbox.reset()
|
||||
return source, limit
|
||||
self.mapbox.update(
|
||||
now, time_validated, v_ego, self.starpilot_planner.gps_valid, self.starpilot_planner.gps_position,
|
||||
sm["carState"].steeringAngleDeg, sm["liveParameters"].angleOffsetDeg,
|
||||
)
|
||||
if self.mapbox.limit >= 1:
|
||||
return SOURCE_MAPBOX, self.mapbox.limit
|
||||
return source, limit
|
||||
|
||||
def _apply_fallback(self, v_cruise, enabled):
|
||||
self._using_experimental_fallback = False
|
||||
self._using_previous_limit_fallback = False
|
||||
previous_vision_filtered = self.last_valid_source == SOURCE_VISION and self.low_vision_limit_filtered(self.last_valid_limit)
|
||||
if self.starpilot_toggles.slc_fallback_previous_speed_limit and self.last_valid_limit > 0 and not previous_vision_filtered:
|
||||
self.source = self.last_valid_source
|
||||
self.target = self.last_valid_limit
|
||||
self._using_previous_limit_fallback = True
|
||||
elif enabled and self.starpilot_toggles.slc_fallback_set_speed:
|
||||
self.source = SOURCE_NONE
|
||||
self.target = v_cruise
|
||||
else:
|
||||
self.source = SOURCE_NONE
|
||||
self.target = 0.0
|
||||
self._using_experimental_fallback = bool(self.starpilot_toggles.slc_fallback_experimental_mode)
|
||||
|
||||
def _confirmation_required(self, limit):
|
||||
current = self.last_valid_limit
|
||||
return ((limit < current and self.starpilot_toggles.speed_limit_confirmation_lower) or
|
||||
(limit > current and self.starpilot_toggles.speed_limit_confirmation_higher))
|
||||
|
||||
def _clear_pending(self):
|
||||
self.pending_limit = 0.0
|
||||
self.pending_source = SOURCE_NONE
|
||||
self.confirmation_time = 0.0
|
||||
|
||||
def _reconcile_set_speed_override(self, old_limit, new_limit):
|
||||
if self.set_speed_override <= 0 or old_limit <= 0 or new_limit <= 0 or abs(new_limit - old_limit) < 0.1:
|
||||
return
|
||||
if (new_limit < old_limit or
|
||||
self.set_speed_override <= new_limit + self.get_offset(new_limit) + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND):
|
||||
self.set_speed_override = 0.0
|
||||
self.overridden_speed = self.pedal_override
|
||||
|
||||
def _accept_limit(self, source, limit, *, persist=True):
|
||||
assert source in REAL_SOURCES and limit >= 1
|
||||
old_limit = self.last_valid_limit
|
||||
self._reconcile_set_speed_override(old_limit, limit)
|
||||
self.source = source
|
||||
self.target = limit
|
||||
self.last_valid_limit = limit
|
||||
self.last_valid_source = source
|
||||
self.denied_limit = 0.0
|
||||
self._clear_pending()
|
||||
if persist and abs(limit - old_limit) >= 0.1:
|
||||
self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(limit))
|
||||
|
||||
def _reject_limit(self, limit):
|
||||
self.denied_limit = limit
|
||||
self.source = SOURCE_NONE
|
||||
self._clear_pending()
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
|
||||
def _update_limit(self, source, limit, sm):
|
||||
road_name = sm["mapdOut"].roadName if source == SOURCE_MAP else ""
|
||||
if road_name and road_name != self.previous_road_name:
|
||||
self.denied_limit = 0.0
|
||||
self.previous_road_name = road_name
|
||||
|
||||
if source == SOURCE_NONE:
|
||||
self._clear_pending()
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
return
|
||||
|
||||
self.override_disable_timer = 0.0
|
||||
current = self.last_valid_limit
|
||||
if current > 0 and abs(limit - current) < SAME_LIMIT_TOLERANCE:
|
||||
self._accept_limit(source, limit, persist=False)
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
return
|
||||
|
||||
target_to_use = self.target_to_use
|
||||
target_with_offset = target_to_use + self.get_offset(target_to_use)
|
||||
confirmation_required = self._confirmation_required(limit)
|
||||
if self.denied_limit > 0 and abs(limit - self.denied_limit) < SAME_LIMIT_TOLERANCE:
|
||||
if confirmation_required:
|
||||
self._clear_pending()
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
self.source = SOURCE_NONE
|
||||
return
|
||||
# Turning confirmation off applies the already-announced candidate.
|
||||
self._accept_limit(source, limit)
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
return
|
||||
self.denied_limit = 0.0
|
||||
|
||||
set_speed = v_cruise + v_cruise_diff
|
||||
bidirectional_set_speed = getattr(self.starpilot_toggles, "redneck_cruise", False)
|
||||
|
||||
if self._persistent_override_speed > 0:
|
||||
if bidirectional_set_speed:
|
||||
if set_speed <= 0:
|
||||
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
|
||||
if self.pending_limit == 0 or abs(limit - self.pending_limit) >= SAME_LIMIT_TOLERANCE:
|
||||
self.pending_limit = limit
|
||||
self.pending_source = source
|
||||
self.confirmation_time = 0.0
|
||||
self.limit_change_started = True
|
||||
new_pending = True
|
||||
else:
|
||||
self.clear_override()
|
||||
self.pending_source = source
|
||||
new_pending = False
|
||||
|
||||
if not confirmation_required:
|
||||
self._accept_limit(source, limit)
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
return
|
||||
|
||||
self.source = SOURCE_NONE
|
||||
self.confirmation_time += DT_MDL
|
||||
long_active = sm["carControl"].longActive
|
||||
accel_accept = bool(sm["starpilotCarState"].accelPressed and long_active)
|
||||
if new_pending and accel_accept:
|
||||
# This press cannot confirm a candidate that was replaced on this frame.
|
||||
self.confirmation_button_consumed = True
|
||||
self.consume_set_speed_change = True
|
||||
memory = self.starpilot_planner.params_memory
|
||||
ui_accept = memory.get_bool("SpeedLimitAccepted")
|
||||
if ui_accept:
|
||||
memory.remove("SpeedLimitAccepted")
|
||||
|
||||
fully_disengaged = not long_active and not sm["selfdriveState"].enabled
|
||||
if ((accel_accept or ui_accept) and not new_pending) or fully_disengaged:
|
||||
pending_limit, pending_source = self.pending_limit, self.pending_source
|
||||
higher = pending_limit > current
|
||||
self._accept_limit(pending_source, pending_limit)
|
||||
if accel_accept:
|
||||
self.consume_set_speed_change = True
|
||||
self.confirmation_button_consumed = True
|
||||
set_speed_kph = float(sm["carState"].vCruise)
|
||||
target_with_offset = self.target + self.offset
|
||||
if (higher and long_active and 0 < set_speed_kph < V_CRUISE_UNSET and
|
||||
set_speed_kph * CV.KPH_TO_MS < target_with_offset):
|
||||
memory.put_float("SLCForceCruiseSpeed", target_with_offset)
|
||||
elif sm["starpilotCarState"].decelPressed or (self.confirmation_time >= 30 and long_active):
|
||||
self._reject_limit(self.pending_limit)
|
||||
|
||||
def _process_adopt_request(self, source, limit):
|
||||
memory = self.starpilot_planner.params_memory
|
||||
if not memory.get_bool("SLCAdoptSpeedLimit"):
|
||||
return
|
||||
memory.remove("SLCAdoptSpeedLimit")
|
||||
if source not in REAL_SOURCES or limit < 1:
|
||||
return
|
||||
self.clear_override()
|
||||
self.consume_set_speed_change = True
|
||||
self._accept_limit(source, limit)
|
||||
memory.put_float("SLCForceCruiseSpeed", self.target + self.offset)
|
||||
|
||||
def clear_override(self):
|
||||
self.set_speed_override = 0.0
|
||||
self.pedal_override = 0.0
|
||||
self.overridden_speed = 0.0
|
||||
|
||||
def _update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
|
||||
previous = self.previous_set_speed
|
||||
self.previous_set_speed = v_cruise
|
||||
changed = previous is not None and abs(v_cruise - previous) > SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND
|
||||
raised = previous is not None and v_cruise > previous + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND
|
||||
consumed = self.consume_set_speed_change
|
||||
if consumed and (changed or not sm["starpilotCarState"].accelPressed):
|
||||
self.consume_set_speed_change = False
|
||||
|
||||
if not sm["selfdriveState"].enabled:
|
||||
self.override_disable_time += DT_MDL
|
||||
if self.override_disable_time >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
|
||||
self.clear_override()
|
||||
return
|
||||
self.override_disable_time = 0.0
|
||||
|
||||
target = self.target_to_use
|
||||
target_with_offset = target + self.get_offset(target)
|
||||
set_speed = v_cruise + v_cruise_diff
|
||||
bidirectional = getattr(self.starpilot_toggles, "redneck_cruise", False)
|
||||
if self.set_speed_override > 0:
|
||||
if bidirectional:
|
||||
self.set_speed_override = max(set_speed, 0.0)
|
||||
elif set_speed <= 0 or (target_with_offset > 0 and set_speed <= target_with_offset and
|
||||
(self.source != SOURCE_NONE or changed)):
|
||||
self.set_speed_override = 0.0
|
||||
else:
|
||||
self.set_speed_override = set_speed
|
||||
elif (target_with_offset > 0 and set_speed > 0 and not consumed and
|
||||
((bidirectional and changed) or (not bidirectional and raised and set_speed > target_with_offset))):
|
||||
self.set_speed_override = set_speed
|
||||
|
||||
self.pedal_override = v_ego + v_ego_diff if sm["carState"].gasPressed and v_ego > target_with_offset > 0 else 0.0
|
||||
self.overridden_speed = self.pedal_override or self.set_speed_override
|
||||
|
||||
def update(self, dashboard_speed_limit, now, time_validated, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm,
|
||||
*, active=True, display_only=False):
|
||||
self.limit_change_started = False
|
||||
self.confirmation_button_consumed = False
|
||||
self._using_experimental_fallback = False
|
||||
self._using_previous_limit_fallback = False
|
||||
mode = "display" if display_only else "active" if active else "off"
|
||||
if mode != self._mode:
|
||||
self.mapbox.reset()
|
||||
self._mode = mode
|
||||
if not active and not display_only:
|
||||
self.reset_control_state()
|
||||
self.mapbox.reset()
|
||||
self.source, self.target = SOURCE_NONE, 0.0
|
||||
self.map_speed_limit = self.next_speed_limit = self.vision_limit = 0.0
|
||||
return
|
||||
|
||||
self._update_map_speed_limit(v_ego, sm)
|
||||
vision_limit = self._get_vision_limit(v_ego, sm, display_only)
|
||||
source, limit = self._select_limit(dashboard_speed_limit, self.map_speed_limit, vision_limit)
|
||||
source, limit = self._apply_mapbox_filler(source, limit, now, time_validated, v_ego, sm)
|
||||
|
||||
if display_only:
|
||||
self.reset_control_state()
|
||||
self.source, self.target = (source, limit) if limit >= 1 else (SOURCE_NONE, 0.0)
|
||||
return
|
||||
|
||||
self._active_control = True
|
||||
if source == SOURCE_NONE:
|
||||
self._update_limit(source, limit, sm)
|
||||
self._apply_fallback(v_cruise, sm["selfdriveState"].enabled)
|
||||
else:
|
||||
self._update_limit(source, limit, sm)
|
||||
self._process_adopt_request(source, limit)
|
||||
self._update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm)
|
||||
|
||||
@@ -216,7 +216,6 @@ class StarPilotAcceleration:
|
||||
|
||||
effective_slc_target = get_active_slc_control_target(
|
||||
getattr(starpilot_toggles, "speed_limit_controller", False),
|
||||
getattr(starpilot_toggles, "set_speed_limit", False),
|
||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 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),
|
||||
@@ -271,7 +270,6 @@ class StarPilotAcceleration:
|
||||
v_ego_diff = v_ego_cluster - v_ego
|
||||
effective_slc_target = get_active_slc_control_target(
|
||||
getattr(starpilot_toggles, "speed_limit_controller", False),
|
||||
getattr(starpilot_toggles, "set_speed_limit", False),
|
||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 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),
|
||||
@@ -312,7 +310,6 @@ class StarPilotAcceleration:
|
||||
v_ego_cluster = v_ego
|
||||
effective_slc_target = get_active_slc_control_target(
|
||||
getattr(starpilot_toggles, "speed_limit_controller", False),
|
||||
getattr(starpilot_toggles, "set_speed_limit", False),
|
||||
getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 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),
|
||||
|
||||
@@ -189,7 +189,7 @@ class StarPilotEvents:
|
||||
else:
|
||||
self.events.add(StarPilotEventName.openpilotCrashed)
|
||||
|
||||
if self.starpilot_planner.starpilot_vcruise.slc.speed_limit_changed_timer == DT_MDL and starpilot_toggles.speed_limit_changed_alert:
|
||||
if self.starpilot_planner.starpilot_vcruise.slc.limit_change_started and starpilot_toggles.speed_limit_changed_alert:
|
||||
self.events.add(StarPilotEventName.speedLimitChanged)
|
||||
|
||||
self.startup_seen |= sm["starpilotSelfdriveState"].alertText1 == starpilot_toggles.startup_alert_top and sm["starpilotSelfdriveState"].alertText2 == starpilot_toggles.startup_alert_bottom
|
||||
|
||||
@@ -105,10 +105,9 @@ def get_lead_veto_distance(car_params):
|
||||
return LEAD_VETO_M_OVERRIDES.get(fingerprint, LEAD_VETO_M)
|
||||
|
||||
|
||||
def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed,
|
||||
def get_active_slc_control_target(speed_limit_controller, slc_target, slc_offset, overridden_speed,
|
||||
v_ego_diff, allow_lower_override=False):
|
||||
# `SetSpeedLimit` only controls engage-time set-speed initialization. Ongoing
|
||||
# SLC speed matching must remain active whenever Speed Limit Controller is on.
|
||||
# SetSpeedLimit controls engage-time initialization; SLC limits ongoing cruise.
|
||||
if not speed_limit_controller:
|
||||
return 0.0
|
||||
|
||||
@@ -209,6 +208,7 @@ class StarPilotVCruise:
|
||||
self._nav_instruction_state_raw = None
|
||||
self._nav_instruction_state = {}
|
||||
self._applied_slc_control_target = 0.0
|
||||
self.slc_is_limiting_max_set = False
|
||||
self.csc_controlling_speed = False
|
||||
self.csc_glow_release_timer = 0.0
|
||||
self.csc_override = False
|
||||
@@ -343,6 +343,7 @@ class StarPilotVCruise:
|
||||
# ===== Main update =====
|
||||
|
||||
def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles):
|
||||
self.slc_is_limiting_max_set = False
|
||||
if not controls_enabled or not getattr(starpilot_toggles, "speed_limit_controller", False):
|
||||
self._applied_slc_control_target = 0.0
|
||||
|
||||
@@ -568,6 +569,18 @@ class StarPilotVCruise:
|
||||
v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
|
||||
v_ego_diff = v_ego_cluster - v_ego
|
||||
|
||||
# Resolve this frame's SLC confirmation before CSC can consume accel/+.
|
||||
self.slc.starpilot_toggles = starpilot_toggles
|
||||
slc_active = starpilot_toggles.speed_limit_controller
|
||||
slc_display_only = not slc_active and starpilot_toggles.show_speed_limits
|
||||
self.slc.update(
|
||||
sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated,
|
||||
v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm,
|
||||
active=slc_active, display_only=slc_display_only,
|
||||
)
|
||||
self.slc_offset = self.slc.offset if slc_active else 0
|
||||
self.slc_target = self.slc.target if (slc_active or slc_display_only) else 0
|
||||
|
||||
# Curve Speed Controller
|
||||
following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False))
|
||||
manual_speed_control = is_manual_speed_control(sm)
|
||||
@@ -586,8 +599,9 @@ class StarPilotVCruise:
|
||||
not self.starpilot_planner.driving_in_curve)
|
||||
csc_was_controlling = self.csc_controlling_speed
|
||||
|
||||
slc_confirmation_pending = self.slc.speed_limit_changed_timer > DT_MDL and self.slc.unconfirmed_speed_limit >= 1
|
||||
csc_accel_button = bool(sm["starpilotCarState"].accelPressed) and not slc_confirmation_pending
|
||||
csc_accel_button = (bool(sm["starpilotCarState"].accelPressed) and
|
||||
not self.slc.confirmation_pending and
|
||||
not self.slc.confirmation_button_consumed)
|
||||
|
||||
|
||||
|
||||
@@ -637,24 +651,6 @@ class StarPilotVCruise:
|
||||
self.csc.handle_override(v_ego, csc_was_controlling, sm, accel_button=csc_accel_button)
|
||||
self.csc.log_data(v_ego, sm)
|
||||
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
self.slc.starpilot_toggles = starpilot_toggles
|
||||
|
||||
if starpilot_toggles.speed_limit_controller:
|
||||
self.slc.update_limits(sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm)
|
||||
self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm)
|
||||
|
||||
self.slc_offset = self.slc.offset
|
||||
self.slc_target = self.slc.target
|
||||
elif starpilot_toggles.show_speed_limits:
|
||||
self.slc.update_limits(sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm, display_only=True)
|
||||
|
||||
self.slc_offset = 0
|
||||
self.slc_target = self.slc.target
|
||||
else:
|
||||
self.slc_offset = 0
|
||||
self.slc_target = 0
|
||||
|
||||
self.nav_turn_target = self._get_nav_turn_control_target(v_cruise, sm, starpilot_toggles)
|
||||
|
||||
# Single tuning knob (signed feet -> meters). Defense clamp on top of UI bounds.
|
||||
@@ -750,7 +746,6 @@ class StarPilotVCruise:
|
||||
targets.append(self.csc_target)
|
||||
slc_control_target = get_active_slc_control_target(
|
||||
starpilot_toggles.speed_limit_controller,
|
||||
getattr(starpilot_toggles, "set_speed_limit", False),
|
||||
self.slc_target,
|
||||
self.slc_offset,
|
||||
self.slc.overridden_speed,
|
||||
@@ -766,6 +761,8 @@ class StarPilotVCruise:
|
||||
self.slc.overridden_speed > 0.0,
|
||||
getattr(self.slc, "source", "None"),
|
||||
)
|
||||
# Publish the semantic used by the UI after the lead-drop adjustment.
|
||||
self.slc_is_limiting_max_set = bool(controls_enabled and 0 < slc_control_target < v_cruise)
|
||||
self._applied_slc_control_target = slc_control_target if slc_control_target > 0.0 else 0.0
|
||||
if slc_control_target > 0.0:
|
||||
targets.append(slc_control_target)
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.car.chrysler.values import pacifica_hybrid_aol_requires_set_press
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, HyundaiFlags
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType
|
||||
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType, is_speed_limit_confirmation_pending
|
||||
from openpilot.selfdrive.selfdrived.events import ET
|
||||
|
||||
from openpilot.starpilot.common.experimental_state import (
|
||||
@@ -57,6 +57,8 @@ class StarPilotCard:
|
||||
self.params_memory = Params(memory=True)
|
||||
|
||||
self.accel_pressed = False
|
||||
self.confirmation_button_suppressed = set()
|
||||
self.pressed_accel_buttons = set()
|
||||
self.always_on_lateral_allowed = False
|
||||
self.controller_aol_override = None
|
||||
self.pacifica_aol_set_seen = False
|
||||
@@ -271,6 +273,23 @@ class StarPilotCard:
|
||||
]
|
||||
|
||||
button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents]
|
||||
accel_button_types = (int(ButtonType.accelCruise), int(ButtonType.resumeCruise))
|
||||
confirmation_pending = is_speed_limit_confirmation_pending(sm["starpilotPlan"])
|
||||
if confirmation_pending:
|
||||
self.confirmation_button_suppressed.update(self.pressed_accel_buttons)
|
||||
suppressed_releases = set()
|
||||
for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False):
|
||||
if be_type not in accel_button_types:
|
||||
continue
|
||||
if be.pressed:
|
||||
self.pressed_accel_buttons.add(be_type)
|
||||
if confirmation_pending:
|
||||
self.confirmation_button_suppressed.add(be_type)
|
||||
else:
|
||||
self.pressed_accel_buttons.discard(be_type)
|
||||
if be_type in self.confirmation_button_suppressed:
|
||||
self.confirmation_button_suppressed.remove(be_type)
|
||||
suppressed_releases.add(be_type)
|
||||
button_aol_supported = self.always_on_lateral_supported and (
|
||||
self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol
|
||||
)
|
||||
@@ -392,8 +411,11 @@ class StarPilotCard:
|
||||
if not self.always_on_lateral_supported:
|
||||
self.always_on_lateral_allowed = False
|
||||
|
||||
if sm.updated["starpilotPlan"] or any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types):
|
||||
self.accel_pressed = any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types)
|
||||
if sm.updated["starpilotPlan"] or any(be_type in accel_button_types for be_type in button_event_types):
|
||||
self.accel_pressed = any(
|
||||
be_type in accel_button_types and (be.pressed or be_type not in suppressed_releases)
|
||||
for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False)
|
||||
)
|
||||
|
||||
if sm.updated["starpilotPlan"] or any(be_type == ButtonType.decelCruise for be_type in button_event_types):
|
||||
self.decel_pressed = any(be_type == ButtonType.decelCruise for be_type in button_event_types)
|
||||
|
||||
@@ -382,7 +382,9 @@ class StarPilotPlanner:
|
||||
starpilotPlan.slcSpeedLimit = self.starpilot_vcruise.slc_target
|
||||
starpilotPlan.slcSpeedLimitOffset = self.starpilot_vcruise.slc_offset
|
||||
starpilotPlan.slcSpeedLimitSource = self.starpilot_vcruise.slc.source
|
||||
starpilotPlan.speedLimitChanged = self.starpilot_vcruise.slc.speed_limit_changed_timer > DT_MDL
|
||||
starpilotPlan.slcPresentedSpeedLimitSource = self.starpilot_vcruise.slc.presented_source
|
||||
starpilotPlan.slcIsLimitingMaxSet = self.starpilot_vcruise.slc_is_limiting_max_set
|
||||
starpilotPlan.speedLimitChanged = self.starpilot_vcruise.slc.confirmation_pending
|
||||
starpilotPlan.unconfirmedSlcSpeedLimit = self.starpilot_vcruise.slc.unconfirmed_speed_limit
|
||||
|
||||
starpilotPlan.themeUpdated = theme_updated
|
||||
|
||||
@@ -39,8 +39,8 @@ sys.modules["openpilot.selfdrive.controls.lib.longitudinal_planner"] = _module(
|
||||
)
|
||||
sys.modules["openpilot.starpilot.controls.lib.starpilot_vcruise"] = _module(
|
||||
"openpilot.starpilot.controls.lib.starpilot_vcruise",
|
||||
get_active_slc_control_target=lambda enabled, set_speed_limit, target, offset, overridden_speed, *_args, **_kwargs: (
|
||||
float(overridden_speed or target) + float(offset) if enabled and set_speed_limit else 0.0
|
||||
get_active_slc_control_target=lambda enabled, target, offset, overridden_speed, *_args, **_kwargs: (
|
||||
float(overridden_speed or target) + float(offset) if enabled else 0.0
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@@ -59,7 +59,7 @@ def make_sm():
|
||||
"carControl": SimpleNamespace(longActive=False),
|
||||
"selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False),
|
||||
"starpilotSelfdriveState": SimpleNamespace(alertType=[]),
|
||||
"starpilotPlan": SimpleNamespace(lateralCheck=True),
|
||||
"starpilotPlan": SimpleNamespace(lateralCheck=True, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0.0),
|
||||
"liveCalibration": SimpleNamespace(calPerc=100),
|
||||
}, updated={"starpilotPlan": False})
|
||||
|
||||
@@ -295,6 +295,37 @@ def make_wrapped_button_event(button_type, pressed):
|
||||
return SimpleNamespace(type=SimpleNamespace(raw=int(button_type)), pressed=pressed)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("pending_before_press", [False, True])
|
||||
def test_slc_confirmation_release_does_not_republish_accel(monkeypatch, tmp_path, pending_before_press):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0))
|
||||
sm = make_sm()
|
||||
toggles = make_toggles(speed_limit_controller=True)
|
||||
starpilot_car_state = SimpleNamespace(distancePressed=False)
|
||||
button_type = spc.ButtonType.accelCruise
|
||||
|
||||
if pending_before_press:
|
||||
sm["starpilotPlan"].speedLimitChanged = True
|
||||
sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 20.0
|
||||
pressed = make_car_state(button_events=[make_wrapped_button_event(button_type, True)])
|
||||
assert card.update(pressed, starpilot_car_state, sm, toggles).accelPressed
|
||||
|
||||
if not pending_before_press:
|
||||
sm["starpilotPlan"].speedLimitChanged = True
|
||||
sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 20.0
|
||||
card.update(make_car_state(), starpilot_car_state, sm, toggles)
|
||||
|
||||
sm["starpilotPlan"].speedLimitChanged = False
|
||||
sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 0.0
|
||||
released = make_car_state(button_events=[make_wrapped_button_event(button_type, False)])
|
||||
assert not card.update(released, starpilot_car_state, sm, toggles).accelPressed
|
||||
assert not card.confirmation_button_suppressed
|
||||
|
||||
assert card.update(pressed, starpilot_car_state, sm, toggles).accelPressed
|
||||
assert card.update(released, starpilot_car_state, sm, toggles).accelPressed
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("car_fingerprint", "expect_normalized_release"),
|
||||
(
|
||||
|
||||
Reference in New Issue
Block a user