diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 381d3c3cf6..89c2da8ca4 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/cereal/log.capnp b/cereal/log.capnp index f59a94691b..751f8fa994 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -825,13 +825,30 @@ struct SelfdriveState { alertStatus @5 :AlertStatus; alertSize @6 :AlertSize; alertType @7 :Text; - alertSound @8 :Car.CarControl.HUDControl.AudibleAlert; + alertSound @13 :AudibleAlert; alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert; # configurable driving settings experimentalMode @10 :Bool; personality @11 :LongitudinalPersonality; + enum AudibleAlert { + none @0; + + engage @1; + disengage @2; + refuse @3; + + warningSoft @4; + warningImmediate @5; + + prompt @6; + promptRepeat @7; + promptDistracted @8; + + preAlert @9; + } + enum OpenpilotState @0xdbe58b96d2d1ac61 { disabled @0; preEnabled @1; @@ -852,6 +869,10 @@ struct SelfdriveState { mid @2; full @3; } + + deprecated :group { + alertSound @8 :Car.CarControl.HUDControl.AudibleAlert; + } } struct ControlsState @0x97ff69c53601abf1 { @@ -2242,8 +2263,10 @@ struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { struct DriverMonitoringState { lockout @0 :Bool; - alertCountLockoutPercent @1 :Int8; - alertTimeLockoutPercent @2 :Int8; + lockoutRecoveryPercent @11 :Int8; + alert3Count @12 :Int8; + noResponseCount @13 :Int8; + noResponseForceDecel @14 :Bool; alwaysOn @3 :Bool; alwaysOnLockout @4 :Bool; @@ -2307,6 +2330,11 @@ struct DriverMonitoringState { calibratedPercent @0 :Int8; offset @1 :Float32; } + + deprecated :group { + alertCountLockoutPercent @1 :Int8; + alertTimeLockoutPercent @2 :Int8; + } } struct Boot { diff --git a/selfdrive/assets/sounds/pre_alert.wav b/selfdrive/assets/sounds/pre_alert.wav new file mode 100644 index 0000000000..46f64ff5b8 Binary files /dev/null and b/selfdrive/assets/sounds/pre_alert.wav differ diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav index c3d4475caa..241f645866 100644 Binary files a/selfdrive/assets/sounds/prompt_distracted.wav and b/selfdrive/assets/sounds/prompt_distracted.wav differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 18bc290173..ce9cf23e57 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -342,7 +342,7 @@ class Controls: cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) cs.ufAccelCmd = float(self.LoC.pid.f) - cs.forceDecel = bool((self.sm['driverMonitoringState'].alertLevel == log.DriverMonitoringState.AlertLevel.three) or + cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or (self.sm['selfdriveState'].state == State.softDisabling) or self.sm["starpilotCarState"].forceCoast) lat_tuning = self.CP.lateralTuning.which() diff --git a/selfdrive/monitoring/policy.py b/selfdrive/monitoring/policy.py index 620e5c6e19..877897c9a2 100644 --- a/selfdrive/monitoring/policy.py +++ b/selfdrive/monitoring/policy.py @@ -25,21 +25,27 @@ def to_percent(v): class DRIVER_MONITOR_SETTINGS: def __init__(self): - # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15. - self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24. - self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30. - # https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf - self._VISION_POLICY_ALERT_1_TIMEOUT = 3. - self._VISION_POLICY_ALERT_2_TIMEOUT = 5. - self._VISION_POLICY_ALERT_3_TIMEOUT = 11. + # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899 + self._ALERT_MIN_SPEED = 2.8 # 10 km/h + + self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5. + self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15. + self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25. + self._VISION_POLICY_ALERT_1_TIMEOUT = 5. + self._VISION_POLICY_ALERT_2_TIMEOUT = 8. + self._VISION_POLICY_ALERT_3_TIMEOUT = 13. + + # no response = alert_3 sustained for certain amount of time + self._NO_RESPONSE_TIMEOUT = 5. + + # lockout specs + self._MAX_ALERT_3 = 2 + self._MAX_NO_RESPONSE = 1 + self._LOCKOUT_TIME = int(1800 / DT_DMON) self._TIMEOUT_RECOVERY_FACTOR_MAX = 5. self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25 - self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts - self._MAX_TERMINAL_DURATION = int(30 / DT_DMON) # not allowed to engage after 30s of terminal alerts - self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.65 self._SG_THRESHOLD = 0.9 @@ -143,8 +149,11 @@ class DriverMonitoring: self.wheel_on_right_default = rhd_saved self.wheel_on_right_override = rhd_override self.face_detected = False - self.terminal_alert_cnt = 0 - self.terminal_time = 0 + self.alert_3_cnt = 0 + self.cnt_since_alert_3 = 0 + self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON) + self.no_response_cnt = 0 + self.lockout_time = 0 self.step_change = 0. self.active_policy = MonitoringPolicy.vision self.driver_interacting = False @@ -231,7 +240,7 @@ class DriverMonitoring: self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD) self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH) - def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.): + def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.): rhd_pred = driver_state.wheelOnRightProb # calibrates only when there's movement and either face detected if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or @@ -285,7 +294,7 @@ class DriverMonitoring: if self.face_detected and not self.driver_distracted: dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD - if dcam_uncertain and not standstill: + if dcam_uncertain and not lowspeed: self.dcam_uncertain_cnt += 1 self.dcam_reset_cnt = 0 else: @@ -300,14 +309,22 @@ class DriverMonitoring: elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear): + def _update_events(self, driver_engaged, op_engaged, lowspeed, wrong_gear): self.alert_level = AlertLevel.none self.driver_interacting = driver_engaged - if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ - self.terminal_time >= self.settings._MAX_TERMINAL_DURATION: + if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE: self.too_distracted = True + if self.too_distracted: + self.lockout_time += 1 + if self.lockout_time > self.settings._LOCKOUT_TIME: + self.too_distracted = False + self.alert_3_cnt = 0 + self.cnt_since_alert_3 = 0 + self.no_response_cnt = 0 + self.lockout_time = 0 + always_on_valid = self.always_on and not wrong_gear if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \ (not always_on_valid and not op_engaged) or \ @@ -319,11 +336,11 @@ class DriverMonitoring: awareness_prev = self.awareness _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1 _reaching_alert_3 = self.awareness - self.step_change <= 0 - standstill_exemption = standstill and _reaching_alert_1 + lowspeed_exemption = lowspeed and _reaching_alert_1 always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3 if self.awareness > 0 and \ - ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_exemption): + ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or lowspeed_exemption): if self.driver_interacting: self._reset_awareness() return @@ -340,21 +357,26 @@ class DriverMonitoring: maybe_distracted = self.is_model_uncertain or not self.face_detected if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at standstill and reaching green + # should always be counting if distracted unless at low speed and reaching green # also will not be reaching 0 if DM is active when not engaged - if not (standstill_exemption or always_on_exemption): + if not (lowspeed_exemption or always_on_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) if self.awareness <= 0.: # terminal alert: disengagement required self.alert_level = AlertLevel.three - self.terminal_time += 1 if awareness_prev > 0.: - self.terminal_alert_cnt += 1 - elif self.awareness <= self.threshold_alert_2: - self.alert_level = AlertLevel.two - elif self.awareness <= self.threshold_alert_1: - self.alert_level = AlertLevel.one + self.alert_3_cnt += 1 + self.cnt_since_alert_3 = 0 + else: + self.cnt_since_alert_3 += 1 + if self.cnt_since_alert_3 == self.no_response_timeout: + self.no_response_cnt += 1 + else: + if self.awareness <= self.threshold_alert_2: + self.alert_level = AlertLevel.two + elif self.awareness <= self.threshold_alert_1: + self.alert_level = AlertLevel.one def get_state_packet(self, valid=True): # build driverMonitoringState packet @@ -362,8 +384,10 @@ class DriverMonitoring: dm = dat.driverMonitoringState dm.lockout = self.too_distracted - dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS) - dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION) + dm.lockoutRecoveryPercent = to_percent(self.lockout_time / self.settings._LOCKOUT_TIME) + dm.alert3Count = self.alert_3_cnt + dm.noResponseCount = self.no_response_cnt + dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout dm.alwaysOn = self.always_on dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2 dm.alertLevel = self.alert_level @@ -400,7 +424,7 @@ class DriverMonitoring: car_speed = 30 enabled = True wrong_gear = False - standstill = False + lowspeed = False driver_engaged = False brake_disengage_prob = 1.0 steering_angle_deg = 0.0 @@ -409,7 +433,7 @@ class DriverMonitoring: car_speed = sm['carState'].vEgo enabled = sm['selfdriveState'].enabled wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) - standstill = sm['carState'].standstill + lowspeed = car_speed < self.settings._ALERT_MIN_SPEED driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s steering_angle_deg = sm['carState'].steeringAngleDeg @@ -426,7 +450,7 @@ class DriverMonitoring: cal_rpy=rpyCalib, car_speed=car_speed, op_engaged=enabled, - standstill=standstill, + lowspeed=lowspeed, demo_mode=demo, steering_angle_deg=steering_angle_deg, ) @@ -435,6 +459,6 @@ class DriverMonitoring: self._update_events( driver_engaged=driver_engaged, op_engaged=enabled, - standstill=standstill, + lowspeed=lowspeed, wrong_gear=wrong_gear, ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 2a83b48eb5..69319450d1 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,5 +1,3 @@ -import numpy as np - from cereal import log from openpilot.common.realtime import DT_DMON from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS @@ -49,15 +47,15 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON) class TestMonitoring: - def _run_seq(self, msgs, interaction, engaged, standstill): + def _run_seq(self, msgs, interaction, engaged, lowspeed): DM = DriverMonitoring() alert_lvls = [] for idx in range(len(msgs)): - DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx]) + DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0) + DM._update_events(interaction[idx], engaged[idx], lowspeed[idx], 0) alert_lvls.append(DM.alert_level) assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs" return alert_lvls, DM @@ -92,6 +90,25 @@ class TestMonitoring: (TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 assert isinstance(d_status.awareness, float) + # engaged, distracted past red and beyond the no-response window -> unavailability response + lockout + def test_distracted_lockout(self): + alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + s = d_status.settings + assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3 + assert d_status.alert_3_cnt == 1 + assert d_status.no_response_cnt == s._MAX_NO_RESPONSE + assert d_status.too_distracted + assert d_status.lockout_time > 0 + + # no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout + def test_invisible_lockout(self): + _, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + s = d_status.settings + assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch + assert d_status.alert_3_cnt == 1 + assert d_status.no_response_cnt == s._MAX_NO_RESPONSE + assert d_status.too_distracted + # engaged, no face detected the whole time, no action def test_fully_invisible_driver(self): alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) @@ -147,22 +164,22 @@ class TestMonitoring: # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # - both actions should clear the alert, but momentary appearance should not def test_sometimes_transparent_commuter(self): - _visible_time = np.random.choice([0.5, 10]) - ds_vector = always_no_face[:]*2 - interaction_vector = always_false[:]*2 - ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ - [msg_ATTENTIVE] * int(_visible_time/DT_DMON) - interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) - assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 - if _visible_time == 0.5: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 1 - elif _visible_time == 10: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 + for _visible_time in (0.5, 10): + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ + [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) + alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 + if _visible_time == 0.5: + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 2 + elif _visible_time == 10: + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # - only disengage will clear the alert @@ -175,7 +192,7 @@ class TestMonitoring: interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 + assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3 assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3 @@ -192,9 +209,9 @@ class TestMonitoring: # - should only reach green when stopped, but continues counting down on launch def test_long_traffic_light_victim(self): _redlight_time = 60 # seconds - standstill_vector = always_true[:] - standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + lowspeed_vector = always_true[:] + lowspeed_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) + alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector) s = d_status.settings assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0 _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT @@ -202,12 +219,12 @@ class TestMonitoring: assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2 # engaged, distracted while moving, then car stops after reaching orange - # - should reset timer to pre green at standstill + # - should reset timer to pre green at low speed def test_distracted_then_stops(self): _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange - standstill_vector = always_false[:] - standstill_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON) - alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + lowspeed_vector = always_false[:] + lowspeed_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON) + alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector) # just before and briefly after stopping: orange alert; goes away quickly after stopped assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2 assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0 diff --git a/selfdrive/selfdrived/alert_sound.py b/selfdrive/selfdrived/alert_sound.py index 4ab094a289..747c387b9f 100644 --- a/selfdrive/selfdrived/alert_sound.py +++ b/selfdrive/selfdrived/alert_sound.py @@ -1,7 +1,7 @@ -from cereal import car +from cereal import log -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert def filter_forcing_stop_alert_sound(alert_type, audible_alert, forcing_stop, chime_played): diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 2d07b0b4d4..ce2c20daa8 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -11,9 +11,10 @@ import cereal.messaging as messaging 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.common.realtime import DT_CTRL, DT_DMON from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +from openpilot.selfdrive.monitoring.policy import DRIVER_MONITOR_SETTINGS from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER from openpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION from openpilot.system.hardware import HARDWARE @@ -21,13 +22,15 @@ from openpilot.system.hardware import HARDWARE AlertSize = log.SelfdriveState.AlertSize AlertStatus = log.SelfdriveState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert EventName = log.OnroadEvent.EventName StarPilotAlertStatus = custom.StarPilotSelfdriveState.AlertStatus StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.AudibleAlert StarPilotEventName = custom.StarPilotOnroadEvent.EventName +DMON_LOCKOUT_TIME = DRIVER_MONITOR_SETTINGS()._LOCKOUT_TIME + # Alert priorities class Priority(IntEnum): @@ -128,7 +131,7 @@ class Alert: alert_size: log.SelfdriveState.AlertSize, priority: Priority, visual_alert: car.CarControl.HUDControl.VisualAlert, - audible_alert: car.CarControl.HUDControl.AudibleAlert, + audible_alert: log.SelfdriveState.AudibleAlert, duration: float, creation_delay: float = 0.): @@ -161,11 +164,12 @@ EmptyAlert = Alert("" , "", AlertStatus.normal, AlertSize.none, Priority.LOWEST, class NoEntryAlert(Alert): def __init__(self, alert_text_2: str, alert_text_1: str = "openpilot Unavailable", - visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none): + visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none, + priority: Priority = Priority.LOW): if HARDWARE.get_device_type() == 'mici': alert_text_1, alert_text_2 = alert_text_2, alert_text_1 super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, - AlertSize.mid, Priority.LOW, visual_alert, + AlertSize.mid, priority, visual_alert, AudibleAlert.refuse, 3.) @@ -193,7 +197,7 @@ class ImmediateDisableAlert(Alert): class EngagementAlert(Alert): - def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert): + def __init__(self, audible_alert: log.SelfdriveState.AudibleAlert): super().__init__("", "", AlertStatus.normal, AlertSize.none, Priority.MID, VisualAlert.none, @@ -282,6 +286,13 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) +def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality, starpilot_toggles: SimpleNamespace) -> Alert: + if sm['driverMonitoringState'].lockout: + mins_left = max(1, round((100 - sm['driverMonitoringState'].lockoutRecoveryPercent) / 100 * DMON_LOCKOUT_TIME * DT_DMON / 60.)) + return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH) + return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH) + + def audio_feedback_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality, starpilot_toggles: SimpleNamespace) -> Alert: duration = FEEDBACK_MAX_DURATION - ((sm['audioFeedback'].blockNum + 1) * SAMPLE_BUFFER / SAMPLE_RATE) return NormalPermanentAlert( @@ -615,7 +626,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1), }, EventName.driverDistracted2: { @@ -878,7 +889,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventName.tooDistracted: { - ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), + ET.NO_ENTRY: too_distracted_alert, }, EventName.excessiveActuation: { @@ -1423,7 +1434,7 @@ if HARDWARE.get_device_type() == 'mici': "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 2), + Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2), }, EventName.driverDistracted2: { ET.PERMANENT: Alert( diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index cc0ef6eee9..f2b6d8c3af 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -335,10 +335,13 @@ class SelfdriveD: self.events.add(EventName.resumeBlocked) if not self.CP.notCar: - # Block engaging until ignition cycle after max number or time of distractions + # Block engaging until lockout times out or ignition reset if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set: self.params.put_bool("DriverTooDistracted", True) self.dm_lockout_set = True + elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set: + self.params.remove("DriverTooDistracted") + self.dm_lockout_set = False # No entry conditions if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout: self.events.add(EventName.tooDistracted) diff --git a/selfdrive/selfdrived/tests/test_alert_sound.py b/selfdrive/selfdrived/tests/test_alert_sound.py index 89da781b25..2f437d8177 100644 --- a/selfdrive/selfdrived/tests/test_alert_sound.py +++ b/selfdrive/selfdrived/tests/test_alert_sound.py @@ -1,9 +1,9 @@ -from cereal import car +from cereal import log from openpilot.selfdrive.selfdrived.alert_sound import filter_forcing_stop_alert_sound -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert def test_forcing_stop_chimes_once_during_standstill_flicker(): diff --git a/selfdrive/selfdrived/tests/test_alerts.py b/selfdrive/selfdrived/tests/test_alerts.py index 46fc37f57f..e85c6a3a46 100644 --- a/selfdrive/selfdrived/tests/test_alerts.py +++ b/selfdrive/selfdrived/tests/test_alerts.py @@ -13,7 +13,7 @@ from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS AlertSize = log.SelfdriveState.AlertSize -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert EventName = log.OnroadEvent.EventName OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json") diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 428461f74d..220542592c 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -103,13 +103,14 @@ class BaseDriverCameraDialog(Widget): if self._pm is None: return + AudibleAlert = log.SelfdriveState.AudibleAlert + ALERT_SOUNDS = { + 'one': AudibleAlert.preAlert, + 'two': AudibleAlert.promptDistracted, + 'three': AudibleAlert.warningImmediate, + } msg = messaging.new_message('selfdriveState') if dm_state is not None: - AudibleAlert = car.CarControl.HUDControl.AudibleAlert - ALERT_SOUNDS = { - 'two': AudibleAlert.promptDistracted, - 'three': AudibleAlert.warningImmediate, - } msg.selfdriveState.alertSound = ALERT_SOUNDS.get(str(dm_state.alertLevel), AudibleAlert.none) self._pm.send('selfdriveState', msg) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index b62d444b59..234365355c 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -32,7 +32,7 @@ VOLUME_BASE = 20 if HARDWARE.get_device_type() in ("tici", "tizi"): VOLUME_BASE = 10 -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.AudibleAlert @@ -47,6 +47,8 @@ sound_list: dict[int, tuple[str, int | None, float]] = { AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), + AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME), + AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index a9da8455eb..b85f42dba4 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -1,11 +1,11 @@ -from cereal import car +from cereal import log from cereal import messaging from cereal.messaging import SubMaster, PubMaster from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert import time -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlert = log.SelfdriveState.AudibleAlert class TestSoundd: @@ -32,4 +32,3 @@ class TestSoundd: assert check_selfdrive_timeout_alert(sm) # TODO: add test with micd for checking that soundd actually outputs sounds -