diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ad23f9ec7..d7b36fba2 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -39,19 +39,17 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, b return float(new_curvature), limited_accel or limited_max_curv -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3): if len(speeds) == len(t_idxs): v_now = speeds[0] a_now = accels[0] v_target = np.interp(action_t, t_idxs, speeds) a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) else: + v_now = 0.0 v_target = 0.0 - v_target_1sec = 0.0 a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) + should_stop = (v_now < vEgoStopping and a_target < 0.1) return a_target, should_stop diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 1f1f69749..038024a8d 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -342,10 +342,14 @@ class DriverMonitoring: self._reset_awareness() return - driver_attentive = self.driver_distraction_filter.x < 0.37 awareness_prev = self.awareness + _reaching_pre = self.awareness - self.step_change <= self.threshold_pre + _reaching_terminal = self.awareness - self.step_change <= 0 + standstill_orange_exemption = standstill and _reaching_pre + always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal - if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): + if self.awareness > 0 and \ + ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption): if driver_engaged: self._reset_awareness() return @@ -358,19 +362,13 @@ class DriverMonitoring: if self.awareness > self.threshold_prompt: return - _reaching_pre = self.awareness - self.step_change <= self.threshold_pre - _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt - _reaching_terminal = self.awareness - self.step_change <= 0 - standstill_exemption = standstill and _reaching_pre - always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal - certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected if certainly_distracted or maybe_distracted: # should always be counting if distracted unless at standstill and reaching green # also will not be reaching 0 if DM is active when not engaged - if not (standstill_exemption or always_on_red_exemption): + if not (standstill_orange_exemption or always_on_red_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index b0a555709..73f3dbadc 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -189,6 +189,17 @@ class TestMonitoring: assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.preDriverDistracted assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted + # engaged, distracted while moving, then car stops after reaching orange + # - should reset timer to pre green at standstill + 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) + events, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + # just before and briefly after stopping: orange alert; goes away quickly after stopped + assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0 + # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert def test_somehow_indecisive_model(self):