Force Stop: add dashboard signal back

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -130,6 +130,7 @@ class CarState(CarStateBase):
     self.ccnc_162_ts_ns = 0
     self.ccnc_1b5_ts_ns = 0
     self.dashboard_limit = LimitTracker()
+    self.dashboard_stop_sign = None
     self.ioniq6_bsm_sources = (Ioniq6BlindspotSources()
                                if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl else None)
     self.ioniq6_bsm_now_ns = 0
@@ -604,6 +605,9 @@ class CarState(CarStateBase):
       status, value_mps = hyundai_canfd_sign(speed, system_status, self.is_metric)
       self.dashboard_limit.update(timestamp, status, value_mps, valid_until_ns=expiry)
 
+    stamp = cp_cam.ts_nanos["ADAS_0x380"]["STOP_SIGN"]
+    self.dashboard_stop_sign = (bool(cp_cam.vl["ADAS_0x380"]["STOP_SIGN"]), stamp) if stamp > 0 else None
+
     return ret
 
   def get_can_parsers_canfd(self, CP):
@@ -699,6 +703,7 @@ class CarState(CarStateBase):
         msgs.append(("SCC_CONTROL", 50))
       if ("CAM_0x2a4", 20) not in cam_msgs:
         cam_msgs.append(("CAM_0x2a4", 20))
+    cam_msgs.append(("ADAS_0x380", math.nan))
     return {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),
       # Native CANParser accepts NaN to subscribe without a CAN-valid frequency gate.

opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc

@@ -695,6 +695,9 @@ BO_ 874 BLINDSPOTS_FRONT_CORNER_2: 16 XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
  SG_ SIDE_DETECT_STATE : 24|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 896 ADAS_0x380: 24 XXX
+ SG_ STOP_SIGN : 83|1@1+ (1,0) [0|1] "" XXX
+
 BO_ 961 BLINKER_STALKS: 8 XXX
  SG_ CHECKSUM_MAYBE : 7|8@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX

openpilot/selfdrive/car/card.py

@@ -1122,6 +1122,9 @@ class Car:
     if self.CP.brand == 'mazda':
       from openpilot.starpilot.car.mazda.dashboard import compose
       companion = compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
+    elif self.CP.brand == 'hyundai':
+      from openpilot.starpilot.car.hyundai.dashboard import compose as hyundai_compose
+      companion = hyundai_compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
     if self.v_cruise_helper.toyota is not None:
       if companion is None:
         companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))

openpilot/selfdrive/controls/plannerd.py

@@ -442,6 +442,8 @@ def starpilot_main():
   optional_slc = ['slcDashboardObservation', 'slcVisionObservation', 'mapdOut', 'starpilotCarState'] if slc_available else []
   if slc_available and feature_enabled(params, CP, 'aol', os.environ):
     optional_slc.append('aolAxisState')
+  if force_stop_owner is not None and 'starpilotCarState' not in optional_slc:
+    optional_slc.append('starpilotCarState')
   subscribed_services.extend(optional_slc)
   subscribed_services.append('starpilotNavigation')
   optional_conditional = ['starpilotNavigation'] + (['starpilotRadarState'] if conditional_host is not None else []) + optional_slc

openpilot/starpilot/car/hyundai/dashboard.py

@@ -0,0 +1,13 @@
+"""Optional Hyundai CAN-FD camera stop-sign observation; never a longitudinal control grant."""
+from opendbc.car.hyundai.values import HyundaiFlags
+from openpilot.starpilot.car.mazda.dashboard import write_stop_sign
+
+
+def capable(cp):
+  return cp.brand == 'hyundai' and bool(int(cp.flags) & HyundaiFlags.CANFD) and not cp.notCar
+
+
+def compose(cp, parsed, state, message, *, car_state_stamp):
+  if not capable(cp):
+    return message
+  return write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)

openpilot/starpilot/car/mazda/dashboard.py

@@ -17,6 +17,10 @@ def capable(cp):
 def compose(cp, parsed, state, message, *, car_state_stamp):
   if not capable(cp):
     return message
+  return write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)
+
+
+def write_stop_sign(parsed, state, message, *, car_state_stamp):
   if message is None:
     message = messaging.new_message('starpilotCarState', valid=bool(state.canValid), logMonoTime=car_state_stamp)
     message.starpilotCarState.sourceCarStateMonoTime = car_state_stamp

openpilot/starpilot/longitudinal/force_stop.py

@@ -7,6 +7,11 @@ from openpilot.common.realtime import DT_MDL
 
 STANDSTILL_HOLD_NS = 2_000_000_000
 STANDSTILL_CLEAR_NS = 750_000_000
+SIGN_MAX_SPEED_MPS = 17.88
+SIGN_MODEL_AGREE_M = 50.0
+SIGN_SEED_M = 27.0
+SIGN_APPROACH_DECEL = 1.0
+MODEL_APPROACH_DECEL = .65
 
 
 @dataclass(frozen=True)
@@ -50,6 +55,7 @@ class StopFrame:
   steering_angle_deg: float
   resume_requested: bool = False
   perception_available: bool = True
+  sign: bool = False
 
 
 @dataclass(frozen=True)
@@ -86,6 +92,7 @@ class ForceStop:
     self.standstill_since_ns = 0
     self.standstill_clear_ns = 0
     self.standstill_reason = None
+    self.sign_confirmed = False
     self.previously_enabled = False
     self.distance = 0.0
     self.distance_cap = 0.0
@@ -114,7 +121,7 @@ class ForceStop:
       self.reset()
       return StopPlan()
     available = frame.perception_available
-    if available and (frame.light or frame.raw_model_stopped):
+    if available and (frame.light or frame.raw_model_stopped) or frame.sign:
       if not frame.standstill:
         self.stop_seen_ns = now
     if frame.standstill:
@@ -128,7 +135,10 @@ class ForceStop:
     permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and
                  not frame.tracking_lead and not lead)
     model_path = frame.light and model_active and permitted and not curved
-    active = model_path
+    sign_path = frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and horizon < SIGN_MODEL_AGREE_M
+    if sign_path:
+      self.sign_confirmed = True
+    active = model_path or sign_path
     if model_path:
       self.from_light = True
       self.light_clear_ns = 0
@@ -139,7 +149,11 @@ class ForceStop:
     go_scene = available and horizon >= 50.0 and not frame.raw_model_stopped and not frame.model_should_stop
     held_before = self.previously_enabled and self.standstill_reason is not None
     engaged_stopped = not self.previously_enabled and frame.standstill
-    if frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None:
+    if frame.standstill and self.sign_confirmed and self.standstill_reason != 'sign':
+      self.standstill_since_ns = now
+      self.standstill_reason = 'sign'
+      self.distance = 0
+    elif frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None:
       self.standstill_since_ns = now
       self.standstill_reason = 'unclassified'
       self.distance = 0
@@ -148,7 +162,7 @@ class ForceStop:
         self.standstill_clear_ns = 0
       elif not self.standstill_clear_ns:
         self.standstill_clear_ns = now
-    hold_expired = self.standstill_reason is not None and (
+    hold_expired = self.standstill_reason not in (None, 'sign') and (
       now - self.standstill_since_ns >= STANDSTILL_HOLD_NS or
       self.standstill_clear_ns > 0 and now - self.standstill_clear_ns >= STANDSTILL_CLEAR_NS)
     if held_before and frame.standstill and frame.resume_requested:
@@ -163,18 +177,18 @@ class ForceStop:
     if not available and not self.forcing:
       self.timer = 0
     elif active and not frame.standstill:
-      self.timer = min(2.0, self.timer + DT_MDL)
+      self.timer = min(2.0, self.timer + DT_MDL * (2 if frame.sign else 1))
     elif turn and not self.forcing and not frame.standstill:
       self.timer = 0
     elif held:
       self.timer = max(self.timer, .5)
-    elif self.forcing and frame.standstill and not frame.light and not frame.raw_model_stopped:
+    elif self.forcing and frame.standstill and not frame.sign and not frame.light and not frame.raw_model_stopped:
       self.timer = 0
     else:
       self.timer = max(0, self.timer - DT_MDL * .25)
     force = self.timer >= .5 or self.forcing and not frame.standstill or held
     light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and
-                     available and not frame.light and go_scene)
+                     available and not frame.light and not frame.sign and go_scene)
     low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and
                       speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25)
     if light_cleared and not low_speed_hold:
@@ -203,7 +217,7 @@ class ForceStop:
         ceiling = 0.0
       else:
         self.distance = max(0.0, self.distance - speed * dt)
-        if (available and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
+        if (available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
             horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or
                                             speed >= self.entry_speed - tune.reanchor_speed_tolerance)):
           self.distance = horizon
@@ -211,18 +225,25 @@ class ForceStop:
           self.distance = min(self.distance, horizon)
         self.distance_cap = max(0.0, self.distance_cap - speed * dt)
         self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1))
+        if frame.sign and available:
+          if horizon < SIGN_MODEL_AGREE_M:
+            self.distance = min(self.distance, SIGN_SEED_M)
+          if horizon < SIGN_SEED_M:
+            self.distance = horizon
         effective = self.distance + offset + tune.distance_bias_m
-        ceiling = math.sqrt(2 * .65 * max(0, effective - tune.handoff_m))
+        decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
+        ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
     else:
       self.forcing = False
       self.entry_speed = None
+      self.sign_confirmed = False
       self._clear_standstill()
       self.distance = self.distance_cap = horizon if available else 0.0
       if frame.light and permitted and not curved and not overridden:
         approach = horizon
         effective = approach + offset + tune.distance_bias_m
         if effective > tune.handoff_m:
-          ceiling = math.sqrt(2 * .65 * (effective - tune.handoff_m))
+          ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m))
     self.previously_enabled = frame.enabled
     stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach
     # The MPC obstacle includes its standing-distance term; adding it at the

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -33,6 +33,17 @@ def standstill_takeoff_light(model, car) -> bool | None:
   return should_stop or horizon < FROZEN_RAW_STOP_DISTANCE_M + STOP_MODEL_RELEASE_MARGIN_M
 
 
+def dashboard_stop_sign(sm, *, now_ns: int, drive_id: int) -> bool:
+  try:
+    if not sm.seen['starpilotCarState'] or not sm.valid['starpilotCarState']:
+      return False
+    stamp = int(sm.logMonoTime['starpilotCarState'])
+    return bool(drive_id < stamp <= now_ns and now_ns - stamp <= SOURCE_AGE_NS and
+                sm['starpilotCarState'].dashboardStopSign > 0)
+  except (AttributeError, KeyError, TypeError, ValueError):
+    return False
+
+
 class ForceStopRuntime:
   def __init__(self, params):
     self.params = params
@@ -173,7 +184,8 @@ class ForceStopRuntime:
                         tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead),
                         raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None,
                         bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested,
-                        perception_available=perception_available)
+                        perception_available=perception_available,
+                        sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id))
       self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
       if takeoff_enabled:
         self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -199,6 +199,36 @@ class ForceStopTests(unittest.TestCase):
       self.assertFalse(release.forcing)
       self.assertFalse(release.should_stop)
 
+  def test_dashboard_sign_arms_only_slow_and_with_model_agreement(self):
+    for changes, armed in (({}, True), ({'speed_mps': 18.5}, False), ({'horizon_m': 60.}, False),
+                           ({'tracking_lead': True}, False)):
+      with self.subTest(changes=changes):
+        self.owner.reset()
+        for tick in range(6):
+          plan = self.owner.step(frame(tick, **{'speed_mps': 8., 'horizon_m': 45., 'light': False, 'sign': True, **changes}),
+                                 self.tune)
+        self.assertEqual(plan.forcing, armed)
+        if armed:
+          self.assertLessEqual(plan.tracked_distance_m, 27.)
+
+  def test_dashboard_sign_hold_waits_for_each_driver_action(self):
+    for action in ('pedal_override', 'resume_requested'):
+      with self.subTest(action=action):
+        self.owner.reset()
+        for tick in range(15):
+          self.owner.step(frame(tick, speed_mps=8., horizon_m=45., light=False, sign=True), self.tune)
+        for tick in range(15, 415):
+          plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+          self.assertTrue(plan.manual_hold)
+          self.assertTrue(plan.should_stop)
+        released = self.owner.step(frame(415, speed_mps=0., horizon_m=190., light=False, standstill=True,
+                                         **{action: True}), self.tune)
+        self.assertFalse(released.manual_hold)
+        self.assertFalse(released.forcing)
+        for tick in range(416, 600):
+          plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+          self.assertFalse(plan.forcing)
+
   def test_odometry_ceiling_bounds_reopened_model(self):
     for tick in range(15):
       self.owner.step(frame(tick), self.tune)

openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py

@@ -10,7 +10,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.plannerd import update_curve_frame
 from openpilot.starpilot.curve_speed.host import CurveHost
 from openpilot.starpilot.longitudinal.force_stop import StopPlan
-from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime
+from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime, dashboard_stop_sign
 from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages, snapshot
 from openpilot.starpilot.longitudinal.tests.test_stop_resume import event as button_event
 
@@ -360,6 +360,21 @@ class ForceStopRuntimeTests(unittest.TestCase):
         self.assertEqual(snapshot(original), snapshot(proposed))
         np.testing.assert_array_equal(original.mpc.params, proposed.mpc.params)
 
+  def test_dashboard_stop_sign_requires_fresh_valid_companion(self):
+    from openpilot.cereal import messaging
+    message = messaging.new_message('starpilotCarState')
+    message.starpilotCarState.dashboardStopSign = 1
+    for defect, expected in ((None, True), ('stale', False), ('invalid', False), ('unseen', False), ('missing', False)):
+      with self.subTest(defect=defect):
+        sm = Frame(0)
+        if defect != 'missing':
+          sm['starpilotCarState'] = message.starpilotCarState
+          sm.logMonoTime['starpilotCarState'] = sm.stamp
+          sm.valid['starpilotCarState'] = defect != 'invalid'
+        sm.seen = {'starpilotCarState': defect not in ('unseen', 'missing')}
+        now = sm.stamp + (150_000_001 if defect == 'stale' else 1_000_000)
+        self.assertEqual(dashboard_stop_sign(sm, now_ns=now, drive_id=DRIVE), expected)
+
 
 if __name__ == '__main__':
   unittest.main()