diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index c9d4763b3..016909d9c 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -84,6 +84,9 @@ class DesireHelper: torque_applied |= frogpilot_toggles.nudgeless torque_applied &= self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay + desired_lane_width = frogpilotPlan.laneWidthLeft if self.lane_change_direction == LaneChangeDirection.left else frogpilotPlan.laneWidthRight + torque_applied &= desired_lane_width >= frogpilot_toggles.lane_detection_width + if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 902ec1d2e..dbbd8f33b 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -12,6 +12,7 @@ from openpilot.common.constants import CV from openpilot.common.git import get_short_branch from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER from openpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION @@ -461,6 +462,17 @@ def nnff_loaded_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMast Priority.LOW, VisualAlert.none, AudibleAlert.engage, 5.0) +def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality, frogpilot_toggles: SimpleNamespace) -> Alert: + lane_width = sm["frogpilotPlan"].laneWidthLeft if sm["modelV2"].meta.laneChangeDirection == LaneChangeDirection.left else sm["frogpilotPlan"].laneWidthRight + lane_width_msg = f"{lane_width:.1f} Meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} Feet" + + return Alert( + "No Lane Available", + f"Detected Lane Width Is Only {lane_width_msg}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -1150,6 +1162,10 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: nnff_loaded_alert, }, + FrogPilotEventName.noLaneAvailable: { + ET.WARNING: no_lane_available_alert, + }, + FrogPilotEventName.openpilotCrashed: { ET.IMMEDIATE_DISABLE: Alert( "openpilot crashed", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 78f2605cc..a2ab25bac 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -300,9 +300,15 @@ class SelfdriveD: self.events.add(EventName.laneChangeBlocked) else: if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) + if self.sm['frogpilotPlan'].laneWidthLeft >= self.frogpilot_toggles.lane_detection_width: + self.events.add(EventName.preLaneChangeLeft) + else: + self.frogpilot_events.add(FrogPilotEventName.noLaneAvailable) else: - self.events.add(EventName.preLaneChangeRight) + if self.sm['frogpilotPlan'].laneWidthRight >= self.frogpilot_toggles.lane_detection_width: + self.events.add(EventName.preLaneChangeRight) + else: + self.frogpilot_events.add(FrogPilotEventName.noLaneAvailable) elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange)