diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index e4d4e56..ed3e48c 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -37,26 +37,28 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, { {0x315, 2, 5}, // ch bus {0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan -const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, // 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}, {0x200, 0, 6}, // pt bus - {0x184, 2, 8}}; // camera bus + {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus + +const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus + {0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. RxCheck gm_rx_checks[] = { {.msg = {{0x184, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, {.msg = {{0x34A, 0, 5, .frequency = 10U}, { 0 }, { 0 }}}, {.msg = {{0x1E1, 0, 7, .frequency = 10U}, { 0 }, { 0 }}}, - {.msg = {{0xBE, 0, 6, .frequency = 10U}, // Volt, Silverado, Acadia Denali - {0xBE, 0, 7, .frequency = 10U}, // Bolt EUV - {0xBE, 0, 8, .frequency = 10U}}}, // Escalade + {.msg = {{0xF1, 0, 6, .frequency = 10U}, { 0 }, { 0 }}}, {.msg = {{0x1C4, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, {.msg = {{0xC9, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, }; const uint16_t GM_PARAM_HW_CAM = 1; const uint16_t GM_PARAM_HW_CAM_LONG = 2; +const uint16_t GM_PARAM_CC_LONG = 4; const uint16_t GM_PARAM_HW_ASCM_LONG = 8; const uint16_t GM_PARAM_NO_CAMERA = 16; const uint16_t GM_PARAM_NO_ACC = 32; @@ -75,6 +77,7 @@ 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_skip_relay_check = false; bool gm_force_ascm = false; @@ -97,7 +100,7 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } // ACC steering wheel buttons (GM_CAM is tied to the PCM) - if ((addr == 0x1E1) && !gm_pcm_cruise) { + if ((addr == 0x1E1) && (!gm_pcm_cruise || gm_cc_long)) { int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4; // enter controls on falling edge of set or rising edge of resume (avoids fault) @@ -140,7 +143,11 @@ static void gm_rx_hook(const CANPacket_t *to_push) { // Cruise check for CC only cars if ((addr == 0x3D1) && !gm_has_acc) { bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U; - cruise_engaged_prev = cruise_engaged; + if (gm_cc_long) { + pcm_cruise_check(cruise_engaged); + } else { + cruise_engaged_prev = cruise_engaged; + } } if (addr == 0xBD) { @@ -213,11 +220,16 @@ 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_pedal_long)) { + if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long || gm_cc_long)) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; - bool allowed_cancel = (button == 6) && cruise_engaged_prev; - if (!allowed_cancel) { + bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev; + // For standard CC, allow spamming of SET / RESUME + if (gm_cc_long) { + allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS); + } + + if (!allowed_btn) { tx = false; } } @@ -264,14 +276,22 @@ static safety_config gm_init(uint16_t param) { } gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG); - gm_pcm_cruise = ((gm_hw == GM_CAM) && !gm_cam_long && !gm_force_ascm && !gm_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_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) { - ret = gm_cam_long ? BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS) : BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS); + if (gm_cc_long) { + ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CC_LONG_TX_MSGS); + } else if (gm_cam_long) { + ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS); + } else { + ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS); + } } return ret; } diff --git a/panda/python/__init__.py b/panda/python/__init__.py index edb2141..046007b 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -229,6 +229,7 @@ class Panda: FLAG_GM_HW_CAM = 1 FLAG_GM_HW_CAM_LONG = 2 + FLAG_GM_CC_LONG = 4 FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred FLAG_GM_GAS_INTERCEPTOR = 256 diff --git a/panda/tests/safety/common.py b/panda/tests/safety/common.py index 25ada0f..fc99218 100644 --- a/panda/tests/safety/common.py +++ b/panda/tests/safety/common.py @@ -849,7 +849,7 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}): continue - if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmInterceptorSafety'}): + if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmInterceptorSafety', 'TestGmCcLongitudinalSafety'}): continue if attr.startswith('TestFord') and current_test.startswith('TestFord'): continue diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index a99a85d..6e97cae 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -309,5 +309,44 @@ class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafet return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values) +class TestGmCcLongitudinalSafety(TestGmCameraSafety): + TX_MSGS = [[384, 0], [481, 0], [388, 2]] + FWD_BLACKLISTED_ADDRS = {2: [384], 0: [388]} # block LKAS message and PSCMStatus + BUTTONS_BUS = 0 # tx only + + MAX_GAS = 3400 + MAX_REGEN = 1514 + INACTIVE_REGEN = 1554 + MAX_BRAKE = 400 + + def setUp(self): + self.packer = CANPackerPanda("gm_global_a_powertrain_generated") + self.packer_chassis = CANPackerPanda("gm_global_a_chassis") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_GM, Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_NO_ACC | Panda.FLAG_GM_CC_LONG) + self.safety.init_tests() + + def _pcm_status_msg(self, enable): + values = {"CruiseActive": enable} + return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values) + + def test_fwd_hook(self): + pass + + def test_buttons(self): + self.safety.set_controls_allowed(0) + for btn in range(8): + self.assertFalse(self._tx(self._button_msg(btn))) + + self.safety.set_controls_allowed(1) + for btn in range(8): + self.assertFalse(self._tx(self._button_msg(btn))) + + for enabled in (True, False): + for btn in (Buttons.RES_ACCEL, Buttons.DECEL_SET, Buttons.CANCEL): + self._rx(self._pcm_status_msg(enabled)) + self.assertEqual(enabled, self._tx(self._button_msg(btn))) + + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index aee576d..08299da 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,6 +1,7 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp, clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command @@ -11,6 +12,8 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert NetworkLocation = car.CarParams.NetworkLocation LongCtrlState = car.CarControl.Actuators.LongControlState +GearShifter = car.CarState.GearShifter +TransmissionType = car.CarParams.TransmissionType # Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s CAMERA_CANCEL_DELAY_FRAMES = 10 @@ -25,10 +28,12 @@ class CarController(CarControllerBase): self.apply_steer_last = 0 self.apply_gas = 0 self.apply_brake = 0 + self.apply_speed = 0 self.frame = 0 self.last_steer_frame = 0 self.last_button_frame = 0 self.cancel_counter = 0 + self.pedal_steady = 0. self.lka_steering_cmd_counter = 0 self.lka_icon_status_last = (False, False) @@ -39,6 +44,9 @@ class CarController(CarControllerBase): self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + # FrogPilot variables + self.params_ = Params() + @staticmethod def calc_pedal_command(accel: float, long_active: bool) -> float: if not long_active: return 0. @@ -122,6 +130,10 @@ class CarController(CarControllerBase): at_full_stop = CC.longActive and CS.out.standstill near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) + if self.CP.flags & GMFlags.CC_LONG.value: + 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: @@ -191,6 +203,7 @@ class CarController(CarControllerBase): new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake + new_actuators.speed = self.apply_speed self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index cea7798..ff7538a 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -1,5 +1,10 @@ +import math + +from cereal import log +from openpilot.common.conversions import Conversions as CV +from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import make_can_msg -from openpilot.selfdrive.car.gm.values import CAR +from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus def create_buttons(packer, bus, idx, button): @@ -171,3 +176,49 @@ def create_lka_icon_command(bus, active, critical, steer): else: dat = b"\x00\x00\x00" return make_can_msg(0x104c006c, dat, bus) + + +def create_gm_cc_spam_command(packer, controller, CS, actuators): + if controller.params_.get_bool("IsMetric"): + _CV = CV.MS_TO_KPH + RATE_UP_MAX = 0.04 + RATE_DOWN_MAX = 0.04 + else: + _CV = CV.MS_TO_MPH + RATE_UP_MAX = 0.2 + RATE_DOWN_MAX = 0.2 + + accel = actuators.accel * _CV # m/s/s to mph/s + speedSetPoint = int(round(CS.out.cruiseState.speed * _CV)) + + cruiseBtn = CruiseButtons.INIT + if speedSetPoint == CS.CP.minEnableSpeed and accel < -1: + cruiseBtn = CruiseButtons.CANCEL + controller.apply_speed = 0 + rate = 0.04 + elif accel < 0: + cruiseBtn = CruiseButtons.DECEL_SET + if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible + rate = RATE_DOWN_MAX + else: + rate = max(-1 / accel, RATE_DOWN_MAX) + controller.apply_speed = speedSetPoint - 1 + elif accel > 0: + cruiseBtn = CruiseButtons.RES_ACCEL + if speedSetPoint < (CS.out.vEgo * _CV) - 3.0: + rate = RATE_UP_MAX + else: + rate = max(1 / accel, RATE_UP_MAX) + controller.apply_speed = speedSetPoint + 1 + else: + controller.apply_speed = speedSetPoint + rate = float('inf') + + # Check rlogs closely - our message shouldn't show up on the pt bus for us + # Or bus 2, since we're forwarding... but I think it does + if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate): + controller.last_button_frame = controller.frame + idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV + return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)] + else: + return [] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 3dccda5..c4e58fe 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -109,7 +109,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0.] if candidate in CAMERA_ACC_CAR: - ret.experimentalLongitudinalAvailable = True + ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR ret.networkLocation = NetworkLocation.fwdCamera ret.radarUnavailable = True # no radar ret.pcmCruise = True @@ -239,6 +239,31 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiV = [0.1, 0.1] ret.longitudinalTuning.kf = 0.15 ret.stoppingDecelRate = 0.8 + else: # Pedal used for SNG, ACC for longitudinal control otherwise + ret.startingState = True + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 + + elif candidate in CC_ONLY_CAR: + ret.flags |= GMFlags.CC_LONG.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CC_LONG + ret.radarUnavailable = True + ret.experimentalLongitudinalAvailable = False + ret.minEnableSpeed = 24 * CV.MPH_TO_MS + ret.openpilotLongitudinalControl = True + ret.pcmCruise = False + + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s + ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) + ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this + ret.longitudinalActuatorDelayUpperBound = 2. + + ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph + ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC @@ -284,6 +309,9 @@ class CarInterface(CarInterfaceBase): if ret.vEgo < self.CP.minSteerSpeed: events.add(EventName.belowSteerSpeed) + 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 \ diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 1221185..4737755 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -235,6 +235,7 @@ class CanBus: class GMFlags(IntFlag): PEDAL_LONG = 1 + CC_LONG = 2 NO_CAMERA = 4 NO_ACCELERATOR_POS_MSG = 8 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a589a2d..0eda445 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -582,6 +582,9 @@ class Controls: t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) + if len(long_plan.speeds): + actuators.speed = long_plan.speeds[-1] + # Steering PID loop and lateral MPC self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) actuators.curvature = self.desired_curvature