diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 2442ac0a92..c4dd585728 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/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. diff --git a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc index 6b0e1ddb5d..53e16480ad 100644 --- a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc +++ b/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 diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 134ae3ef0e..a51bbf8159 100644 --- a/openpilot/selfdrive/car/card.py +++ b/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)) diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index 3193217a21..f762a68824 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/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 diff --git a/openpilot/starpilot/car/hyundai/dashboard.py b/openpilot/starpilot/car/hyundai/dashboard.py new file mode 100644 index 0000000000..d82d8e3461 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/mazda/dashboard.py b/openpilot/starpilot/car/mazda/dashboard.py index 60ffc31c7f..5d473913ba 100644 --- a/openpilot/starpilot/car/mazda/dashboard.py +++ b/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 diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 6eb034f95f..6417e1b737 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/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 diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index d51b3794d5..edd3f85bbc 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index d48373a992..fe1b9f5a58 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index efb9ce2458..c4dc90b5b5 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/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()