diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index be79b9a8b..ab4288706 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -6,6 +6,9 @@ from opendbc.car.gm import gmcan from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons from opendbc.car.interfaces import CarControllerBase +from opendbc.sunnypilot.car.gm.carcontroller_ext import GasInterceptorCarController +from opendbc.sunnypilot.car.gm.values_ext import GMFlagsSP +from opendbc.sunnypilot.car.gm.icbm import IntelligentCruiseButtonManagementInterface VisualAlert = structs.CarControl.HUDControl.VisualAlert NetworkLocation = structs.CarParams.NetworkLocation @@ -17,7 +20,7 @@ CAMERA_CANCEL_DELAY_FRAMES = 10 MIN_STEER_MSG_INTERVAL_MS = 15 -class CarController(CarControllerBase): +class CarController(CarControllerBase, GasInterceptorCarController, IntelligentCruiseButtonManagementInterface): def __init__(self, dbc_names, CP, CP_SP): super().__init__(dbc_names, CP, CP_SP) self.start_time = 0. @@ -37,6 +40,9 @@ class CarController(CarControllerBase): self.packer_obj = CANPacker(DBC[self.CP.carFingerprint][Bus.radar]) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint][Bus.chassis]) + GasInterceptorCarController.__init__(self, CP, CP_SP) + IntelligentCruiseButtonManagementInterface.__init__(self, CP, CP_SP) + def update(self, CC, CC_SP, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -108,8 +114,14 @@ class CarController(CarControllerBase): at_full_stop = at_full_stop and stopping friction_brake_bus = CanBus.POWERTRAIN - # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) + pedal_interceptor_active = self.CP.enableGasInterceptorDEPRECATED and (self.CP_SP.flags & GMFlagsSP.NON_ACC) + if pedal_interceptor_active: + self.apply_brake = 0 + + # GasRegenCmdActive needs to reflect whether openpilot is actually commanding longitudinal torque + gas_regen_active = CC.longActive and CC.enabled + if not pedal_interceptor_active: + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, gas_regen_active, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP)) @@ -148,6 +160,11 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL)) + # Intelligent Cruise Button Management (stock ACC speed nudging) + if self.CP_SP.intelligentCruiseButtonManagementAvailable: + can_sends.extend(IntelligentCruiseButtonManagementInterface.update( + self, CC, CC_SP, CS, self.packer_pt, self.frame, self.last_button_frame)) + if self.CP.networkLocation == NetworkLocation.fwdCamera: # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1 if self.frame % 10 == 0: @@ -159,5 +176,8 @@ class CarController(CarControllerBase): new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake + # sunnypilot gas interceptor / NON_ACC extensions + self.extend_with_interceptor(CC, CS, new_actuators, can_sends) + self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 909e7d40a..c22d60054 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -10,12 +10,24 @@ from opendbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAM from opendbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType -from opendbc.sunnypilot.car.gm.interface_ext import CarInterfaceExt from opendbc.sunnypilot.car.gm.values_ext import GMFlagsSP, GMSafetyFlagsSP TransmissionType = structs.CarParams.TransmissionType NetworkLocation = structs.CarParams.NetworkLocation +PEDAL_MSG = 0x201 +NON_ACC_ICBM_CARS = { + CAR.CHEVROLET_BOLT_NON_ACC, + CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN, + CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, + CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN, + CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, + CAR.CADILLAC_CT6_NON_ACC_1ST_GEN, + CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, + CAR.CADILLAC_XT5_NON_ACC_1ST_GEN, +} + + # sunnypilot-specific torque parameters for Bolt cars that actually use the d parameter NON_LINEAR_TORQUE_PARAMS_SP = { CAR.CHEVROLET_BOLT_NON_ACC: [2.24, 1.1, 0.28, -0.07], @@ -30,14 +42,13 @@ NON_LINEAR_TORQUE_PARAMS = { } -class CarInterface(CarInterfaceBase, CarInterfaceExt): +class CarInterface(CarInterfaceBase): CarState = CarState CarController = CarController RadarInterface = RadarInterface def __init__(self, CP, CP_SP): CarInterfaceBase.__init__(self, CP, CP_SP) - CarInterfaceExt.__init__(self, CP, CarInterfaceBase) @staticmethod def get_pid_accel_limits(CP, CP_SP, current_speed, cruise_speed): @@ -102,6 +113,8 @@ class CarInterface(CarInterfaceBase, CarInterfaceExt): ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)] ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] + if PEDAL_MSG in fingerprint[0]: + ret.enableGasInterceptorDEPRECATED = True if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct @@ -149,6 +162,8 @@ class CarInterface(CarInterfaceBase, CarInterfaceExt): # Tuning ret.longitudinalTuning.kiV = [2.4, 1.5] + ret.intelligentCruiseButtonManagementAvailable = ret.pcmCruise or candidate in NON_ACC_ICBM_CARS + # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to opendbc/car/tests/routes.py, we can remove it from this list. @@ -232,13 +247,27 @@ class CarInterface(CarInterfaceBase, CarInterfaceExt): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.dashcamOnly = True # Needs steerRatio, tireStiffness, and lat accel factor tuning + if ret.enableGasInterceptorDEPRECATED: + ret.networkLocation = NetworkLocation.fwdCamera + ret.alphaLongitudinalAvailable = True + ret.minEnableSpeed = -1 + ret.pcmCruise = False + ret.openpilotLongitudinalControl = True + ret.autoResumeSng = True + ret.safetyConfigs[0].safetyParam |= GMSafetyFlagsSP.GAS_INTERCEPTOR.value + ret.safetyConfigs[0].safetyParam |= GMSafetyFlagsSP.PEDAL_LONG.value + + # Pedal interceptor tuning + ret.longitudinalTuning.kiBP = [0., 3., 6., 35.] + ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33] + ret.stoppingDecelRate = 0.8 + return ret @staticmethod def _get_params_sp(stock_cp: structs.CarParams, ret: structs.CarParamsSP, candidate, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_sp: bool, docs: bool) -> structs.CarParamsSP: - if candidate in (CAR.CHEVROLET_MALIBU_NON_ACC_9TH_GEN, CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN, - CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN): + if ret.flags & GMFlagsSP.NON_ACC: stock_cp.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning) @@ -248,15 +277,29 @@ class CarInterface(CarInterfaceBase, CarInterfaceExt): # NON_ACC vehicles should use camera car speed thresholds if ret.flags & GMFlagsSP.NON_ACC: stock_cp.dashcamOnly = False - stock_cp.alphaLongitudinalAvailable = False stock_cp.networkLocation = NetworkLocation.fwdCamera - stock_cp.openpilotLongitudinalControl = False - stock_cp.pcmCruise = True stock_cp.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value - ret.safetyParam |= GMSafetyFlagsSP.NON_ACC + ret.safetyParam |= GMSafetyFlagsSP.NON_ACC.value stock_cp.minEnableSpeed = 24 * CV.MPH_TO_MS # 24 mph stock_cp.minSteerSpeed = 3.0 # ~6 mph + # Check if pedal interceptor is present + has_pedal = PEDAL_MSG in fingerprint[0] + + if has_pedal: + # With pedal interceptor: enable alpha long, disable PCM cruise + stock_cp.alphaLongitudinalAvailable = True + stock_cp.openpilotLongitudinalControl = True + stock_cp.pcmCruise = False + stock_cp.minEnableSpeed = -1. + ret.safetyParam |= GMSafetyFlagsSP.GAS_INTERCEPTOR.value + ret.safetyParam |= GMSafetyFlagsSP.PEDAL_LONG.value + else: + # Without pedal interceptor: disable longitudinal + stock_cp.alphaLongitudinalAvailable = False + stock_cp.openpilotLongitudinalControl = False + stock_cp.pcmCruise = True + # dashcamOnly platforms: untested platforms, need user validations if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN, CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, diff --git a/opendbc_repo/opendbc/sunnypilot/car/gm/carcontroller_ext.py b/opendbc_repo/opendbc/sunnypilot/car/gm/carcontroller_ext.py new file mode 100644 index 000000000..d8940c3ed --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/gm/carcontroller_ext.py @@ -0,0 +1,15 @@ +""" +Minimal GM carcontroller extensions for branches without pedal/interceptor support. +""" + + +class GasInterceptorCarController: + def __init__(self, CP, CP_SP): + # No-op initializer; kept for interface compatibility + self.frame = 0 + self.CP = CP + self.CP_SP = CP_SP + + def extend_with_interceptor(self, CC, CS, actuators, can_sends): + # Stubbed out for branches without pedal/interceptor support + return diff --git a/opendbc_repo/opendbc/sunnypilot/car/gm/icbm.py b/opendbc_repo/opendbc/sunnypilot/car/gm/icbm.py new file mode 100644 index 000000000..db7b54600 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/gm/icbm.py @@ -0,0 +1,81 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from opendbc.car import DT_CTRL, structs +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.can_definitions import CanData +from opendbc.car.gm import gmcan +from opendbc.car.gm.values import CanBus, CruiseButtons +from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase +from opendbc.sunnypilot.car.gm.values_ext import GMFlagsSP + +SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState + +BUTTONS = { + SendButtonState.increase: CruiseButtons.RES_ACCEL, + SendButtonState.decrease: CruiseButtons.DECEL_SET, +} + +# Match the aggressive cadence used by the legacy GM cruise spam tune +BUTTON_SPAM_INTERVAL = 0.2 + + +class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase): + def __init__(self, CP, CP_SP): + super().__init__(CP, CP_SP) + + def _next_button_counter(self, CS) -> int: + # Predict the next rolling counter slot used by ASCMSteeringButton + return (CS.buttons_counter + 1) % 4 + + def _compute_redneck_button(self, CC, CS) -> CruiseButtons | None: + accel = CC.actuators.accel + v_ego = CS.out.vEgo + + # TODO: detect metric clusters; default to MPH like the original tune + ms_to_speed = CV.MS_TO_MPH + + speed_set = int(round(CS.out.cruiseState.speed * ms_to_speed)) + desired_set = int(round((v_ego * 1.01 + 3 * accel) * ms_to_speed)) + + if self.CP.minEnableSpeed - (desired_set / ms_to_speed) > 3.25: + return CruiseButtons.CANCEL + + if desired_set < speed_set and speed_set > self.CP.minEnableSpeed * ms_to_speed + 1: + return CruiseButtons.DECEL_SET + + if desired_set > speed_set: + return CruiseButtons.RES_ACCEL + + return None + + def update(self, CC, CC_SP, CS, packer, frame, last_button_frame) -> list[CanData]: + can_sends: list[CanData] = [] + self.CC_SP = CC_SP + self.ICBM = CC_SP.intelligentCruiseButtonManagement + self.frame = frame + self.last_button_frame = last_button_frame + + if self.CP_SP.flags & GMFlagsSP.NON_ACC: + redneck_button = self._compute_redneck_button(CC, CS) + if redneck_button: + interval = 1.0 if abs(CC.actuators.accel) <= 0.15 else 0.2 + if (self.frame - self.last_button_frame) * DT_CTRL > interval: + idx = self._next_button_counter(CS) + can_sends.append(gmcan.create_buttons(packer, CanBus.POWERTRAIN, idx, redneck_button)) + self.last_button_frame = self.frame + return can_sends + + if self.ICBM.sendButton != SendButtonState.none: + send_button = BUTTONS[self.ICBM.sendButton] + + if (self.frame - self.last_button_frame) * DT_CTRL > BUTTON_SPAM_INTERVAL: + idx = self._next_button_counter(CS) + can_sends.append(gmcan.create_buttons(packer, CanBus.POWERTRAIN, idx, send_button)) + self.last_button_frame = self.frame + + return can_sends