diff --git a/iqpilot/cereal/custom.capnp b/iqpilot/cereal/custom.capnp index d056049a2..f779c3982 100644 --- a/iqpilot/cereal/custom.capnp +++ b/iqpilot/cereal/custom.capnp @@ -323,6 +323,8 @@ struct IQOnroadEvent @0xf4621d3ee9233bc9 { # camera hardware wideCamFaulty @32; + lateralEdgeBlocked @33; + } } @@ -505,8 +507,15 @@ enum IQTurnSignalDirection { turnRight @2; } +enum IQLateralEdgeBlock { + none @0; + left @1; + right @2; +} + struct IQDriveModelData @0xcdf0f7f14f46cb86 { turnSignalDirection @0 :IQTurnSignalDirection; + lateralEdgeBlock @1 :IQLateralEdgeBlock; } enum NavDirection { diff --git a/iqpilot/common/params_keys.h b/iqpilot/common/params_keys.h index 0cb80d796..952dffb58 100644 --- a/iqpilot/common/params_keys.h +++ b/iqpilot/common/params_keys.h @@ -162,6 +162,7 @@ inline static std::unordered_map keys = { // --- iqpilot params --- // {"ApiCache_DriveStats", {PERSISTENT, JSON}}, {"WideCamFaulty", {CLEAR_ON_MANAGER_START, BOOL}}, + {"IQEdgeGuard", {PERSISTENT, BOOL, "0"}}, {"IQLaneChangeBsmDelay", {PERSISTENT, BOOL, "0"}}, {"IQLaneChangeTimer", {PERSISTENT, INT, "0"}}, {"NavExitLaneChange", {PERSISTENT, BOOL, "0"}}, @@ -294,7 +295,7 @@ inline static std::unordered_map keys = { {"HyundaiCameraSCC", {PERSISTENT, INT, "0"}}, {"IsLdwsCar", {PERSISTENT, INT, "0"}}, {"LaneLineCheck", {PERSISTENT, INT, "0"}}, - {"LongitudinalPersonalityMax", {PERSISTENT, INT, "3"}}, + {"LongitudinalPersonalityMax", {PERSISTENT, INT, "2"}}, {"MaxAngleFrames", {PERSISTENT, INT, "89"}}, {"SpeedFromPCM", {PERSISTENT, INT, "2"}}, {"IQSubaruCreepAssist", {PERSISTENT, BOOL, "0"}}, diff --git a/iqpilot/docs/CHANGELOG.md b/iqpilot/docs/CHANGELOG.md index 0bfdfaa66..3dc10a1bb 100644 --- a/iqpilot/docs/CHANGELOG.md +++ b/iqpilot/docs/CHANGELOG.md @@ -66,6 +66,10 @@ - Added Always-On Lateral support through compatible Hyundai LFA buttons. - Added an optional mode that pauses steering torque when the driver takes the wheel and resumes after release. +#### Lane Changes + +- Added an optional model-based lane edge guard that blocks lane changes when a road edge is detected on the target side. + #### Lateral Tuning - Added configurable steering smoothing, slew limiting, and curvature lookahead. diff --git a/iqpilot/selfdrive/controls/lib/desire_helper.py b/iqpilot/selfdrive/controls/lib/desire_helper.py index e19a2e24f..ce77ab134 100644 --- a/iqpilot/selfdrive/controls/lib/desire_helper.py +++ b/iqpilot/selfdrive/controls/lib/desire_helper.py @@ -14,11 +14,13 @@ from iqpilot.selfdrive.controls.lib.helpers.lane_change import ( AutoLaneChangeMode, NavExitLaneChangeController, ) +from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection TurnDirection = custom.IQTurnSignalDirection +LateralEdgeBlock = custom.IQLateralEdgeBlock NavManeuverPhase = custom.IQNavState.ManeuverPhase LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS @@ -105,6 +107,8 @@ class DesireHelper: self.alc = IQLaneSwapController(self) self.lane_turn_controller = IQNavTurnController(self) self.nav_exit = NavExitLaneChangeController(_read_enable_bsm()) + self.lateral_edge_guard = LateralEdgeGuard() + self.lateral_edge_block = LateralEdgeBlock.none self.lane_turn_direction = TurnDirection.none self.nav_turn_direction = TurnDirection.none self.turn_desire_stop_timer = 0.0 @@ -174,6 +178,8 @@ class DesireHelper: def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None: self._refresh_requested_direction(one_blinker, nav_exit_active) blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction) + self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction) + lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction) nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed @@ -182,7 +188,7 @@ class DesireHelper: if (not (one_blinker or nav_exit_active)) or below_speed: self._clear_lane_change() - elif allowed_to_launch and not blindspot_detected: + elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked: self.lane_change_state = LaneChangeState.laneChangeStarting def _step_lane_change_starting(self, lane_change_prob: float) -> None: @@ -269,6 +275,8 @@ class DesireHelper: def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None): self._last_carstate = carstate + self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL) + self.lateral_edge_block = LateralEdgeBlock.none one_blinker = carstate.leftBlinker != carstate.rightBlinker below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN nav_exit_active = self._refresh_turn_overrides(carstate, nav_state) diff --git a/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py new file mode 100644 index 000000000..602ca2231 --- /dev/null +++ b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py @@ -0,0 +1,268 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, replace +from enum import IntEnum +from typing import Any + +from iqpilot.cereal import custom, log +from iqpilot.common.constants import CV +from iqpilot.common.params import Params +from iqpilot.common.swaglog import cloudlog + + +MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS +MAX_VALID_ROAD_EDGE_STD_M = 1.0 +EDGE_CONFIDENCE_SIGMA = 1.0 +ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0 +ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0 +LANE_CENTER_OFFSET_M = 3.5 +VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0 +EDGE_CLEARANCE_MARGIN_M = 0.25 +ADJACENT_LANE_LINE_PROB = 0.5 +EGO_LANE_LINE_PROB_MIN = 0.5 +MIN_MEASURED_LANE_WIDTH_M = 2.5 +MAX_MEASURED_LANE_WIDTH_M = 4.5 +OUTER_LANE_LINE_INDEX = (0, 3) +EGO_LANE_LINE_INDEX = (1, 2) +REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M +BLOCK_DEBOUNCE_S = 0.30 +CLEAR_DEBOUNCE_S = 0.50 +UNAVAILABLE_HOLD_S = 0.50 +TIMER_EPSILON_S = 1e-9 +PARAM_REFRESH_FRAMES = 50 + +LaneChangeDirection = log.LaneChangeDirection +LateralEdgeBlock = custom.IQLateralEdgeBlock + + +class RoadEdgeDataState(IntEnum): + VALID = 0 + UNAVAILABLE = 1 + INVALID = 2 + + +@dataclass(frozen=True, slots=True) +class RoadEdgeMeasurement: + state: RoadEdgeDataState + lateral_distance_m: float | None = None + conservative_distance_m: float | None = None + should_block: bool | None = None + + +@dataclass(frozen=True, slots=True) +class _SideState: + blocked: bool = False + block_timer_s: float = 0.0 + clear_timer_s: float = 0.0 + unavailable_timer_s: float = 0.0 + fallback_reported: bool = False + + +def evaluate_road_edge(edge: Any, std_m: Any, direction: int, + lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement: + if edge is None or std_m is None: + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + try: + xs = edge.x + ys = edge.y + count = len(xs) + y_count = len(ys) + except (AttributeError, TypeError): + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + if count == 0 or y_count != count: + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + try: + std = float(std_m) + except (TypeError, ValueError): + return RoadEdgeMeasurement(RoadEdgeDataState.INVALID) + if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M: + return RoadEdgeMeasurement(RoadEdgeDataState.INVALID) + + lateral_distance_m: float | None = None + for idx in range(count): + try: + x_m = float(xs[idx]) + y_m = float(ys[idx]) + except (IndexError, TypeError, ValueError): + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + if not math.isfinite(x_m) or not math.isfinite(y_m): + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M: + continue + if ((direction == LaneChangeDirection.left and y_m >= 0.0) or + (direction == LaneChangeDirection.right and y_m <= 0.0)): + return RoadEdgeMeasurement(RoadEdgeDataState.INVALID) + distance_m = abs(y_m) + lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m) + + if lateral_distance_m is None: + return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std + required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M + return RoadEdgeMeasurement( + RoadEdgeDataState.VALID, + lateral_distance_m, + conservative_distance_m, + conservative_distance_m < required_distance_m, + ) + + +def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool, + dt_s: float) -> tuple[_SideState, bool]: + if not speed_active: + return _SideState(), False + + if measurement.state == RoadEdgeDataState.UNAVAILABLE: + unavailable_timer_s = state.unavailable_timer_s + dt_s + if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S: + return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s, + fallback_reported=state.fallback_reported), False + fallback_started = not state.fallback_reported + return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started + + should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False + if should_block == state.blocked: + return _SideState(blocked=state.blocked), False + + if should_block: + block_timer_s = state.block_timer_s + dt_s + if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S: + return _SideState(blocked=True), False + return _SideState(block_timer_s=block_timer_s), False + + clear_timer_s = state.clear_timer_s + dt_s + if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S: + return _SideState(), False + return _SideState(blocked=True, clear_timer_s=clear_timer_s), False + + +class LateralEdgeGuard: + def __init__(self, enabled: bool | None = None) -> None: + self._params = Params() if enabled is None else None + self._param_refresh_frame = 0 + self.enabled = self._read_enabled() if enabled is None else enabled + self._active = False + self._left = _SideState() + self._right = _SideState() + self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + def _read_enabled(self) -> bool: + try: + return bool(self._params and self._params.get_bool("IQEdgeGuard")) + except Exception: + return False + + def _refresh_enabled(self) -> None: + if self._params is not None and self._param_refresh_frame % PARAM_REFRESH_FRAMES == 0: + self.enabled = self._read_enabled() + self._param_refresh_frame += 1 + + def _reset(self) -> None: + self._left = _SideState() + self._right = _SideState() + self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE) + + @staticmethod + def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]: + if modeldata is None: + return None, None + try: + edges = modeldata.roadEdges + stds = modeldata.roadEdgeStds + if len(edges) <= side_index or len(stds) <= side_index: + return None, None + return edges[side_index], stds[side_index] + except (AttributeError, TypeError): + return None, None + + @staticmethod + def _lane_line_prob(modeldata: Any, index: int) -> float | None: + if modeldata is None: + return None + try: + probs = modeldata.laneLineProbs + if len(probs) <= index: + return None + value = float(probs[index]) + except (AttributeError, TypeError, IndexError, ValueError): + return None + return value if math.isfinite(value) else None + + @classmethod + def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool: + prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index]) + return prob is not None and prob > ADJACENT_LANE_LINE_PROB + + @classmethod + def _measured_lane_width(cls, modeldata: Any) -> float: + left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0]) + right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1]) + if left_prob is None or right_prob is None: + return LANE_CENTER_OFFSET_M + if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN: + return LANE_CENTER_OFFSET_M + try: + lines = modeldata.laneLines + left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0]) + right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0]) + except (AttributeError, TypeError, IndexError, ValueError): + return LANE_CENTER_OFFSET_M + width = abs(right_y - left_y) + if not math.isfinite(width): + return LANE_CENTER_OFFSET_M + return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M) + + @staticmethod + def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement: + if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block: + return replace(measurement, should_block=False) + return measurement + + def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None: + self._refresh_enabled() + if not self.enabled: + if self._active: + self._reset() + self._active = False + return + if not self._active: + self._reset() + self._active = True + + dt = max(float(dt_s), 0.0) + left_edge, left_std = self._model_side(modeldata, 0) + right_edge, right_std = self._model_side(modeldata, 1) + lane_width_m = self._measured_lane_width(modeldata) + self.left_measurement = self._apply_lane_evidence( + evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m), + self._adjacent_lane_visible(modeldata, 0)) + self.right_measurement = self._apply_lane_evidence( + evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m), + self._adjacent_lane_visible(modeldata, 1)) + speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS + self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt) + self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt) + if left_fallback: + cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking") + if right_fallback: + cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking") + + def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock: + if not self.enabled: + return LateralEdgeBlock.none + if direction == LaneChangeDirection.left and self._left.blocked: + return LateralEdgeBlock.left + if direction == LaneChangeDirection.right and self._right.blocked: + return LateralEdgeBlock.right + return LateralEdgeBlock.none diff --git a/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py new file mode 100644 index 000000000..97a42b4e1 --- /dev/null +++ b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py @@ -0,0 +1,236 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +from iqpilot.cereal import custom, log +import iqpilot.cereal.messaging as messaging +from iqpilot.common.realtime import DT_MDL +from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode +from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import ( + ADJACENT_LANE_LINE_PROB, + BLOCK_DEBOUNCE_S, + CLEAR_DEBOUNCE_S, + LANE_CENTER_OFFSET_M, + MAX_MEASURED_LANE_WIDTH_M, + MAX_VALID_ROAD_EDGE_STD_M, + MIN_ACTIVE_SPEED_MPS, + MIN_MEASURED_LANE_WIDTH_M, + REQUIRED_ROAD_EDGE_DISTANCE_M, + UNAVAILABLE_HOLD_S, + LateralEdgeGuard, + RoadEdgeDataState, + evaluate_road_edge, +) +from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ, ET +from iqpilot.selfdrive.selfdrived.selfdrived import SelfdriveD + + +@dataclass +class Edge: + x: list[float] + y: list[float] + + +@dataclass +class ModelData: + roadEdges: list[Edge] + roadEdgeStds: list[float] + + +@dataclass +class LaneModelData: + roadEdges: list[Edge] + roadEdgeStds: list[float] + laneLines: list[Edge] + laneLineProbs: list[float] + + +class CarState: + def __init__(self, left_blindspot: bool = False) -> None: + self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0 + self.leftBlinker = True + self.rightBlinker = False + self.leftBlindspot = left_blindspot + self.rightBlindspot = False + self.steeringPressed = True + self.steeringTorque = 1.0 + self.brakePressed = False + self.standstill = False + + +def edge_model(left_distance_m: float = 6.0, right_distance_m: float = 6.0, + left_std_m: float = 0.0, right_std_m: float = 0.0) -> ModelData: + xs = [5.0, 20.0, 40.0] + return ModelData( + [Edge(xs, [-left_distance_m] * len(xs)), Edge(xs, [right_distance_m] * len(xs))], + [left_std_m, right_std_m], + ) + + +def lane_model(left_distance_m: float = 4.0, outer_prob: float = 0.0, + ego_width_m: float = 3.5, ego_prob: float = 0.9) -> LaneModelData: + xs = [5.0, 20.0, 40.0] + base = edge_model(left_distance_m, left_distance_m) + half = ego_width_m / 2.0 + lines = [Edge(xs, [-(half + 3.0)] * 3), Edge(xs, [-half] * 3), + Edge(xs, [half] * 3), Edge(xs, [half + 3.0] * 3)] + return LaneModelData(base.roadEdges, base.roadEdgeStds, lines, + [outer_prob, ego_prob, ego_prob, outer_prob]) + + +def cycles(duration_s: float) -> int: + return math.ceil(duration_s / DT_MDL) + + +def update_for(guard: LateralEdgeGuard, modeldata: ModelData | None, duration_s: float, + speed_mps: float = MIN_ACTIVE_SPEED_MPS) -> None: + for _ in range(cycles(duration_s)): + guard.update(modeldata, speed_mps, DT_MDL) + + +def test_valid_geometry_blocks_and_clear_geometry_does_not_block() -> None: + blocked = evaluate_road_edge(edge_model(4.0).roadEdges[0], 0.2, log.LaneChangeDirection.left) + clear = evaluate_road_edge(edge_model(6.0).roadEdges[0], 0.2, log.LaneChangeDirection.left) + assert blocked.state == RoadEdgeDataState.VALID + assert blocked.should_block is True + assert clear.state == RoadEdgeDataState.VALID + assert clear.should_block is False + + +def test_unavailable_and_invalid_are_distinct() -> None: + unavailable = evaluate_road_edge(Edge([5.0], []), 0.2, log.LaneChangeDirection.left) + invalid = evaluate_road_edge(edge_model().roadEdges[0], MAX_VALID_ROAD_EDGE_STD_M + 0.01, + log.LaneChangeDirection.left) + assert unavailable.state == RoadEdgeDataState.UNAVAILABLE + assert unavailable.lateral_distance_m is None + assert invalid.state == RoadEdgeDataState.INVALID + assert invalid.should_block is None + + +def test_distance_threshold_on_either_side() -> None: + epsilon_m = 0.001 + for direction, edge_index in ((log.LaneChangeDirection.left, 0), (log.LaneChangeDirection.right, 1)): + below = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m) + above = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m) + assert evaluate_road_edge(below.roadEdges[edge_index], 0.0, direction).should_block is True + assert evaluate_road_edge(above.roadEdges[edge_index], 0.0, direction).should_block is False + + +def test_disabled_guard_never_blocks() -> None: + guard = LateralEdgeGuard(enabled=False) + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S * 2) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_enabled_guard_blocks_after_debounce() -> None: + guard = LateralEdgeGuard(enabled=True) + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left + + +def test_parameter_refresh_controls_guard() -> None: + class EdgeGuardParams: + enabled = True + + def get_bool(self, key: str) -> bool: + assert key == "IQEdgeGuard" + return self.enabled + + params = EdgeGuardParams() + guard = LateralEdgeGuard(enabled=False) + guard._params = params + guard._param_refresh_frame = 0 + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left + + params.enabled = False + guard._param_refresh_frame = 50 + guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_clear_debounce_rejects_a_single_blocking_frame() -> None: + guard = LateralEdgeGuard(enabled=True) + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S) + update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S - DT_MDL) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left + guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL) + update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_unavailable_holds_then_falls_back_to_not_blocking() -> None: + guard = LateralEdgeGuard(enabled=True) + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S) + update_for(guard, None, UNAVAILABLE_HOLD_S - DT_MDL) + assert guard.left_measurement.state == RoadEdgeDataState.UNAVAILABLE + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left + guard.update(None, MIN_ACTIVE_SPEED_MPS, DT_MDL) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_speed_gate_is_inactive_below_threshold() -> None: + guard = LateralEdgeGuard(enabled=True) + update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS - 0.01) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_visible_outer_lane_line_overrides_edge_block() -> None: + guard = LateralEdgeGuard(enabled=True) + update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB + 0.2), BLOCK_DEBOUNCE_S * 4) + assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none + + +def test_measured_lane_width_is_clamped_and_falls_back() -> None: + assert LateralEdgeGuard._measured_lane_width(None) == LANE_CENTER_OFFSET_M + assert LateralEdgeGuard._measured_lane_width(edge_model(4.0)) == LANE_CENTER_OFFSET_M + assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_prob=0.1)) == LANE_CENTER_OFFSET_M + assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=9.0)) == MAX_MEASURED_LANE_WIDTH_M + assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=0.5)) == MIN_MEASURED_LANE_WIDTH_M + assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=3.2)) == 3.2 + + +def test_desire_helper_blocks_only_when_edge_guard_is_enabled() -> None: + helper = DesireHelper() + helper.lateral_edge_guard = LateralEdgeGuard(enabled=True) + helper.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE + helper.lane_change_state = log.LaneChangeState.preLaneChange + helper.lane_change_direction = log.LaneChangeDirection.left + update_for(helper.lateral_edge_guard, edge_model(4.0), BLOCK_DEBOUNCE_S) + helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0)) + assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left + assert helper.lane_change_state == log.LaneChangeState.preLaneChange + + helper.lateral_edge_guard = LateralEdgeGuard(enabled=False) + helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0)) + assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.none + assert helper.lane_change_state == log.LaneChangeState.laneChangeStarting + + +def test_published_edge_block_maps_to_distinct_event_and_alert() -> None: + message = messaging.new_message("iqDriveModelData") + message.iqDriveModelData.lateralEdgeBlock = custom.IQLateralEdgeBlock.right + + class SubMaster: + updated = {"iqDriveModelData": True} + + def __getitem__(self, service: str): + assert service == "iqDriveModelData" + return message.iqDriveModelData + + selfdrived = SelfdriveD.__new__(SelfdriveD) + selfdrived.sm = SubMaster() + selfdrived._cached_model_event_names = () + selfdrived._refresh_cached_model_events() + + event_name = custom.IQOnroadEvent.EventName.lateralEdgeBlocked + assert selfdrived._cached_model_event_names == (event_name,) + alert = EVENTS_IQ[event_name][ET.WARNING] + assert alert.alert_text_1 == "Lane Change Blocked" + assert alert.alert_text_2 == "Road edge detected" diff --git a/iqpilot/selfdrive/iqmodeld/daemon.py b/iqpilot/selfdrive/iqmodeld/daemon.py index 2f1edd563..97327992f 100755 --- a/iqpilot/selfdrive/iqmodeld/daemon.py +++ b/iqpilot/selfdrive/iqmodeld/daemon.py @@ -604,6 +604,7 @@ class InferenceDaemon: driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction + iq_msg.iqDriveModelData.lateralEdgeBlock = self._desire_logic.lateral_edge_block populate_odometry_message( pose_msg, diff --git a/iqpilot/selfdrive/longitudinal_settings.py b/iqpilot/selfdrive/longitudinal_settings.py new file mode 100644 index 000000000..9041f8b5a --- /dev/null +++ b/iqpilot/selfdrive/longitudinal_settings.py @@ -0,0 +1,80 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from iqpilot.cereal import log + + +LONGITUDINAL_MODE_STOCK = 0 +LONGITUDINAL_MODE_CHILL = 1 +LONGITUDINAL_MODE_DYNAMIC = 2 +LONGITUDINAL_MODE_PILOT = 3 + +PERSONALITY_AGGRESSIVE = log.LongitudinalPersonality.schema.enumerants["aggressive"] +PERSONALITY_STANDARD = log.LongitudinalPersonality.schema.enumerants["standard"] +PERSONALITY_RELAXED = log.LongitudinalPersonality.schema.enumerants["relaxed"] +PERSONALITY_VALUES = (PERSONALITY_AGGRESSIVE, PERSONALITY_STANDARD, PERSONALITY_RELAXED) + + +def get_longitudinal_mode(params) -> int: + if not params.get_bool("AlphaLongitudinalEnabled"): + return LONGITUDINAL_MODE_STOCK + if not params.get_bool("ExperimentalMode"): + return LONGITUDINAL_MODE_CHILL + return LONGITUDINAL_MODE_DYNAMIC if params.get_bool("IQDynamicMode") else LONGITUDINAL_MODE_PILOT + + +def get_valid_personality(params) -> int: + personality = params.get("LongitudinalPersonality", return_default=True) + if personality not in PERSONALITY_VALUES: + personality = min(max(personality, PERSONALITY_AGGRESSIVE), PERSONALITY_RELAXED) + params.put("LongitudinalPersonality", personality) + return personality + + +def set_valid_personality(params, personality: int) -> None: + if personality not in PERSONALITY_VALUES: + raise ValueError(f"invalid longitudinal personality: {personality}") + params.put("LongitudinalPersonality", personality) + + +def apply_longitudinal_mode(params, mode: int) -> None: + if mode == LONGITUDINAL_MODE_STOCK: + params.put_bool("AlphaLongitudinalEnabled", False) + params.put_bool("ExperimentalMode", False) + params.put_bool("IQDynamicMode", False) + elif mode == LONGITUDINAL_MODE_CHILL: + params.put_bool("AlphaLongitudinalEnabled", True) + params.put_bool("ExperimentalMode", False) + params.put_bool("IQDynamicMode", False) + set_valid_personality(params, PERSONALITY_RELAXED) + elif mode == LONGITUDINAL_MODE_DYNAMIC: + params.put_bool("AlphaLongitudinalEnabled", True) + params.put_bool("ExperimentalMode", True) + params.put_bool("IQDynamicMode", True) + elif mode == LONGITUDINAL_MODE_PILOT: + params.put_bool("AlphaLongitudinalEnabled", True) + params.put_bool("ExperimentalMode", True) + params.put_bool("IQDynamicMode", False) + else: + raise ValueError(f"invalid longitudinal mode: {mode}") + + +def get_follow_distance_state(params) -> tuple[int | None, bool]: + mode = get_longitudinal_mode(params) + if mode == LONGITUDINAL_MODE_STOCK: + get_valid_personality(params) + return None, False + if mode == LONGITUDINAL_MODE_CHILL: + if get_valid_personality(params) != PERSONALITY_RELAXED: + set_valid_personality(params, PERSONALITY_RELAXED) + return PERSONALITY_RELAXED, False + return get_valid_personality(params), True + + +def get_runtime_personality(params) -> int: + if get_longitudinal_mode(params) == LONGITUDINAL_MODE_CHILL: + if get_valid_personality(params) != PERSONALITY_RELAXED: + set_valid_personality(params, PERSONALITY_RELAXED) + return PERSONALITY_RELAXED + return get_valid_personality(params) diff --git a/iqpilot/selfdrive/selfdrived/iq_events.py b/iqpilot/selfdrive/selfdrived/iq_events.py index 89a8aec67..f9951ea5b 100644 --- a/iqpilot/selfdrive/selfdrived/iq_events.py +++ b/iqpilot/selfdrive/selfdrived/iq_events.py @@ -178,6 +178,14 @@ class IQEvents(EventsBase): EVENTS_IQ_TYPE = dict[int, dict[str, Alert | AlertCallbackType]] _GUIDANCE_EVENTS: EVENTS_IQ_TYPE = { + EventNameIQ.lateralEdgeBlocked: { + ET.WARNING: Alert( + "Lane Change Blocked", + "Road edge detected", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), + }, + EventNameIQ.speedLimitActive: { ET.WARNING: speed_limit_adjust_alert, }, diff --git a/iqpilot/selfdrive/selfdrived/selfdrived.py b/iqpilot/selfdrive/selfdrived/selfdrived.py index 6d861d37f..647cffe3d 100755 --- a/iqpilot/selfdrive/selfdrived/selfdrived.py +++ b/iqpilot/selfdrive/selfdrived/selfdrived.py @@ -22,19 +22,12 @@ from iqpilot.selfdrive.selfdrived.events import Events, ET from iqpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck from iqpilot.selfdrive.selfdrived.state import StateMachine from iqpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert +from iqpilot.selfdrive.longitudinal_settings import get_runtime_personality from iqpilot.system.version import get_build_metadata from iqpilot.system.hardware import HARDWARE from iqpilot.sab.behavior import SteeringAssistanceBehavior -def get_sanitize_int_param(key, min_val, max_val, params): - stored = params.get(key, return_default=True) - bounded = min(max(stored, min_val), max_val) - if bounded != stored: - params.put(key, bounded) - return bounded - - from iqpilot.selfdrive.controls.lib.helpers.lane_change import NAV_EXIT_COMMIT_DISTANCE from iqpilot.vehicle.vehicle import VehicleEvents from iqpilot.selfdrive.car.gap_button_actions import GapButtonActions @@ -158,12 +151,7 @@ class SelfdriveD(GapButtonActions): self.not_running_prev = None self.wide_cam_faulty = False self.experimental_mode = False - self.personality = get_sanitize_int_param( - "LongitudinalPersonality", - min(log.LongitudinalPersonality.schema.enumerants.values()), - max(log.LongitudinalPersonality.schema.enumerants.values()), - self.params - ) + self.personality = get_runtime_personality(self.params) self.recalibrating_seen = False self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -225,6 +213,9 @@ class SelfdriveD(GapButtonActions): model_data = self._get_model_data_ext() model_events = [] + if model_data.lateralEdgeBlock != custom.IQLateralEdgeBlock.none: + model_events.append(custom.IQOnroadEvent.EventName.lateralEdgeBlocked) + lane_turn_direction = model_data.turnSignalDirection if lane_turn_direction == TurnDirection.turnLeft: model_events.append(custom.IQOnroadEvent.EventName.modelTurnLeft) @@ -783,16 +774,7 @@ class SelfdriveD(GapButtonActions): self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl - # Params can be changed while selfdrived is running. Keep the live value in - # the same valid enum range enforced during startup; otherwise a stale value - # (for example 3) makes the alert callback lookup raise KeyError and kills - # selfdrived. - self.personality = get_sanitize_int_param( - "LongitudinalPersonality", - min(log.LongitudinalPersonality.schema.enumerants.values()), - max(log.LongitudinalPersonality.schema.enumerants.values()), - self.params, - ) + self.personality = get_runtime_personality(self.params) self.nav_exit_lane_change = self._read_nav_exit_lane_change() self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None diff --git a/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py b/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py index 88ab20676..11b83ac4c 100644 --- a/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py +++ b/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py @@ -1,6 +1,7 @@ from iqpilot.cereal import car -from iqpilot.selfdrive.selfdrived.selfdrived import _cleanup_startup_params, get_sanitize_int_param +from iqpilot.selfdrive.longitudinal_settings import get_valid_personality +from iqpilot.selfdrive.selfdrived.selfdrived import _cleanup_startup_params class DummyParams: @@ -36,5 +37,5 @@ class TestLongitudinalPrefPersistence: self.value = value params = ParamsWithInvalidPersonality() - assert get_sanitize_int_param("LongitudinalPersonality", 0, 2, params) == 2 + assert get_valid_personality(params) == 2 assert params.value == 2 diff --git a/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_settings.py b/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_settings.py new file mode 100644 index 000000000..7c57338da --- /dev/null +++ b/iqpilot/selfdrive/selfdrived/tests/test_longitudinal_settings.py @@ -0,0 +1,110 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +import pytest + +from iqpilot.selfdrive.longitudinal_settings import ( + LONGITUDINAL_MODE_CHILL, + LONGITUDINAL_MODE_DYNAMIC, + LONGITUDINAL_MODE_STOCK, + PERSONALITY_AGGRESSIVE, + PERSONALITY_RELAXED, + PERSONALITY_STANDARD, + PERSONALITY_VALUES, + apply_longitudinal_mode, + get_follow_distance_state, + get_longitudinal_mode, + get_runtime_personality, + set_valid_personality, +) + + +class Params: + def __init__(self, personality=PERSONALITY_STANDARD): + self.values = { + "AlphaLongitudinalEnabled": True, + "ExperimentalMode": True, + "IQDynamicMode": True, + "LongitudinalPersonality": personality, + } + self.personality_writes = [] + + def get(self, key, return_default=False): + return self.values[key] + + def get_bool(self, key): + return bool(self.values[key]) + + def put(self, key, value): + self.values[key] = value + if key == "LongitudinalPersonality": + self.personality_writes.append(value) + + def put_bool(self, key, value): + self.values[key] = bool(value) + + +def test_personality_writer_rejects_stock_value(): + params = Params() + + with pytest.raises(ValueError): + set_valid_personality(params, 3) + + assert params.personality_writes == [] + + +def test_mode_paths_only_write_valid_personalities(): + params = Params(PERSONALITY_AGGRESSIVE) + + for mode in range(4): + apply_longitudinal_mode(params, mode) + + assert all(value in PERSONALITY_VALUES for value in params.personality_writes) + + +def test_stock_mode_preserves_personality_and_dynamic_restores_it(): + params = Params(PERSONALITY_AGGRESSIVE) + + apply_longitudinal_mode(params, LONGITUDINAL_MODE_STOCK) + + assert get_follow_distance_state(params) == (None, False) + assert get_runtime_personality(params) == PERSONALITY_AGGRESSIVE + assert params.values["LongitudinalPersonality"] == PERSONALITY_AGGRESSIVE + assert params.personality_writes == [] + + apply_longitudinal_mode(params, LONGITUDINAL_MODE_DYNAMIC) + + assert get_longitudinal_mode(params) == LONGITUDINAL_MODE_DYNAMIC + assert get_follow_distance_state(params) == (PERSONALITY_AGGRESSIVE, True) + + +def test_stock_mode_sanitizes_legacy_stock_personality_value(): + params = Params(3) + + apply_longitudinal_mode(params, LONGITUDINAL_MODE_STOCK) + + assert get_follow_distance_state(params) == (None, False) + assert params.values["LongitudinalPersonality"] == PERSONALITY_RELAXED + assert params.personality_writes == [PERSONALITY_RELAXED] + + +def test_chill_mode_forces_relaxed_personality(): + params = Params(PERSONALITY_AGGRESSIVE) + + apply_longitudinal_mode(params, LONGITUDINAL_MODE_CHILL) + + assert get_follow_distance_state(params) == (PERSONALITY_RELAXED, False) + assert get_runtime_personality(params) == PERSONALITY_RELAXED + assert params.values["LongitudinalPersonality"] == PERSONALITY_RELAXED + assert params.personality_writes == [PERSONALITY_RELAXED] + + +def test_dynamic_and_pilot_enable_valid_personality_selection(): + params = Params(PERSONALITY_STANDARD) + + assert get_follow_distance_state(params) == (PERSONALITY_STANDARD, True) + + params.values["IQDynamicMode"] = False + + assert get_follow_distance_state(params) == (PERSONALITY_STANDARD, True) diff --git a/iqpilot/selfdrive/ui/mici/layouts/settings/cruise.py b/iqpilot/selfdrive/ui/mici/layouts/settings/cruise.py index 18c5720b5..d5173478d 100644 --- a/iqpilot/selfdrive/ui/mici/layouts/settings/cruise.py +++ b/iqpilot/selfdrive/ui/mici/layouts/settings/cruise.py @@ -3,13 +3,11 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed """ from iqpilot.selfdrive.ui.mici.widgets.stock_button import BigButton, BigParamControl -from iqpilot.selfdrive.ui.mici.layouts.settings.iq_widgets import MappedParamToggle, IQModeSelector, SafeParamControl +from iqpilot.selfdrive.ui.mici.layouts.settings.iq_widgets import FollowDistanceSelector, MappedParamToggle, IQModeSelector, SafeParamControl from iqpilot.system.ui.lib.application import gui_app from iqpilot.system.ui.widgets.scroller import NavScroller from iqpilot.system.ui.lib.multilang import tr -FOLLOW_DISTANCE_VALUES = [0, 1, 2, 3] - MS_TO_MPH = 2.23694 _SPEED_MPH = [10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80] _SPEED_OPTIONS = [f"{s} mph" for s in _SPEED_MPH] @@ -77,12 +75,11 @@ class CruiseLayoutMici(NavScroller): self._dynamic_panel = DynamicSettingsPanel() self._slc_panel = SlcSettingsPanel() - self._mode = IQModeSelector() + self._follow_dist = FollowDistanceSelector() + self._mode = IQModeSelector(self._follow_dist.refresh) self._dynamic_settings = BigButton(tr("iq.dynamic settings")) self._dynamic_settings.set_click_callback(lambda: gui_app.push_widget(self._dynamic_panel)) self._dynamic_settings.set_visible(self._mode.is_dynamic) - self._follow_dist = MappedParamToggle(tr("Follow Distance"), "LongitudinalPersonality", - [tr("aggressive"), tr("standard"), tr("relaxed"), tr("stock")], FOLLOW_DISTANCE_VALUES) self._speed_limit = MappedParamToggle(tr("Speed Limit"), "IQSpeedAssistMode", [tr("off"), tr("info"), tr("warning"), tr("control")]) self._slc_settings = BigButton(tr("speed limit settings")) diff --git a/iqpilot/selfdrive/ui/mici/layouts/settings/iq_widgets.py b/iqpilot/selfdrive/ui/mici/layouts/settings/iq_widgets.py index 3cf4ba0d8..c48e7c42f 100644 --- a/iqpilot/selfdrive/ui/mici/layouts/settings/iq_widgets.py +++ b/iqpilot/selfdrive/ui/mici/layouts/settings/iq_widgets.py @@ -3,6 +3,15 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed """ from iqpilot.common.params import Params, UnknownKeyName +from iqpilot.selfdrive.longitudinal_settings import ( + LONGITUDINAL_MODE_DYNAMIC, + LONGITUDINAL_MODE_PILOT, + PERSONALITY_VALUES, + apply_longitudinal_mode, + get_follow_distance_state, + get_longitudinal_mode, + set_valid_personality, +) from iqpilot.selfdrive.ui.mici.widgets.stock_button import BigButton, BigMultiToggle, BigToggle, BigParamControl from iqpilot.system.ui.lib.multilang import tr @@ -90,27 +99,49 @@ class MappedParamToggle(BigMultiToggle): pass +class FollowDistanceSelector(BigMultiToggle): + OPTIONS = ["aggressive", "standard", "relaxed", "stock"] + + def __init__(self): + self._display_options = [tr(option) for option in self.OPTIONS] + super().__init__(tr("Follow Distance"), self._display_options) + self._params = Params() + self.refresh() + + def refresh(self): + selection, enabled = get_follow_distance_state(self._params) + self.set_value(self._display_options[3 if selection is None else selection]) + self.set_enabled(enabled) + + def _handle_mouse_release(self, mouse_pos): + if get_longitudinal_mode(self._params) not in (LONGITUDINAL_MODE_DYNAMIC, LONGITUDINAL_MODE_PILOT): + return + BigButton._handle_mouse_release(self, mouse_pos) + selection, _ = get_follow_distance_state(self._params) + if selection is None: + self.refresh() + return + next_selection = PERSONALITY_VALUES[(PERSONALITY_VALUES.index(selection) + 1) % len(PERSONALITY_VALUES)] + set_valid_personality(self._params, next_selection) + self.set_value(self._display_options[next_selection]) + + class IQModeSelector(BigMultiToggle): """Longitudinal mode selector: Stock ACC / IQ.Chill / IQ.Dynamic / IQ.Pilot. A single tap cycles to the next mode and applies the matching param combo immediately. """ OPTIONS = ["Stock ACC", "IQ.Chill", "IQ.Dynamic", "IQ.Pilot"] - PERSONALITY_RELAXED = 2 - def __init__(self): + def __init__(self, mode_callback=None): self._display_options = [tr(option) for option in self.OPTIONS] super().__init__(tr("IQ Mode"), self._display_options) self._params = Params() + self._mode_callback = mode_callback self.refresh() def _index(self) -> int: - p = self._params - if not p.get_bool("AlphaLongitudinalEnabled"): - return 0 - if not p.get_bool("ExperimentalMode"): - return 1 - return 2 if p.get_bool("IQDynamicMode") else 3 + return get_longitudinal_mode(self._params) def is_dynamic(self) -> bool: return self._index() == 2 @@ -119,27 +150,12 @@ class IQModeSelector(BigMultiToggle): self.set_value(self._display_options[self._index()]) def _apply(self, idx: int): - p = self._params - if idx == 0: - p.put_bool("AlphaLongitudinalEnabled", False) - p.put_bool("ExperimentalMode", False) - p.put_bool("IQDynamicMode", False) - elif idx == 1: - p.put_bool("AlphaLongitudinalEnabled", True) - p.put_bool("ExperimentalMode", False) - p.put_bool("IQDynamicMode", False) - p.put("LongitudinalPersonality", self.PERSONALITY_RELAXED) - elif idx == 2: - p.put_bool("AlphaLongitudinalEnabled", True) - p.put_bool("ExperimentalMode", True) - p.put_bool("IQDynamicMode", True) - else: - p.put_bool("AlphaLongitudinalEnabled", True) - p.put_bool("ExperimentalMode", True) - p.put_bool("IQDynamicMode", False) - p.put_bool("OnroadCycleRequested", True) + apply_longitudinal_mode(self._params, idx) + self._params.put_bool("OnroadCycleRequested", True) def _handle_mouse_release(self, mouse_pos): nxt = (self._index() + 1) % len(self.OPTIONS) self._apply(nxt) self.set_value(self._display_options[nxt]) + if self._mode_callback: + self._mode_callback() diff --git a/iqpilot/selfdrive/ui/mici/layouts/settings/steering.py b/iqpilot/selfdrive/ui/mici/layouts/settings/steering.py index 060eb21b2..9d49c2754 100644 --- a/iqpilot/selfdrive/ui/mici/layouts/settings/steering.py +++ b/iqpilot/selfdrive/ui/mici/layouts/settings/steering.py @@ -78,8 +78,10 @@ class LaneChangePanel(NavScroller): [tr("off"), tr("nudge"), tr("no nudge"), "0.5 s", "1 s", "2 s", "3 s"], [-1, 0, 1, 2, 3, 4, 5]) self._bsm_delay = BigParamControl(tr("Delay with Blind Spot"), "IQLaneChangeBsmDelay") + self._edge_guard = BigParamControl(tr("Lane Edge Guard"), "IQEdgeGuard") + self._edge_guard.set_value(tr("Blocks lane changes when a road edge is detected on the target side.")) self._continuous = BigParamControl(tr("Continuous Changes"), "LaneChangeContinuous") - self._scroller.add_widgets([self._timer, self._bsm_delay, self._continuous]) + self._scroller.add_widgets([self._timer, self._bsm_delay, self._edge_guard, self._continuous]) def show_event(self): super().show_event() @@ -91,6 +93,7 @@ class LaneChangePanel(NavScroller): self._bsm_delay.set_enabled( enable_bsm and int(ui_state.params.get("IQLaneChangeTimer", return_default=True)) > AutoLaneChangeMode.NUDGE ) + self._edge_guard.refresh() self._continuous.refresh() diff --git a/iqpilot/system/ui/widgets/network.py b/iqpilot/system/ui/widgets/network.py index 7650e0cdd..566b8ea5d 100644 --- a/iqpilot/system/ui/widgets/network.py +++ b/iqpilot/system/ui/widgets/network.py @@ -468,9 +468,9 @@ class WifiManagerUI(Widget): if show_disconnect: disconnect_btn_rect = rl.Rectangle( - forget_btn_rect.x - self.btn_width - spacing, + forget_btn_rect.x - self.disconnect_btn_width - spacing, forget_btn_rect.y, - self.btn_width, + self.disconnect_btn_width, 80, ) self._disconnect_networks_buttons[network.ssid].render(disconnect_btn_rect) diff --git a/iqpilot/tools/install_python_dependencies.sh b/iqpilot/tools/install_python_dependencies.sh index 3a5133381..e8840e3d4 100755 --- a/iqpilot/tools/install_python_dependencies.sh +++ b/iqpilot/tools/install_python_dependencies.sh @@ -20,7 +20,21 @@ echo "updating uv..." uv self update || true echo "installing python packages..." -uv sync --frozen --all-extras +UV_SYNC_ARGS=(--frozen) +if [[ "${IQPILOT_RUNTIME_DEPENDENCIES_ONLY:-0}" != "1" ]]; then + UV_SYNC_ARGS+=(--all-extras) +fi +UV_SYNC_OK=0 +for attempt in 1 2 3; do + if uv sync "${UV_SYNC_ARGS[@]}"; then + UV_SYNC_OK=1 + break + fi + [[ "${attempt}" -lt 3 ]] && sleep "$((attempt * 5))" +done +if [[ "${UV_SYNC_OK}" -ne 1 ]]; then + exit 1 +fi source .venv/bin/activate if [[ "$(uname)" == 'Darwin' ]]; then diff --git a/iqpilot/tools/iqpilot/mici_preview.py b/iqpilot/tools/iqpilot/mici_preview.py index 5f43bb715..ab47e1bbe 100755 --- a/iqpilot/tools/iqpilot/mici_preview.py +++ b/iqpilot/tools/iqpilot/mici_preview.py @@ -144,6 +144,7 @@ def _patch_mock_state(): mp.put("NeuralNetworkFeedForward", False) mp.put("IQLaneChangeTimer", 0) # nudge mp.put("IQLaneChangeBsmDelay", False) + mp.put("IQEdgeGuard", False) # ── Visuals (correct param keys matching visuals.py) ───────────────────── mp.put("IQBlindSpotAlerts", True) @@ -173,7 +174,7 @@ def _patch_mock_state(): # ── Cruise ──────────────────────────────────────────────────────────────── mp.put("ExperimentalMode", False) mp.put("IQDynamicMode", False) - mp.put("LongitudinalPersonality", 1) # 0=aggressive,1=standard,2=relaxed,3=stock + mp.put("LongitudinalPersonality", 1) mp.put("IQSpeedAssistMode", 0) # 0=off # ── Misc / system ───────────────────────────────────────────────────────── diff --git a/iqpilot/ui/layouts/settings/iq_panels.py b/iqpilot/ui/layouts/settings/iq_panels.py index 529f4c93f..bb06fa680 100644 --- a/iqpilot/ui/layouts/settings/iq_panels.py +++ b/iqpilot/ui/layouts/settings/iq_panels.py @@ -614,6 +614,11 @@ class LaneChangeSettingsLayout(Widget): description=lambda: tr("Hold the automatic lane change while blind spot monitoring reports a car in the " "target lane, releasing it once the lane is clear."), ) + self._edge_guard = toggle_item( + param="IQEdgeGuard", + title=lambda: tr("Lane Edge Guard"), + description=lambda: tr("Blocks lane changes when a road edge is detected on the target side."), + ) self._continuous = toggle_item( param="LaneChangeContinuous", title=lambda: tr("Auto Lane Change: Continuous Changes"), @@ -626,6 +631,8 @@ class LaneChangeSettingsLayout(Widget): IQLineSeparator(40), self._bsm_delay, IQLineSeparator(40), + self._edge_guard, + IQLineSeparator(40), self._continuous, ]