diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fb58d5647..f49fbef5e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -28,6 +28,7 @@ from selfdrive.locationd.calibrationd import Calibration from selfdrive.hardware import HARDWARE, TICI, EON from selfdrive.manager.process_config import managed_processes +SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL @@ -430,7 +431,7 @@ class Controls: if self.state == State.enabled: if self.events.any(ET.SOFT_DISABLE): self.state = State.softDisabling - self.soft_disable_timer = int(3 / DT_CTRL) + self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) # SOFT DISABLING @@ -613,7 +614,7 @@ class Controls: self.events.add(EventName.ldw) clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None - alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) + alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer]) self.AM.add_many(self.sm.frame, alerts) self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 6254786b8..0048ca661 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1,5 +1,5 @@ from enum import IntEnum -from typing import Dict, Union, Callable, Any +from typing import Dict, Union, Callable from cereal import log, car import cereal.messaging as messaging @@ -198,11 +198,31 @@ def get_display_speed(speed_ms: float, metric: bool) -> str: # ********** alert callback functions ********** -def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: + +AlertCallbackType = Callable[[car.CarParams, messaging.SubMaster, bool, int], Alert] + + +def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return SoftDisableAlert(alert_text_2) + return func + + +def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return UserSoftDisableAlert(alert_text_2) + return func + + +def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}") -def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: +def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: return Alert( f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", "", @@ -210,7 +230,7 @@ def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) -def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: +def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: return Alert( "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", @@ -218,7 +238,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) -def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: +def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] return Alert( "Poor GPS reception", @@ -227,21 +247,22 @@ def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Al Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.) -def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: +def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: text = "Cruise Mode Disabled" if CP.carName == "honda": text = "Main Switch Off" return NoEntryAlert(text) -def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: +def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: axes = sm['testJoystick'].axes gb, steer = list(axes)[:2] if len(axes) else (0., 0.) vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" return NormalPermanentAlert("Joystick Mode", vals) -EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = { + +EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { # ********** events with no alerts ********** EventName.stockFcw: {}, @@ -358,7 +379,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub EventName.vehicleModelInvalid: { ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"), - ET.SOFT_DISABLE: SoftDisableAlert("Vehicle Parameter Identification Failed"), + ET.SOFT_DISABLE: soft_disable_alert("Vehicle Parameter Identification Failed"), }, EventName.steerTempUnavailableSilent: { @@ -544,7 +565,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.steerTempUnavailable: { - ET.SOFT_DISABLE: SoftDisableAlert("Steering Temporarily Unavailable"), + ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"), ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), }, @@ -581,12 +602,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.overheat: { ET.PERMANENT: NormalPermanentAlert("System Overheated"), - ET.SOFT_DISABLE: SoftDisableAlert("System Overheated"), + ET.SOFT_DISABLE: soft_disable_alert("System Overheated"), ET.NO_ENTRY: NoEntryAlert("System Overheated"), }, EventName.wrongGear: { - ET.SOFT_DISABLE: UserSoftDisableAlert("Gear not D"), + ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"), ET.NO_ENTRY: NoEntryAlert("Gear not D"), }, @@ -597,33 +618,33 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # See https://comma.ai/setup for more information EventName.calibrationInvalid: { ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"), - ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Remount Device & Recalibrate"), + ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"), ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), }, EventName.calibrationIncomplete: { ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"), + ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), }, EventName.doorOpen: { - ET.SOFT_DISABLE: UserSoftDisableAlert("Door Open"), + ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), ET.NO_ENTRY: NoEntryAlert("Door Open"), }, EventName.seatbeltNotLatched: { - ET.SOFT_DISABLE: UserSoftDisableAlert("Seatbelt Unlatched"), + ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"), ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), }, EventName.espDisabled: { - ET.SOFT_DISABLE: SoftDisableAlert("ESP Off"), + ET.SOFT_DISABLE: soft_disable_alert("ESP Off"), ET.NO_ENTRY: NoEntryAlert("ESP Off"), }, EventName.lowBattery: { - ET.SOFT_DISABLE: SoftDisableAlert("Low Battery"), + ET.SOFT_DISABLE: soft_disable_alert("Low Battery"), ET.NO_ENTRY: NoEntryAlert("Low Battery"), }, @@ -632,7 +653,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # is thrown. This can mean a service crashed, did not broadcast a message for # ten times the regular interval, or the average interval is more than 10% too high. EventName.commIssue: { - ET.SOFT_DISABLE: SoftDisableAlert("Communication Issue between Processes"), + ET.SOFT_DISABLE: soft_disable_alert("Communication Issue between Processes"), ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"), }, @@ -642,7 +663,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.radarFault: { - ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), + ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"), ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), }, @@ -650,7 +671,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # is not processing frames fast enough they have to be dropped. This alert is # thrown when over 20% of frames are dropped. EventName.modeldLagging: { - ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"), + ET.SOFT_DISABLE: soft_disable_alert("Driving model lagging"), ET.NO_ENTRY: NoEntryAlert("Driving model lagging"), }, @@ -660,25 +681,25 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # usually means the model has trouble understanding the scene. This is used # as a heuristic to warn the driver. EventName.posenetInvalid: { - ET.SOFT_DISABLE: SoftDisableAlert("Model Output Uncertain"), + ET.SOFT_DISABLE: soft_disable_alert("Model Output Uncertain"), ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"), }, # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we # alert the driver the device might have fallen from the windshield. EventName.deviceFalling: { - ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"), + ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"), ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), }, EventName.lowMemory: { - ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), + ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"), ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"), ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), }, EventName.highCpuUsage: { - #ET.SOFT_DISABLE: SoftDisableAlert("System Malfunction: Reboot Your Device"), + #ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"), #ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"), ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"), }, @@ -714,7 +735,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # Sometimes the USB stack on the device can get into a bad state # causing the connection to the panda to be lost EventName.usbError: { - ET.SOFT_DISABLE: SoftDisableAlert("USB Error: Reboot Your Device"), + ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"), ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""), ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), }, diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index ca238b429..f28d5373f 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -52,7 +52,7 @@ def cycle_alerts(duration=200, is_metric=False): events.clear() events.add(alert) - a = events.create_alerts([et, ], [CP, sm, is_metric]) + a = events.create_alerts([et, ], [CP, sm, is_metric, 0]) AM.add_many(frame, a) AM.process_alerts(frame) print(AM.alert)