diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index fef03f10f..d7694f708 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index ffc6b083a..2a9d9ce54 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index ffc6b083a..2a9d9ce54 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index fef03f10f..d7694f708 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index b28fbb2a8..bf9b31336 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 4fd62917e..edf594279 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote.bin b/panda/board/obj/panda_h7_remote.bin index 0fd036317..25b3c7c84 100755 Binary files a/panda/board/obj/panda_h7_remote.bin and b/panda/board/obj/panda_h7_remote.bin differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 30d3a7a3a..05a88f6dc 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed new file mode 100644 index 000000000..bfffeef84 Binary files /dev/null and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 687461434..ddb4786c9 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -48,16 +48,16 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, { {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus {0x315, 2, 5}}; // ch bus -const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus +const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x370, 0, 6}, {0x200, 0, 6}, {0x1E1, 0, 7}, {0x3D1, 0, 8}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // 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}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus +const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, {0x3D1, 0, 8}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x315, 2, 5}, {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x184, 2, 8}}; // camera bus -const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus +const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x370, 0, 6}, {0x1E1, 0, 7}, {0x3D1, 0, 8}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. @@ -86,6 +86,14 @@ const uint16_t GM_PARAM_HW_SDGM = 1024; const uint16_t GM_PARAM_BOLT_2017 = 2048; const uint16_t GM_PARAM_BOLT_2022_PEDAL = 4096; const uint16_t GM_PARAM_REMOTE_START_BOOTS_COMMA = 8192; +const uint16_t GM_PARAM_PANDA_3D1_SCHED = 16384; + +const uint32_t GM_3D1_PERIOD_US = 100000U; +const uint32_t GM_3D1_TX_OFFSET_US = 0U; +const uint32_t GM_3D1_LOCK_TOLERANCE_US = 20000U; + +void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); +void can_set_checksum(CANPacket_t *packet); enum { GM_BTN_UNPRESS = 1, @@ -111,6 +119,41 @@ bool gm_bolt_2022_pedal = false; bool gm_ascm_int = false; bool gm_force_brake_c9 = false; bool gm_remote_start_boots_comma = false; +bool gm_panda_3d1_sched = false; + +bool gm_3d1_spoof_valid = false; +bool gm_3d1_internal_tx = false; +uint8_t gm_3d1_spoof_data[8] = {0U}; +uint32_t gm_3d1_next_tx_us = 0U; +uint32_t gm_3d1_expected_stock_us = 0U; +uint32_t gm_3d1_last_stock_us = 0U; +bool gm_3d1_phase_locked = false; + +static void gm_try_send_3d1_spoof(uint32_t now_us) { + if (!(gm_panda_3d1_sched && gm_3d1_spoof_valid && (gm_3d1_next_tx_us != 0U))) { + return; + } + + if ((int32_t)(now_us - gm_3d1_next_tx_us) < 0) { + return; + } + + CANPacket_t to_send = {0}; + to_send.returned = 0U; + to_send.rejected = 0U; + to_send.extended = 0U; + to_send.addr = 0x3D1U; + to_send.bus = 0U; + to_send.data_len_code = 8U; + (void)memcpy(to_send.data, gm_3d1_spoof_data, 8U); + can_set_checksum(&to_send); + + gm_3d1_internal_tx = true; + can_send(&to_send, 0U, false); + gm_3d1_internal_tx = false; + + gm_3d1_next_tx_us += GM_3D1_PERIOD_US; +} static void gm_rx_hook(const CANPacket_t *to_push) { if (GET_BUS(to_push) == 0U) { @@ -183,12 +226,35 @@ static void gm_rx_hook(const CANPacket_t *to_push) { // Cruise check for CC only cars if ((addr == 0x3D1) && !gm_has_acc) { + uint32_t now_us = microsecond_timer_get(); + gm_3d1_last_stock_us = now_us; 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 (gm_panda_3d1_sched) { + if (!gm_3d1_phase_locked) { + gm_3d1_phase_locked = true; + gm_3d1_expected_stock_us = now_us + GM_3D1_PERIOD_US; + } else { + int32_t phase_err_us = (int32_t)(now_us - gm_3d1_expected_stock_us); + if (phase_err_us < 0) { + phase_err_us = -phase_err_us; + } + + if ((uint32_t)phase_err_us <= GM_3D1_LOCK_TOLERANCE_US) { + gm_3d1_expected_stock_us += GM_3D1_PERIOD_US; + } else { + gm_3d1_expected_stock_us = now_us + GM_3D1_PERIOD_US; + } + } + + gm_3d1_next_tx_us = now_us + GM_3D1_TX_OFFSET_US; + gm_try_send_3d1_spoof(now_us); + } } if (addr == 0xBD) { @@ -291,6 +357,28 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } } + // Cruise status spoofing only for non-ACC (CC-only) paths + if (addr == 0x3D1) { + bool allowed_cruise_status = !gm_has_acc; + if (!allowed_cruise_status) { + tx = false; + } else if (gm_panda_3d1_sched) { + if (gm_3d1_internal_tx) { + tx = true; + } else { + uint32_t now_us = microsecond_timer_get(); + (void)memcpy(gm_3d1_spoof_data, to_send->data, 8U); + gm_3d1_spoof_valid = true; + if (gm_3d1_next_tx_us == 0U) { + gm_3d1_next_tx_us = now_us + GM_3D1_TX_OFFSET_US; + } + bool stock_stale = (gm_3d1_last_stock_us == 0U) || (get_ts_elapsed(now_us, gm_3d1_last_stock_us) > 300000U); + bool scheduler_ready = gm_3d1_phase_locked && !stock_stale; + tx = !scheduler_ready; + } + } + } + // REGEN PADDLE if (addr == 0xBD) { bool regen_apply = GET_BIT(to_send, 7) || GET_BIT(to_send, 6) || GET_BIT(to_send, 5) || GET_BIT(to_send, 4); @@ -317,26 +405,32 @@ static int gm_fwd_hook(int bus_num, int addr) { if (bus_num == 0) { // block PSCMStatus; forwarded through openpilot to hide an alert from the camera bool is_pscm_msg = (addr == 0x184); - if (!is_pscm_msg) { + // For non-ACC camera/SDGM paths, keep stock ECMCruiseControl off camera side + // so openpilot's spoofed 0x3D1 is the only cruise-status source there. + bool is_ecm_cruise_status_msg = (addr == 0x3D1) && !gm_has_acc; + if (!is_pscm_msg && !is_ecm_cruise_status_msg) { bus_fwd = 2; } } if (bus_num == 2) { bool is_lkas_msg = (addr == 0x180); - bool block_msg = false; - if (gm_bolt_2022_pedal) { - // Block 0x370 only for experimental long without pedal interceptor - bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB); - if (gm_cam_long && !enable_gas_interceptor) { - is_acc_msg = is_acc_msg || (addr == 0x370); - } - block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long); - } else { - // block lkas message and acc messages if gm_cam_long, forward all others - bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB) || (addr == 0x370); - block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long); + bool is_acc_status_msg = (addr == 0x370); + bool is_acc_actuation_msg = (addr == 0x315) || (addr == 0x2CB); + + // Block steering if we are controlling LKA + bool block_msg = is_lkas_msg; + + // Block Dashboard Status if we are in Native Long OR Pedal Long + if (gm_cam_long || gm_pedal_long) { + block_msg |= is_acc_status_msg; } + + // Block Native Actuation ONLY if we are in Native Long (not Pedal) + if (gm_cam_long) { + block_msg |= is_acc_actuation_msg; + } + if (!block_msg) { bus_fwd = 0; } @@ -376,6 +470,14 @@ static safety_config gm_init(uint16_t param) { gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); gm_force_brake_c9 = GET_FLAG(param, GM_PARAM_FORCE_BRAKE_C9); gm_remote_start_boots_comma = GET_FLAG(param, GM_PARAM_REMOTE_START_BOOTS_COMMA); + gm_panda_3d1_sched = GET_FLAG(param, GM_PARAM_PANDA_3D1_SCHED) && gm_pedal_long && !gm_has_acc && !gm_bolt_2022_pedal; + + gm_3d1_spoof_valid = false; + gm_3d1_internal_tx = false; + gm_3d1_next_tx_us = 0U; + gm_3d1_expected_stock_us = 0U; + gm_3d1_last_stock_us = 0U; + gm_3d1_phase_locked = false; 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 088ee8689..4a99c58e7 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -243,6 +243,7 @@ class Panda: FLAG_GM_BOLT_2017 = 2048 FLAG_GM_BOLT_2022_PEDAL = 4096 FLAG_GM_REMOTE_START_BOOTS_COMMA = 8192 + FLAG_GM_PANDA_3D1_SCHED = 16384 FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 817b457f0..5be506e5b 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -368,7 +368,21 @@ class CarController(CarControllerBase): if now_nanos - self.last_steer_ts_ns >= flush_gap_ns: can_sends.extend(paddle_sends) + spoof_ecm_cruise_cars = { + CAR.CHEVROLET_BOLT_CC_2017, + CAR.CHEVROLET_BOLT_CC_2019_2021, + CAR.CHEVROLET_BOLT_CC_2022_2023, + CAR.CHEVROLET_MALIBU_HYBRID_CC, + } + non_acc_pedal_long = (self.CP.flags & GMFlags.PEDAL_LONG.value) and self.CP.carFingerprint in spoof_ecm_cruise_cars and self.CP.enableGasInterceptor + if non_acc_pedal_long and self.frame % 4 == 0: + spoof_enabled = True + spoof_set_speed_kph = hud_v_cruise * CV.MS_TO_KPH + can_sends.append(gmcan.create_ecm_cruise_control_command( + self.packer_pt, CanBus.POWERTRAIN, spoof_enabled, spoof_set_speed_kph)) + if self.CP.openpilotLongitudinalControl: + # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping @@ -422,7 +436,7 @@ class CarController(CarControllerBase): accel = clip(actuators.accel + accel_due_to_pitch, self.params.ACCEL_MIN, accel_max) torque = self.tireRadius * ((self.mass*accel) + (0.5*self.coeffDrag*self.frontalArea*self.airDensity*CS.out.vEgo**2)) - + scaled_torque = torque + self.params.ZERO_GAS apply_gas_torque = clip(scaled_torque, self.params.MAX_ACC_REGEN, gas_max) BRAKE_SWITCH = int(round(interp(CS.out.vEgo, self.params.BRAKE_SWITCH_LOOKUP_BP, self.params.BRAKE_SWITCH_LOOKUP_V))) @@ -467,7 +481,7 @@ class CarController(CarControllerBase): friction_brake_bus = CanBus.CHASSIS # GM Camera exceptions # TODO: can we always check the longControlState? - if self.CP.networkLocation == NetworkLocation.fwdCamera and self.CP.carFingerprint not in CC_ONLY_CAR: + if self.CP.networkLocation == NetworkLocation.fwdCamera: at_full_stop = at_full_stop and stopping friction_brake_bus = CanBus.POWERTRAIN if self.CP.carFingerprint in SDGM_CAR: @@ -488,10 +502,11 @@ class CarController(CarControllerBase): 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)) - # Send dashboard UI commands (ACC status) + is_bolt_acc_pedal = self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL + if self.CP.carFingerprint not in CC_ONLY_CAR or is_bolt_acc_pedal: send_fcw = hud_alert == VisualAlert.fcw - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, - hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw)) + can_sends.append(gmcan.create_acc_dashboard_command( + self.packer_pt, CanBus.POWERTRAIN, CC.enabled, hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw)) else: # to keep accel steady for logs when not sending gas accel += self.accel_g diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f1e94af90..8958c5513 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -34,6 +34,8 @@ class CarState(CarStateBase): self.single_pedal_mode = False self.pedal_steady = 0. + self.ecm_cruise_control_ts_nanos = 0 + self.accelerator_pedal2_ts_nanos = 0 def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_toggles): ret = car.CarState.new_message() @@ -193,6 +195,8 @@ class CarState(CarStateBase): if self.CP.pcmCruise and self.CP.carFingerprint not in ASCM_INT: ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3) if self.CP.carFingerprint in CC_ONLY_CAR: + self.ecm_cruise_control_ts_nanos = pt_cp.ts_nanos["ECMCruiseControl"]["CruiseActive"] + self.accelerator_pedal2_ts_nanos = pt_cp.ts_nanos["AcceleratorPedal2"]["CruiseState"] ret.accFaulted = False ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL: @@ -206,6 +210,9 @@ class CarState(CarStateBase): ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0 except: ret.cruiseState.enabled = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCmdActive"] != 0 + else: + self.ecm_cruise_control_ts_nanos = 0 + self.accelerator_pedal2_ts_nanos = 0 if self.CP.enableBsm: if not sdgm_non_volt: diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 565cf75fb..dc36bd2bb 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -185,6 +185,23 @@ def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_con return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) +def create_ecm_cruise_control_command(packer, bus, enabled, target_speed_kph): + dat = bytearray(8) + dat[0] = 0x01 + # Match observed stock shape on non-ACC CC paths: byte4 is usually 0x00 + # (with occasional 0x80 from stock state transitions). Keep this spoofed + # path at 0x00 to avoid plausibility mismatch on non-speed bits. + dat[4] = 0x00 + + set_speed_raw = 0 + if enabled: + set_speed_raw = int(round(max(0., target_speed_kph) / 0.0625)) + set_speed_raw = max(0, min(set_speed_raw, 0x0FFF)) + + dat[2] = (set_speed_raw >> 8) & 0xFF + dat[3] = set_speed_raw & 0xFF + return make_can_msg(0x3D1, bytes(dat), bus) + def create_adas_time_status(bus, tt, idx): dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4e6d2b6f7..dfe8bad86 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -534,6 +534,16 @@ class CarInterface(CarInterfaceBase): if ACCELERATOR_POS_MSG not in fingerprint.get(CanBus.POWERTRAIN, {}): ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value + use_panda_3d1_sched = ( + ret.openpilotLongitudinalControl and + ret.enableGasInterceptor and + bool(ret.flags & GMFlags.PEDAL_LONG.value) and + candidate in CC_ONLY_CAR and + candidate != CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL + ) + if use_panda_3d1_sched: + gm_safety_cfg.safetyParam |= Panda.FLAG_GM_PANDA_3D1_SCHED + return ret # returns a car.CarState