diff --git a/cereal/car.capnp b/cereal/car.capnp index aff19ad1..05dd2e87 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -124,6 +124,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { leadDeparting @124; noLaneAvailable @125; openpilotCrashed @126; + pedalInterceptorNoBrake @128; torqueNNLoad @130; radarCanErrorDEPRECATED @15; diff --git a/panda/board/safety.h b/panda/board/safety.h index 58f2ed7b..a2e803ea 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -544,7 +544,7 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit } bool longitudinal_interceptor_checks(const CANPacket_t *to_send) { - return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)); + return (!get_longitudinal_allowed() || brake_pressed_prev) && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)); } // Safety checks for torque-based steering commands diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 4bf11dbc..3aeadeff 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -41,15 +41,20 @@ const LongitudinalLimits *gm_long_limits; const int GM_STANDSTILL_THRSLD = 10; // 0.311kph -const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus +// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches +// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state +const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file +#define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks + +const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus {0x315, 2, 5}, // ch bus {0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan -const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus +const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus -const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus +const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus @@ -74,6 +79,10 @@ const uint16_t GM_PARAM_HW_CAM = 1; const uint16_t GM_PARAM_HW_CAM_LONG = 2; const uint16_t GM_PARAM_HW_SDGM = 4; const uint16_t GM_PARAM_CC_LONG = 8; +const uint16_t GM_PARAM_HW_ASCM_LONG = 16; +const uint16_t GM_PARAM_NO_ACC = 64; +const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred +const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256; enum { GM_BTN_UNPRESS = 1, @@ -85,7 +94,10 @@ enum { enum {GM_ASCM, GM_CAM, GM_SDGM} gm_hw = GM_ASCM; bool gm_cam_long = false; bool gm_pcm_cruise = false; +bool gm_has_acc = true; +bool gm_pedal_long = false; bool gm_cc_long = false; +bool gm_force_ascm = false; static void handle_gm_wheel_buttons(const CANPacket_t *to_push) { int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4; @@ -145,23 +157,43 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } if (addr == 0x1C4) { - gas_pressed = GET_BYTE(to_push, 5) != 0U; + if (!enable_gas_interceptor) { + gas_pressed = GET_BYTE(to_push, 5) != 0U; + } // enter controls on rising edge of ACC, exit controls when ACC off - if (gm_pcm_cruise) { + if (gm_pcm_cruise && gm_has_acc) { bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U; pcm_cruise_check(cruise_engaged); } } + // Cruise check for CC only cars + if ((addr == 0x3D1) && !gm_has_acc) { + bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U; + if (gm_cc_long) { + pcm_cruise_check(cruise_engaged); + } else { + cruise_engaged_prev = cruise_engaged; + } + } + if (addr == 0xBD) { regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; } + // Pedal Interceptor + if ((addr == 0x201) && enable_gas_interceptor) { + int gas_interceptor = GM_GET_INTERCEPTOR(to_push); + gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD; + gas_interceptor_prev = gas_interceptor; +// gm_pcm_cruise = false; + } + bool stock_ecu_detected = (addr == 0x180); // ASCMLKASteeringCmd // Check ASCMGasRegenCmd only if we're blocking it - if (!gm_pcm_cruise && (addr == 0x2CB)) { + if (!gm_pcm_cruise && !gm_pedal_long && (addr == 0x2CB)) { stock_ecu_detected = true; } generic_rx_checks(stock_ecu_detected); @@ -193,6 +225,13 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } } + // GAS: safety check (interceptor) + if (addr == 0x200) { + if (longitudinal_interceptor_checks(to_send)) { + tx = 0; + } + } + // GAS/REGEN: safety check if (addr == 0x2CB) { bool apply = GET_BIT(to_send, 0U); @@ -209,7 +248,7 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } // BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal - if ((addr == 0x1E1) && (gm_pcm_cruise || gm_cc_long)) { + if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long || gm_cc_long)) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev; @@ -263,7 +302,9 @@ static safety_config gm_init(uint16_t param) { gm_hw = GM_ASCM; } - if (gm_hw == GM_ASCM) { + gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG); + + if (gm_hw == GM_ASCM || gm_force_ascm) { if (sport_mode) { gm_long_limits = &GM_ASCM_LONG_LIMITS_SPORT; } else { @@ -278,9 +319,13 @@ static safety_config gm_init(uint16_t param) { } else { } + gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG); gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG); gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG) && !gm_cc_long; gm_pcm_cruise = ((gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) && !gm_force_ascm && !gm_pedal_long) || (gm_hw == GM_SDGM); + gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA); + gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); + enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR); safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS); if (gm_hw == GM_CAM) { diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 1b62830f..96f44ce2 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -236,6 +236,9 @@ class Panda: FLAG_GM_HW_SDGM = 4 FLAG_GM_CC_LONG = 8 FLAG_GM_HW_ASCM_LONG = 16 + FLAG_GM_NO_ACC = 64 + FLAG_GM_PEDAL_LONG = 128 + FLAG_GM_GAS_INTERCEPTOR = 256 FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 52214a32..76ff7078 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,10 +1,10 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.numpy_fast import interp +from openpilot.common.numpy_fast import interp, clip from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone @@ -53,6 +53,21 @@ class CarController: self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz self.accel_g = 0.0 + @staticmethod + def calc_pedal_command(accel: float, long_active: bool) -> float: + if not long_active: return 0. + + zero = 0.15625 # 40/256 + if accel > 0.: + # Scales the accel from 0-1 to 0.156-1 + pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.) + else: + # if accel is negative, -0.1 -> 0.015625 + pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen + + return pedal_gas + + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators accel = actuators.accel @@ -102,6 +117,7 @@ class CarController: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping + interceptor_gas_cmd = 0 # Pitch compensated acceleration; # TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay @@ -125,6 +141,9 @@ class CarController: # FIXME: brakes aren't applied immediately when enabling at a stop if stopping: self.apply_gas = self.params.INACTIVE_REGEN + if self.CP.carFingerprint in CC_ONLY_CAR: + # gas interceptor only used for full long control on cars without ACC + interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive) idx = (self.frame // 4) % 4 @@ -134,6 +153,8 @@ class CarController: if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed: # Using extend instead of append since the message is only sent intermittently can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators)) + if self.CP.enableGasInterceptor: + can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx)) if self.CP.carFingerprint not in CC_ONLY_CAR: friction_brake_bus = CanBus.CHASSIS # GM Camera exceptions @@ -174,6 +195,15 @@ class CarController: if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) + # TODO: integrate this with the code block below? + if ( + (self.CP.flags & GMFlags.PEDAL_LONG.value) # Always cancel stock CC when using pedal interceptor + or (self.CP.flags & GMFlags.CC_LONG.value and not CC.enabled) # Cancel stock CC if OP is not active + ) and CS.out.cruiseState.enabled: + if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: + self.last_button_frame = self.frame + can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL)) + else: # While car is braking, cancel button causes ECM to enter a soft disable state with a fault status. # A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7eae2c98..2521ef71 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -9,6 +9,7 @@ from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRES TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation +GearShifter = car.CarState.GearShifter STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS @@ -27,6 +28,7 @@ class CarState(CarStateBase): self.buttons_counter = 0 # FrogPilot variables + self.single_pedal_mode = False def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables): ret = car.CarState.new_message() @@ -80,10 +82,15 @@ class CarState(CarStateBase): # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 + self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 - ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. - threshold = 15 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 - ret.gasPressed = ret.gas > threshold + if self.CP.enableGasInterceptor: + ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. + threshold = 15 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 + ret.gasPressed = ret.gas > threshold + else: + ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. + ret.gasPressed = ret.gas > 1e-5 ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] @@ -230,13 +237,21 @@ class CarState(CarStateBase): ] if CP.transmissionType == TransmissionType.direct: - messages.append(("EBCMRegenPaddle", 50)) + messages += [ + ("EBCMRegenPaddle", 50), + ("EVDriveMode", 0), + ] if CP.carFingerprint in CC_ONLY_CAR: messages += [ ("ECMCruiseControl", 10), ] + if CP.enableGasInterceptor: + messages += [ + ("GAS_SENSOR", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN) @staticmethod diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 231180a7..53a7d9b8 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -22,6 +22,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D ACCELERATOR_POS_MSG = 0xbe +PEDAL_MSG = 0x201 NON_LINEAR_TORQUE_PARAMS = { CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], @@ -95,6 +96,10 @@ class CarInterface(CarInterfaceBase): ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] + if PEDAL_MSG in fingerprint[0]: + ret.enableGasInterceptor = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR + if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: @@ -152,12 +157,9 @@ class CarInterface(CarInterfaceBase): # Tuning ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiV = [0.36] - - # 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 selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} or \ - (ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable) + if ret.enableGasInterceptor: + # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_ASCM_LONG # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -259,6 +261,10 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if ret.enableGasInterceptor: + # ACC Bolts use pedal for full longitudinal control, not just sng + ret.flags |= GMFlags.PEDAL_LONG.value + elif candidate == CAR.SILVERADO: ret.mass = 2450. ret.wheelbase = 3.75 @@ -321,6 +327,26 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.7 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if ret.enableGasInterceptor: + ret.networkLocation = NetworkLocation.fwdCamera + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM + ret.minEnableSpeed = -1 + ret.pcmCruise = False + ret.openpilotLongitudinalControl = True + ret.stoppingControl = True + ret.autoResumeSng = True + + if candidate in CC_ONLY_CAR: + ret.flags |= GMFlags.PEDAL_LONG.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG + # Note: Low speed, stop and go not tested. Should be fairly smooth on highway + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [0.35, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.0] + ret.longitudinalTuning.kiV = [0.1, 0.1] + ret.longitudinalTuning.kf = 0.15 + ret.stoppingDecelRate = 0.8 + elif candidate in CC_ONLY_CAR: ret.flags |= GMFlags.CC_LONG.value ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CC_LONG @@ -391,6 +417,12 @@ class CarInterface(CarInterfaceBase): if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled: events.add(EventName.speedTooLow) + if (self.CP.flags & GMFlags.PEDAL_LONG.value) and \ + self.CP.transmissionType == TransmissionType.direct and \ + not self.CS.single_pedal_mode and \ + c.longActive: + events.add(EventName.pedalInterceptorNoBrake) + ret.events = events.to_msg() return ret diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index d8cbb558..917588b4 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -184,6 +184,7 @@ class CanBus: DROPPED = 192 class GMFlags(IntFlag): + PEDAL_LONG = 1 CC_LONG = 2 NO_ACCELERATOR_POS_MSG = 8 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index b78eb2ca..de6486bd 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1028,6 +1028,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { Priority.HIGH, VisualAlert.none, AudibleAlert.none, .1), }, + EventName.pedalInterceptorNoBrake: { + ET.WARNING: Alert( + "Braking Unavailable", + "Shift to L", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.), + }, + EventName.torqueNNLoad: { ET.PERMANENT: torque_nn_load_alert, },