mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 11:23:49 +08:00
iPhone FoldGate
This commit is contained in:
@@ -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) /
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user