diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index 686b777f59..fb6f22a7b5 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -56,6 +56,20 @@ GM_CANDIDATE_PREFIXES = ("CHEVROLET_", "GMC_", "CADILLAC_", "BUICK_", "HOLDEN_") GM_CORE_FINGERPRINT_MSGS = frozenset((190, 201, 209, 211, 241)) GM_CAMERA_BUS = 2 GM_VOLT_CAMERA_MSG = 0x320 +GM_SUBURBAN_CAMERA_VIN_PREFIX = "1GNSKJKJ" +GM_SUBURBAN_CAMERA_PT_SIGNATURE = { + 190: 6, + 201: 8, + 209: 7, + 211: 2, + 241: 6, + 304: 1, + 320: 3, +} +GM_CAMERA_DIAGNOSTIC_MESSAGES = { + 0x24b: 8, + 0x64b: 8, +} def _normalize_forced_candidate(candidate: str | None) -> str | None: @@ -152,6 +166,24 @@ def _normalize_gm_volt_candidate(candidate: str | None, fingerprints: dict[int, return candidate +def _normalize_gm_suburban_camera_candidate(candidate: str | None, fingerprints: dict[int, dict], vin: str | None) -> str | None: + """Resolve the 2019 Suburban camera-harness variant when CAN is shared with Yukon.""" + if candidate not in (None, "GMC_YUKON", "GMC_YUKON_CC"): + return candidate + + if not isinstance(vin, str) or not vin.startswith(GM_SUBURBAN_CAMERA_VIN_PREFIX): + return candidate + + powertrain = fingerprints.get(0, {}) + camera = fingerprints.get(GM_CAMERA_BUS, {}) + if not all(powertrain.get(address) == length for address, length in GM_SUBURBAN_CAMERA_PT_SIGNATURE.items()): + return candidate + if not all(camera.get(address) == length for address, length in GM_CAMERA_DIAGNOSTIC_MESSAGES.items()): + return candidate + + return "CHEVROLET_SUBURBAN_CAMERA" + + def _is_gm_candidate(candidate: str | None) -> bool: return isinstance(candidate, str) and candidate.startswith(GM_CANDIDATE_PREFIXES) @@ -307,6 +339,10 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip stored_candidate = _normalize_forced_candidate(params.get("CarModel")) cached_candidate = _normalize_forced_candidate(getattr(cached_params, "carFingerprint", None)) + if candidate is None and stored_candidate is None and cached_candidate is None: + candidate = _normalize_gm_suburban_camera_candidate(candidate, fingerprints, vin) + fingerprinted_candidate = candidate + if candidate is None: gm_fallback_candidate = _get_gm_stored_candidate_fallback(fingerprints, stored_candidate, cached_candidate) if gm_fallback_candidate is not None: diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 7ea61ba3ac..ec18829838 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -1159,9 +1159,12 @@ class CarController(CarControllerBase): if should_send_cc_button_spam(self.CP, CC, CS): if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC: # Using extend instead of append since the message is only sent intermittently - lead_visible = bool(getattr(CS, "openpilot_lead_visible", CC.hudControl.leadVisible)) + longitudinal_adjustment_active = bool(getattr( + CS, "openpilot_longitudinal_adjustment_active", CC.hudControl.leadVisible, + )) can_sends.extend(gmcan.create_gm_cc_spam_command( - self.packer_pt, self, CS, actuators, starpilot_toggles, lead_visible=lead_visible, + self.packer_pt, self, CS, actuators, starpilot_toggles, + longitudinal_adjustment_active=longitudinal_adjustment_active, )) else: if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and diff --git a/opendbc_repo/opendbc/car/gm/fingerprints.py b/opendbc_repo/opendbc/car/gm/fingerprints.py index 922d7f849e..f2bd001983 100644 --- a/opendbc_repo/opendbc/car/gm/fingerprints.py +++ b/opendbc_repo/opendbc/car/gm/fingerprints.py @@ -213,9 +213,9 @@ FINGERPRINTS.update({ CAR.CADILLAC_ESCALADE_ESV_2019_ASCM: [{**fp, SASCM_ADDRESS: 8} for fp in FINGERPRINTS[CAR.CADILLAC_ESCALADE_ESV_2019]], CAR.CHEVROLET_SUBURBAN: FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC], CAR.CHEVROLET_SUBURBAN_ASCM: FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC], - CAR.CHEVROLET_SUBURBAN_CAMERA: [ - {**fp, CAMERA_DIAGNOSTIC_ADDRESS: 8, CAMERA_DIAGNOSTIC_RX_ADDRESS: 8} for fp in FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC] - ], + # The camera-harness Suburban shares the observed CAN map with the 2019 Yukon; + # VIN and camera-bus diagnostics disambiguate it during live fingerprinting. + CAR.CHEVROLET_SUBURBAN_CAMERA: FINGERPRINTS[CAR.GMC_YUKON], CAR.GMC_YUKON_CC: FINGERPRINTS[CAR.GMC_YUKON], CAR.CADILLAC_XT6: FINGERPRINTS[CAR.CHEVROLET_TRAVERSE], CAR.CADILLAC_XT5: FINGERPRINTS[CAR.CHEVROLET_TRAVERSE], diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index a5f97ce22b..670180cbd5 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -32,7 +32,7 @@ VOLT_CC_CARS = { CAR.CHEVROLET_VOLT_CC, } VOLT_CC_FREE_REQUEST_DEADBAND_MPH = 5.0 -VOLT_CC_LEAD_REQUEST_DEADBAND_MPH = 2.0 +VOLT_CC_ACTIVE_REQUEST_DEADBAND_MPH = 2.0 def malibu_phase_map_for_button(button): @@ -341,12 +341,15 @@ def stabilize_bolt_cc_button(controller, CP, requested_button): return requested_button -def _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible): +def _create_volt_cc_spam_command(CS, actuators, ms_convert, longitudinal_adjustment_active): accel = float(actuators.accel) speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert)) ego_speed = CS.out.vEgo * ms_convert requested_setpoint = (CS.out.vEgo * 1.01 + 3 * accel) * ms_convert - deadband_mph = VOLT_CC_LEAD_REQUEST_DEADBAND_MPH if lead_visible else VOLT_CC_FREE_REQUEST_DEADBAND_MPH + deadband_mph = ( + VOLT_CC_ACTIVE_REQUEST_DEADBAND_MPH if longitudinal_adjustment_active + else VOLT_CC_FREE_REQUEST_DEADBAND_MPH + ) request_deadband = deadband_mph * (CV.MPH_TO_KPH if ms_convert == CV.MS_TO_KPH else 1.0) target_setpoint = None @@ -361,6 +364,9 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible): ) if target_setpoint is not None and accel > 0.0 and speed_setpoint >= target_setpoint: return CruiseButtons.INIT, float("inf") + if (target_setpoint is not None and accel < 0.0 and speed_setpoint <= target_setpoint and + not longitudinal_adjustment_active): + return CruiseButtons.INIT, float("inf") if not moving_toward_target and abs(requested_setpoint - speed_setpoint) <= request_deadband: return CruiseButtons.INIT, float("inf") @@ -382,7 +388,7 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible): return CruiseButtons.RES_ACCEL, rate -def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles, lead_visible=False): +def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles, longitudinal_adjustment_active=False): accel = actuators.accel v_ego = CS.out.vEgo cruise_btn = CruiseButtons.INIT @@ -397,7 +403,7 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl comparison_setpoint = projected_setpoint if bolt_cc else desired_setpoint if CS.CP.carFingerprint in VOLT_CC_CARS: - cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert, lead_visible) + cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert, longitudinal_adjustment_active) else: if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25: cruise_btn = CruiseButtons.CANCEL diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 915895990a..f338d78c10 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -1135,7 +1135,7 @@ class TestGMCarController: assert msgs == [] assert controller.apply_speed == 100 - def test_volt_cc_redneck_holds_medium_decel_request_at_max_without_lead(self): + def test_volt_cc_redneck_holds_strong_decel_request_at_max_during_free_cruise(self): packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt]) controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0) cs = SimpleNamespace( @@ -1154,13 +1154,13 @@ class TestGMCarController: ) msgs = gmcan.create_gm_cc_spam_command( - packer, controller, cs, SimpleNamespace(accel=-0.5), SimpleNamespace(is_metric=True), lead_visible=False, + packer, controller, cs, SimpleNamespace(accel=-1.2), SimpleNamespace(is_metric=True), ) assert msgs == [] assert controller.apply_speed == 100 - def test_volt_cc_redneck_brakes_for_lead_inside_free_road_deadband(self): + def test_volt_cc_redneck_brakes_for_active_lead_inside_free_road_deadband(self): packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt]) controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0) cs = SimpleNamespace( @@ -1179,7 +1179,7 @@ class TestGMCarController: ) msgs = gmcan.create_gm_cc_spam_command( - packer, controller, cs, SimpleNamespace(accel=-0.5), SimpleNamespace(is_metric=True), lead_visible=True, + packer, controller, cs, SimpleNamespace(accel=-0.5), SimpleNamespace(is_metric=True), longitudinal_adjustment_active=True, ) assert len(msgs) == 1 @@ -1236,7 +1236,7 @@ class TestGMCarController: ) msgs = gmcan.create_gm_cc_spam_command( - packer, controller, cs, SimpleNamespace(accel=-1.36), SimpleNamespace(is_metric=True), + packer, controller, cs, SimpleNamespace(accel=-1.36), SimpleNamespace(is_metric=True), longitudinal_adjustment_active=True, ) assert len(msgs) == 1 diff --git a/opendbc_repo/opendbc/car/tests/test_can_fingerprint.py b/opendbc_repo/opendbc/car/tests/test_can_fingerprint.py index 48c47c8cdd..50155d3098 100644 --- a/opendbc_repo/opendbc/car/tests/test_can_fingerprint.py +++ b/opendbc_repo/opendbc/car/tests/test_can_fingerprint.py @@ -2,7 +2,7 @@ from types import SimpleNamespace import pytest from opendbc.car.can_definitions import CanData -from opendbc.car.car_helpers import FRAME_FINGERPRINT, _apply_starpilot_access_policy, _get_gm_stored_candidate_fallback, can_fingerprint +from opendbc.car.car_helpers import FRAME_FINGERPRINT, _apply_starpilot_access_policy, _get_gm_stored_candidate_fallback, _normalize_gm_suburban_camera_candidate, can_fingerprint from opendbc.car.fingerprints import _FINGERPRINTS as FINGERPRINTS from opendbc.car.gm.values import CAR as GM from opendbc.car.toyota.values import CAR as TOYOTA @@ -116,3 +116,14 @@ class TestCanFingerprint: candidate = _apply_starpilot_access_policy("CHEVROLET_VOLT_CC", SimpleNamespace(block_user=True)) assert candidate == "CHEVROLET_VOLT_CC" + + def test_gm_suburban_camera_variant_uses_vin_and_camera_bus_signature(self): + fingerprints = { + 0: {190: 6, 201: 8, 209: 7, 211: 2, 241: 6, 304: 1, 320: 3}, + 2: {0x24b: 8, 0x64b: 8}, + } + + assert _normalize_gm_suburban_camera_candidate(None, fingerprints, "1GNSKJKJXKR148371") == "CHEVROLET_SUBURBAN_CAMERA" + assert _normalize_gm_suburban_camera_candidate("GMC_YUKON", fingerprints, "1GNSKJKJXKR148371") == "CHEVROLET_SUBURBAN_CAMERA" + assert _normalize_gm_suburban_camera_candidate(None, fingerprints, "1GNSKCKC5KR255194") is None + assert _normalize_gm_suburban_camera_candidate(None, {0: fingerprints[0], 2: {0x320: 3}}, "1GNSKJKJXKR148371") is None diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 9e2d1a4e8d..5f793ac594 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -481,9 +481,19 @@ class Car: def _update_openpilot_lead_state(self, CC: car.CarControl) -> None: lead_visible = bool(CC.hudControl.leadVisible) + longitudinal_adjustment_active = lead_visible lead_distance = 0.0 lead_rel_speed = 0.0 + if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']: + longitudinal_adjustment_active = bool(self.sm['starpilotPlan'].trackingLead) + + if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']: + longitudinal_plan = self.sm['longitudinalPlan'] + longitudinal_adjustment_active |= bool( + longitudinal_plan.shouldStop or str(longitudinal_plan.longitudinalPlanSource) != "cruise" + ) + if self.sm.seen['radarState'] and self.sm.valid['radarState']: lead = self.sm['radarState'].leadOne if lead.status: @@ -498,6 +508,7 @@ class Car: self.CI.CS.openpilot_lead_visible = lead_visible self.CI.CS.openpilot_lead_distance = lead_distance self.CI.CS.openpilot_lead_rel_speed = lead_rel_speed + self.CI.CS.openpilot_longitudinal_adjustment_active = longitudinal_adjustment_active def _update_redneck_cruise(self, CS: car.CarState, CC: car.CarControl) -> None: if self.redneck_cruise is None: diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 0e281bb66c..c9662319f9 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -217,6 +217,38 @@ class TestRedneckCruise(unittest.TestCase): self.assertAlmostEqual(55.0 * CV.MPH_TO_MS * 1.01 + 1.5, target_speed) self.assertTrue(lead_present) + def test_card_does_not_treat_untracked_cruise_lead_as_longitudinal_adjustment(self): + sm = MagicMock() + sm.seen = {"starpilotPlan": True, "longitudinalPlan": True, "radarState": True} + sm.valid = sm.seen.copy() + sm.__getitem__.side_effect = { + "starpilotPlan": SimpleNamespace(trackingLead=False), + "longitudinalPlan": SimpleNamespace(shouldStop=False, longitudinalPlanSource="cruise"), + "radarState": SimpleNamespace(leadOne=SimpleNamespace(status=True, dRel=85.0, vRel=-0.5)), + }.__getitem__ + car_state = SimpleNamespace() + card = SimpleNamespace(sm=sm, CI=SimpleNamespace(CS=car_state)) + + Car._update_openpilot_lead_state(card, SimpleNamespace(hudControl=SimpleNamespace(leadVisible=True))) + + self.assertTrue(car_state.openpilot_lead_visible) + self.assertFalse(car_state.openpilot_longitudinal_adjustment_active) + + def test_card_marks_stop_plan_as_longitudinal_adjustment_without_lead(self): + sm = MagicMock() + sm.seen = {"starpilotPlan": True, "longitudinalPlan": True, "radarState": False} + sm.valid = sm.seen.copy() + sm.__getitem__.side_effect = { + "starpilotPlan": SimpleNamespace(trackingLead=False), + "longitudinalPlan": SimpleNamespace(shouldStop=True, longitudinalPlanSource="e2e"), + }.__getitem__ + car_state = SimpleNamespace() + card = SimpleNamespace(sm=sm, CI=SimpleNamespace(CS=car_state)) + + Car._update_openpilot_lead_state(card, SimpleNamespace(hudControl=SimpleNamespace(leadVisible=False))) + + self.assertTrue(car_state.openpilot_longitudinal_adjustment_active) + def test_card_target_speed_uses_slc_target_with_longitudinal_control(self): slc_target = 80.0 * CV.KPH_TO_MS starpilot_plan = SimpleNamespace( diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 8052c031b5..d2247d89ce 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -31,6 +31,16 @@ from openpilot.starpilot.system.wheel_controls import ( ) HYUNDAI_MAIN_CRUISE_AOL_CONFIRM_TIMEOUT_FRAMES = 100 +AOL_NON_BLOCKING_IMMEDIATE_DISABLE_ALERTS = { + f"speedTooLow/{ET.IMMEDIATE_DISABLE}", +} + + +def aol_blocked_by_immediate_disable(*alert_types) -> bool: + return any( + ET.IMMEDIATE_DISABLE in alert_type and alert_type not in AOL_NON_BLOCKING_IMMEDIATE_DISABLE_ALERTS + for alert_type in alert_types + ) class StarPilotCard: @@ -338,8 +348,9 @@ class StarPilotCard: self.always_on_lateral_enabled &= not hyundai_aol_needs_engagement or self.hyundai_aol_ready self.always_on_lateral_enabled &= sm["starpilotPlan"].lateralCheck self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1 - alert_types = sm["selfdriveState"].alertType + sm["starpilotSelfdriveState"].alertType - self.always_on_lateral_enabled &= ET.IMMEDIATE_DISABLE not in alert_types + self.always_on_lateral_enabled &= not aol_blocked_by_immediate_disable( + sm["selfdriveState"].alertType, sm["starpilotSelfdriveState"].alertType, + ) self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < starpilot_toggles.always_on_lateral_pause_speed) or carState.standstill self.always_on_lateral_enabled &= not self.error_log.is_file() diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 1a7a47f6ae..c1b81c92c7 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -1123,6 +1123,46 @@ def test_hyundai_main_aol_persists_after_brake_disengage_without_manual_aol_butt assert ret.alwaysOnLateralEnabled is True +def test_aol_persists_through_longitudinal_speed_too_low_disable(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="gm"), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + sm = make_sm() + sm["selfdriveState"].alertType = f"speedTooLow/{spc.ET.IMMEDIATE_DISABLE}" + + ret = card.update( + make_car_state(available=True), SimpleNamespace(distancePressed=False), sm, + make_toggles(always_on_lateral_main=True), + ) + + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + +def test_aol_still_stops_for_other_immediate_disables(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="gm"), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + sm = make_sm() + sm["selfdriveState"].alertType = f"controlsMismatch/{spc.ET.IMMEDIATE_DISABLE}" + + ret = card.update( + make_car_state(available=True), SimpleNamespace(distancePressed=False), sm, + make_toggles(always_on_lateral_main=True), + ) + + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is False + + def test_non_button_aol_platform_keeps_main_aol_when_main_cruise_is_mapped(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)