iPhone FoldGate

This commit is contained in:
firestar5683
2026-09-09 15:32:38 -05:00
parent 50e2c21dbd
commit eedd73e522
54 changed files with 4210 additions and 216 deletions
-3
View File
@@ -652,9 +652,6 @@ class Controls:
elif CC.latActive and CS.steeringPressed and CS.steeringTorque * blinker_dir < 0.0 and \
self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN and \
self.turn_blinker_swept < CURVATURE_HOLD_CONFIRM_SWEPT:
# an active driver push into the signaled turn BEFORE the turn is made is fresh
# turn intent: re-arm the cycle even after a prior handoff. A long blinker-on
# approach can latch done on a trivial micro-handoff and lock out
# nudge-to-commit ten seconds later at the real turn (0000087f seg 1: +418 haul
# unassisted). The swept gate keeps a light same-direction touch during the
# EXIT unwind from re-latching a large hold against the model's recentering
@@ -632,6 +632,9 @@ class LatControlTorque(LatControl):
output_torque *= get_kia_ev6_center_output_scale(setpoint, CS.vEgo)
elif kia_carnival_active:
output_torque *= kia_carnival_center_taper
output_torque *= get_kia_carnival_unwind_output_scale(
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
)
output_torque *= get_kia_carnival_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif palisade_active:
output_torque *= get_palisade_center_output_scale(setpoint, CS.vEgo)
@@ -314,7 +314,7 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
GENESIS_G70_CURVE_UNWIND_OUTPUT_REDUCTION_MAX = 0.08
GENESIS_G70_CURVE_UNWIND_OUTPUT_REDUCTION_MAX = 0.10
GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
GENESIS_G70_CURVE_UNWIND_LAT = 0.25
@@ -629,6 +629,15 @@ KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.08
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.06
KIA_CARNIVAL_UNWIND_FF_JERK = 0.45
KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.20
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.28
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED = 8.0
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH = 2.0
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF = 16.0
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF_WIDTH = 2.5
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT = 0.25
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT_WIDTH = 0.15
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK = 0.45
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK_WIDTH = 0.20
TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44
TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28
@@ -2854,6 +2863,27 @@ def get_kia_carnival_unwind_ff_scale(setpoint: float, measured_lateral_accel: fl
return 1.0 - (KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX * speed_weight * overshoot_weight * jerk_weight)
def get_kia_carnival_unwind_output_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
if (setpoint * desired_lateral_jerk >= 0.0 or
setpoint * measured_lateral_accel <= 0.0):
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
speed_weight = (_sigmoid((v_ego - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED) /
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH) *
_sigmoid((KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF - v_ego) /
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF_WIDTH))
overshoot_weight = _sigmoid((overshoot - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT) /
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK) /
KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK_WIDTH)
return 1.0 - (KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX * speed_weight * overshoot_weight * jerk_weight)
def _tucson_4th_gen_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) / TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH)
@@ -3126,8 +3156,6 @@ def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: fl
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_UNWIND_FF_OVERSHOOT) /
GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_UNWIND_FF_JERK) /
+73 -6
View File
@@ -38,6 +38,10 @@ HONDA_BOSCH_A_CHALLENGER_STALE_CYCLES = 2
HONDA_BOSCH_A_GROSS_DISTANCE_STALE_CYCLES = 3
HONDA_BOSCH_A_GROSS_DISTANCE_M = 25.0
POST_STANDSTILL_RADAR_LEAD_PERSISTENCE_FRAMES = 3
POST_STANDSTILL_RADAR_LEAD_URGENT_TTC = 1.5
POST_STANDSTILL_RADAR_LEAD_URGENT_DISTANCE = 1.5
def is_bosch_a_radar_car(CP) -> bool:
return CP.brand == "honda" and CP.carFingerprint in HONDA_BOSCH_A and not CP.radarUnavailable
@@ -242,10 +246,17 @@ def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
def honda_bosch_a_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
"""Require a few real Bosch sweeps before a radar-only low-speed takeover."""
return track.cnt >= HONDA_BOSCH_A_LOW_SPEED_MIN_COUNT and track.potential_low_speed_lead(v_ego)
def post_standstill_radar_lead_is_urgent(lead: dict[str, Any]) -> bool:
d_rel = float(lead.get("dRel", math.inf))
v_rel = float(lead.get("vRel", 0.0))
closing_speed = max(-v_rel, 0.0)
ttc = d_rel / closing_speed if closing_speed > 0.1 else math.inf
return d_rel <= POST_STANDSTILL_RADAR_LEAD_URGENT_DISTANCE or ttc <= POST_STANDSTILL_RADAR_LEAD_URGENT_TTC
def track_matches_vision(track: Track, lead: capnp._DynamicStructReader, v_ego: float, *,
dist_scale: float, dist_floor: float, vel_limit: float,
y_std_scale: float, y_floor: float) -> bool:
@@ -454,6 +465,11 @@ class RadarD:
self.preferred_stale_track_ids = [-1, -1]
self.preferred_challenger_stale_counts = [0, 0]
self.preferred_gross_distance_stale_counts = [0, 0]
self._was_standstill = False
self._standstill_had_lead = False
self._post_standstill_gate_active = False
self._post_standstill_candidate_id = -1
self._post_standstill_candidate_frames = 0
self.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
@@ -525,9 +541,57 @@ class RadarD:
self.prev_lead_track_ids[lead_index] = -1
self._reset_preferred_stale_evidence(lead_index)
def _prepare_post_standstill_gate(self, standstill: bool) -> None:
if standstill:
self._post_standstill_gate_active = False
self._post_standstill_candidate_id = -1
self._post_standstill_candidate_frames = 0
elif self._was_standstill and not self._standstill_had_lead:
self._post_standstill_gate_active = True
self._post_standstill_candidate_id = -1
self._post_standstill_candidate_frames = 0
def _filter_post_standstill_lead(self, lead: dict[str, Any]) -> dict[str, Any]:
if not self._post_standstill_gate_active:
return lead
model_lead = float(lead.get("modelProb", 0.0)) > float(
getattr(self.starpilot_toggles, "lead_detection_probability", 0.35))
radar_only = bool(lead.get("status", False) and lead.get("radar", False) and not model_lead)
if not radar_only:
if lead.get("status", False):
self._post_standstill_gate_active = False
self._post_standstill_candidate_id = -1
self._post_standstill_candidate_frames = 0
return lead
track_id = int(lead.get("radarTrackId", -1))
if track_id == self._post_standstill_candidate_id:
self._post_standstill_candidate_frames += 1
else:
self._post_standstill_candidate_id = track_id
self._post_standstill_candidate_frames = 1
persistent = self._post_standstill_candidate_frames >= POST_STANDSTILL_RADAR_LEAD_PERSISTENCE_FRAMES
if persistent or post_standstill_radar_lead_is_urgent(lead):
self._post_standstill_gate_active = False
return lead
return {"status": False}
def _remember_post_standstill_state(self, standstill: bool, lead_status: bool) -> None:
if standstill:
self._was_standstill = True
self._standstill_had_lead |= lead_status
else:
self._was_standstill = False
self._standstill_had_lead = False
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
self.ready = sm.seen['modelV2']
self.current_time = 1e-9 * max(sm.logMonoTime.values())
standstill = bool(sm['carState'].standstill)
self._prepare_post_standstill_gate(standstill)
if sm.recv_frame['carState'] != self.last_v_ego_frame:
self.v_ego = sm['carState'].vEgo
@@ -585,11 +649,12 @@ class RadarD:
self._update_honda_bosch_a_preferred_staleness(i, leads_v3[i], self.lead_prob_filters[i].x)
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'],
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True,
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x,
preferred_track_id=self.prev_lead_track_ids[0],
honda_bosch_a_radar=self.honda_bosch_a_radar)
lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'],
standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True,
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x,
preferred_track_id=self.prev_lead_track_ids[0],
honda_bosch_a_radar=self.honda_bosch_a_radar)
self.radar_state.leadOne = self._filter_post_standstill_lead(lead_one)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'],
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False,
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x,
@@ -616,6 +681,8 @@ class RadarD:
if self.ready:
self.starpilot_radar_state.adjacentStopped = get_adjacent_stopped(self.tracks, sm['modelV2'])
self._remember_post_standstill_state(standstill, bool(self.radar_state.leadOne.status))
self.starpilot_toggles = get_starpilot_toggles(sm)
def publish(self, pm: messaging.PubMaster):
+18 -2
View File
@@ -161,6 +161,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_kia_carnival_friction_threshold,
get_kia_carnival_highway_transition_output_scale,
get_kia_carnival_unwind_ff_scale,
get_kia_carnival_unwind_output_scale,
get_kia_stinger_2022_center_taper_scale,
get_kia_stinger_2022_friction_threshold,
get_tucson_4th_gen_center_taper_scale,
@@ -742,6 +743,17 @@ class TestLatControl:
low_speed_exit = get_kia_carnival_unwind_ff_scale(0.31, 0.43, -0.88, 11.0)
assert low_speed_exit < 0.90
def test_kia_carnival_unwind_output_scale_is_bounded_and_phase_gated(self):
steady_turn = get_kia_carnival_unwind_output_scale(0.80, 0.90, 0.60, 11.0)
clean_unwind = get_kia_carnival_unwind_output_scale(0.20, 0.20, -1.5, 11.0)
overshooting_unwind = get_kia_carnival_unwind_output_scale(0.20, 0.90, -1.5, 11.0)
high_speed_overshoot = get_kia_carnival_unwind_output_scale(0.20, 0.90, -1.5, 25.0)
assert steady_turn == pytest.approx(1.0)
assert clean_unwind == pytest.approx(1.0)
assert 0.70 < overshooting_unwind < 1.0
assert high_speed_overshoot > overshooting_unwind
def test_genesis_g90_ff_scale_curve(self):
assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0
assert get_genesis_g90_ff_scale(0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(-0.5, 0.0, 20.0)
@@ -771,9 +783,13 @@ class TestLatControl:
assert base > left_unwind > right_unwind
def test_genesis_gv70_unwind_ff_scale(self):
assert get_genesis_gv70_unwind_ff_scale(-0.3, -0.3, 0.8, 15.0) == 1.0
steady_unwind = get_genesis_gv70_unwind_ff_scale(-0.3, -0.3, 0.8, 15.0)
assert steady_unwind < 1.0
assert get_genesis_gv70_unwind_ff_scale(-0.3, 0.1, 0.8, 15.0) == 1.0
assert get_genesis_gv70_unwind_ff_scale(-0.3, -0.3, -0.8, 15.0) == 1.0
early_unwind = get_genesis_gv70_unwind_ff_scale(-0.7, -0.6, 0.8, 15.0)
assert early_unwind < 1.0
reduced = get_genesis_gv70_unwind_ff_scale(-0.2, -1.0, 1.0, 20.0)
assert 0.6 < reduced < 1.0
assert get_genesis_gv70_unwind_ff_scale(-0.2, -1.0, -1.0, 20.0) == 1.0
@@ -967,7 +983,7 @@ class TestLatControl:
get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 65.0 * 0.44704)
assert get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 20.0 * 0.44704) > \
get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 65.0 * 0.44704)
assert 0.90 < get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) < 1.0
assert 0.88 < get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) < 1.0
assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0
assert get_genesis_g70_angle_output_scale(55.0, 1.0) > get_genesis_g70_angle_output_scale(85.0, 1.0)
assert get_genesis_g70_angle_output_scale(85.0, -1.0) == pytest.approx(1.0)
+60
View File
@@ -12,6 +12,8 @@ from openpilot.selfdrive.controls.radard import (
DT_MDL,
HONDA_BOSCH_A_RADAR_TS,
RadarD,
POST_STANDSTILL_RADAR_LEAD_PERSISTENCE_FRAMES,
post_standstill_radar_lead_is_urgent,
g90_low_speed_radar_lead_sane,
g90_radar_lead_lateral_sane,
has_slow_radar_tracks,
@@ -106,6 +108,64 @@ class TestLeads:
assert not has_slow_radar_tracks(normal_radar)
assert not has_slow_radar_tracks(unavailable_radar)
@staticmethod
def make_radar_only_lead(track_id: int, d_rel: float = 8.0, v_rel: float = 0.0):
return {
"status": True,
"radar": True,
"modelProb": 0.0,
"radarTrackId": track_id,
"dRel": d_rel,
"vRel": v_rel,
}
def test_post_standstill_radar_lead_requires_same_track_persistence(self):
radar = RadarD()
radar._post_standstill_gate_active = True
lead = self.make_radar_only_lead(42)
assert not radar._filter_post_standstill_lead(lead)["status"]
assert not radar._filter_post_standstill_lead(lead)["status"]
assert radar._filter_post_standstill_lead(lead)["status"]
assert radar._post_standstill_candidate_frames == POST_STANDSTILL_RADAR_LEAD_PERSISTENCE_FRAMES
def test_post_standstill_radar_lead_resets_for_new_track(self):
radar = RadarD()
radar._post_standstill_gate_active = True
assert not radar._filter_post_standstill_lead(self.make_radar_only_lead(42))["status"]
assert not radar._filter_post_standstill_lead(self.make_radar_only_lead(43))["status"]
assert radar._post_standstill_candidate_frames == 1
def test_post_standstill_gate_only_arms_after_no_lead_stop(self):
radar = RadarD()
radar._remember_post_standstill_state(standstill=True, lead_status=False)
radar._prepare_post_standstill_gate(standstill=False)
assert radar._post_standstill_gate_active
radar = RadarD()
radar._remember_post_standstill_state(standstill=True, lead_status=True)
radar._prepare_post_standstill_gate(standstill=False)
assert not radar._post_standstill_gate_active
def test_post_standstill_model_lead_bypasses_gate(self):
radar = RadarD()
radar._post_standstill_gate_active = True
lead = self.make_radar_only_lead(42)
lead["modelProb"] = 0.9
assert radar._filter_post_standstill_lead(lead)["status"]
assert not radar._post_standstill_gate_active
def test_post_standstill_urgent_radar_lead_bypasses_gate(self):
radar = RadarD()
radar._post_standstill_gate_active = True
lead = self.make_radar_only_lead(42, d_rel=3.0, v_rel=-2.1)
assert post_standstill_radar_lead_is_urgent(lead)
assert radar._filter_post_standstill_lead(lead)["status"]
assert not radar._post_standstill_gate_active
@pytest.mark.skipif(platform.system() == "Darwin", reason="SocketEventHandle requires eventfd")
def test_radar_fault(self):
# if there's no radar-related can traffic, radard should either not respond or respond with an error
@@ -0,0 +1,147 @@
from types import SimpleNamespace
from openpilot.selfdrive.ui import ui_state as ui_state_module
class FakeParams:
def __init__(self, **values):
self.values = values
def get_bool(self, key, **_kwargs):
return bool(self.values.get(key, False))
def get_int(self, key, **_kwargs):
return int(self.values.get(key, 0))
class PassthroughFilter:
def update(self, value):
return value
def make_device(monkeypatch, **overrides):
values = {
"ScreenManagement": True,
"ScreenBrightness": 35,
"ScreenBrightnessOnroad": 45,
"ScreenTimeout": 30,
"ScreenTimeoutOnroad": 10,
"StandbyMode": False,
}
values.update(overrides)
state = SimpleNamespace(
ui_params=FakeParams(**values),
status=ui_state_module.UIStatus.DISENGAGED,
started=False,
ignition=False,
light_sensor=-1.0,
sm={},
)
monkeypatch.setattr(ui_state_module, "ui_state", state)
monkeypatch.setattr(ui_state_module.gui_app, "big_ui", lambda: False)
monkeypatch.setattr(ui_state_module.gui_app, "_mouse_events", [])
device = ui_state_module.Device()
device._brightness_filter = PassthroughFilter()
return device, state
def test_manual_brightness_applies_to_current_device_state(monkeypatch):
device, state = make_device(monkeypatch)
assert device._calculate_brightness() == 35
state.started = True
assert device._calculate_brightness() == 45
def test_auto_brightness_preserves_existing_behavior(monkeypatch):
device, state = make_device(monkeypatch, ScreenBrightness=101, ScreenBrightnessOnroad=101)
assert device._calculate_brightness() == ui_state_module.BACKLIGHT_OFFROAD
state.started = True
state.light_sensor = -1.0
assert device._calculate_brightness() == ui_state_module.BACKLIGHT_OFFROAD
def test_screen_management_off_ignores_custom_values(monkeypatch):
device, state = make_device(monkeypatch, ScreenManagement=False, ScreenBrightness=10, ScreenBrightnessOnroad=20,
StandbyMode=True)
assert device._calculate_brightness() == ui_state_module.BACKLIGHT_OFFROAD
state.started = True
device._interaction_time = 0
assert device._calculate_brightness() == ui_state_module.BACKLIGHT_OFFROAD
assert device.interactive_timeout == 30
def test_screen_settings_refresh_after_external_param_change(monkeypatch):
now = 100.0
monkeypatch.setattr(ui_state_module.time, "monotonic", lambda: now)
device, state = make_device(monkeypatch)
state.started = True
state.ignition = True
device._ignition = True
device._interaction_time = now + 10
state.ui_params.values["ScreenBrightnessOnroad"] = 72
state.ui_params.values["ScreenTimeoutOnroad"] = 25
now += device.SCREEN_SETTINGS_REFRESH_INTERVAL
device._refresh_screen_settings()
assert device._calculate_brightness() == 72
assert device.interactive_timeout == 25
assert device._interaction_time == now + 25
def test_standby_blanks_after_timeout_and_touch_wakes(monkeypatch):
now = 100.0
monkeypatch.setattr(ui_state_module.time, "monotonic", lambda: now)
device, state = make_device(monkeypatch, StandbyMode=True)
state.started = True
state.ignition = True
device._ignition = True
device._interaction_time = now - 1
assert device._calculate_brightness() == 0
monkeypatch.setattr(ui_state_module.gui_app, "_mouse_events", [SimpleNamespace(left_down=True)])
device._update_wakefulness()
assert device._interaction_time == now + 10
assert device._calculate_brightness() == 45
def test_hide_ui_blanks_after_timeout_and_touch_wakes(monkeypatch):
now = 100.0
monkeypatch.setattr(ui_state_module.time, "monotonic", lambda: now)
device, state = make_device(monkeypatch, ScreenBrightnessOnroad=0)
state.started = True
state.ignition = True
device._ignition = True
device._interaction_time = now - 1
assert device._calculate_brightness() == 0
monkeypatch.setattr(ui_state_module.gui_app, "_mouse_events", [SimpleNamespace(left_down=True)])
device._update_wakefulness()
assert device._interaction_time == now + 10
assert device._calculate_brightness() == 5
def test_standby_wakes_for_visible_alert(monkeypatch):
now = 100.0
monkeypatch.setattr(ui_state_module.time, "monotonic", lambda: now)
device, state = make_device(monkeypatch, StandbyMode=True)
state.started = True
state.ignition = True
device._ignition = True
device._interaction_time = now - 1
device._visible_onroad_alert = lambda: True
device._update_wakefulness()
assert device._interaction_time == now + 10
assert device._calculate_brightness() == 45
+101 -10
View File
@@ -297,6 +297,8 @@ class UIState:
class Device:
SCREEN_SETTINGS_REFRESH_INTERVAL = 1.0
def __init__(self):
self._ignition = False
self._interaction_time: float = -1
@@ -306,6 +308,16 @@ class Device:
self._awake: bool = True
self._params = ui_state.ui_params
self._screen_settings_refresh_time: float = 0.0
self._screen_management = False
self._screen_brightness = 101
self._screen_brightness_onroad = 101
self._screen_timeout = 30
self._screen_timeout_onroad = 30
self._standby_mode = False
self._last_status = ui_state.status
self._refresh_screen_settings(force=True)
self._offroad_brightness: int = BACKLIGHT_OFFROAD
self._last_brightness: int = 0
self._brightness_filter = FirstOrderFilter(BACKLIGHT_OFFROAD, 10.00, 1 / gui_app.target_fps)
@@ -325,8 +337,8 @@ class Device:
if self._override_interactive_timeout is not None:
return self._override_interactive_timeout
timeout_onroad = self._params.get_int("ScreenTimeoutOnroad", return_default=True)
timeout_offroad = self._params.get_int("ScreenTimeout", return_default=True)
timeout_onroad = self._screen_timeout_onroad
timeout_offroad = self._screen_timeout
if timeout_onroad <= 0:
timeout_onroad = 10 if gui_app.big_ui() else 5
@@ -345,12 +357,54 @@ class Device:
self._interactive_timeout_callbacks.append(callback)
def update(self):
self._refresh_screen_settings()
# do initial reset
if self._interaction_time <= 0:
self._reset_interactive_timeout()
self._update_brightness()
self._update_wakefulness()
self._update_brightness()
def _refresh_screen_settings(self, force: bool = False) -> None:
now = time.monotonic()
if not force and now - self._screen_settings_refresh_time < self.SCREEN_SETTINGS_REFRESH_INTERVAL:
return
previous = (
self._screen_management,
self._screen_brightness,
self._screen_brightness_onroad,
self._screen_timeout,
self._screen_timeout_onroad,
self._standby_mode,
)
self._screen_management = self._params.get_bool("ScreenManagement")
if self._screen_management:
self._screen_brightness = min(max(self._params.get_int("ScreenBrightness", return_default=True), 0), 101)
self._screen_brightness_onroad = min(max(self._params.get_int("ScreenBrightnessOnroad", return_default=True), 0), 101)
self._screen_timeout = self._params.get_int("ScreenTimeout", return_default=True)
self._screen_timeout_onroad = self._params.get_int("ScreenTimeoutOnroad", return_default=True)
self._standby_mode = self._params.get_bool("StandbyMode")
else:
self._screen_brightness = 101
self._screen_brightness_onroad = 101
self._screen_timeout = 30
self._screen_timeout_onroad = 30
self._standby_mode = False
self._screen_settings_refresh_time = now
current = (
self._screen_management,
self._screen_brightness,
self._screen_brightness_onroad,
self._screen_timeout,
self._screen_timeout_onroad,
self._standby_mode,
)
if previous != current and self._interaction_time > 0:
self._reset_interactive_timeout()
def set_offroad_brightness(self, brightness: int | None):
if brightness is None:
@@ -358,6 +412,15 @@ class Device:
self._offroad_brightness = min(max(brightness, 0), 100)
def _update_brightness(self):
brightness = self._calculate_brightness()
if brightness != self._last_brightness:
if self._brightness_thread is None or not self._brightness_thread.is_alive():
self._brightness_thread = threading.Thread(target=HARDWARE.set_screen_brightness, args=(brightness,))
self._brightness_thread.start()
self._last_brightness = brightness
def _calculate_brightness(self) -> int:
clipped_brightness = self._offroad_brightness
if ui_state.started and ui_state.light_sensor >= 0:
@@ -374,19 +437,26 @@ class Device:
brightness = round(self._brightness_filter.update(clipped_brightness))
if not self._awake:
brightness = 0
elif ui_state.started and self._standby_mode and time.monotonic() > self._interaction_time:
brightness = 0
elif ui_state.started and self._screen_brightness_onroad != 101:
brightness = max(5, self._screen_brightness_onroad) if time.monotonic() <= self._interaction_time else self._screen_brightness_onroad
elif not ui_state.started and self._screen_brightness != 101:
brightness = self._screen_brightness
if brightness != self._last_brightness:
if self._brightness_thread is None or not self._brightness_thread.is_alive():
self._brightness_thread = threading.Thread(target=HARDWARE.set_screen_brightness, args=(brightness,))
self._brightness_thread.start()
self._last_brightness = brightness
return brightness
def _update_wakefulness(self):
# Handle interactive timeout
ignition_just_turned_off = not ui_state.ignition and self._ignition
ignition_state_changed = ui_state.ignition != self._ignition
self._ignition = ui_state.ignition
if ignition_just_turned_off or any(ev.left_down for ev in gui_app.mouse_events):
status_changed = ui_state.status != self._last_status and ui_state.status != UIStatus.OVERRIDE
self._last_status = ui_state.status
wake_for_onroad_event = (ui_state.started and self._standby_mode and self._screen_brightness_onroad != 0 and
(status_changed or self._visible_onroad_alert()))
if ignition_state_changed or any(ev.left_down for ev in gui_app.mouse_events) or wake_for_onroad_event:
self._reset_interactive_timeout()
interaction_timeout = time.monotonic() > self._interaction_time
@@ -397,6 +467,27 @@ class Device:
self._set_awake(ui_state.ignition or not interaction_timeout or PC)
@staticmethod
def _visible_onroad_alert() -> bool:
if not ui_state.started:
return False
sm = ui_state.sm
try:
selfdrive_state = sm["selfdriveState"]
if selfdrive_state.alertSize != log.SelfdriveState.AlertSize.none:
return True
if selfdrive_state.alertStatus != log.SelfdriveState.AlertStatus.normal:
return True
except Exception:
pass
try:
starpilot_state = sm["starpilotSelfdriveState"]
return getattr(starpilot_state.alertSize, "raw", 0) != 0
except Exception:
return False
def _set_awake(self, on: bool):
if on != self._awake:
self._awake = on