less creep and DM alert

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
 
 

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

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):