From b81d6fb1ebcedbbd1f7b0d8d6f43b7709888ab74 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 6 May 2025 16:31:36 -0500 Subject: [PATCH] StarPilot --- selfdrive/car/gm/carcontroller.py | 78 +++++++------------------------ selfdrive/car/gm/carstate.py | 19 ++++---- selfdrive/car/gm/gmcan.py | 27 ----------- selfdrive/car/gm/values.py | 20 ++++++-- 4 files changed, 42 insertions(+), 102 deletions(-) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index ab780122b..92fe2a3d8 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,4 +1,3 @@ -from typing import Tuple from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter @@ -8,7 +7,7 @@ from openpilot.common.params_pyx import Params from opendbc.can.packer import CANPacker 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, EV_CAR, AccState, CC_REGEN_PADDLE_CAR +from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, AccState from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -22,7 +21,8 @@ TransmissionType = car.CarParams.TransmissionType # Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s CAMERA_CANCEL_DELAY_FRAMES = 10 # Enforce a minimum interval between steering messages to avoid a fault -MIN_STEER_MSG_INTERVAL_MS = 10 +MIN_STEER_MSG_INTERVAL_MS = 15 + # Constants for pitch compensation PITCH_DEADZONE = 0.01 # [radians] 0.01 ≈ 1% grade BRAKE_PITCH_FACTOR_BP = [5., 10.] # [m/s] smoothly revert to planned accel at low speeds @@ -44,8 +44,6 @@ class CarController(CarControllerBase): self.lka_steering_cmd_counter = 0 self.lka_icon_status_last = (False, False) - self.last_oem_prndl2_ts_nanos = 0 - self.last_oem_regen_paddle_ts_nanos = 0 self.params = CarControllerParams(self.CP) self.params_ = Params() @@ -57,51 +55,22 @@ class CarController(CarControllerBase): # FrogPilot variables self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz self.accel_g = 0.0 - self.regen_paddle_pressed = False - self.aego = 0.0 - def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]: - if not long_active: - return 0., False + @staticmethod + def calc_pedal_command(accel: float, long_active: bool, car_velocity) -> float: + if not long_active: return 0. - # Regen paddle hysteresis (200ms = 20 frames) - if not hasattr(self, 'regen_paddle_timer'): - self.regen_paddle_timer = 0 - - if self.aego < -0.7 and accel <= 0.0: - self.regen_paddle_timer += 1 + if accel < -0.5: + pedal_gas = 0 else: - self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0) - self.regen_paddle_pressed = self.regen_paddle_timer >= 20 - - press_regen_paddle = self.regen_paddle_pressed - - # Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle - speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63, - 11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820, - 22.939, 24.058, 25.177, 26.296] - regen_gain_ratio = [1.01, 1.01, 1.02, 1.05, 1.08, 1.345979, 1.369975, - 1.376302, 1.388052, 1.370367, 1.388498, 1.386030, 1.405950, 1.387555, - 1.390392, 1.394946, 1.414915, 1.428535, 1.439611, 1.440106, 1.441438, - 1.439395, 1.446909, 1.445738] - - gain = interp(car_velocity, speed_mps, regen_gain_ratio) - - pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240]) - - if press_regen_paddle: - pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0) - else: + pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240]) pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0) - - - return pedal_gas, press_regen_paddle + + return pedal_gas def update(self, CC, CS, now_nanos, frogpilot_toggles): - self.CS = CS - self.aego = CS.out.aEgo actuators = CC.actuators accel = brake_accel = actuators.accel hud_control = CC.hudControl @@ -113,19 +82,6 @@ class CarController(CarControllerBase): # Send CAN commands. can_sends = [] - - - # Only apply PRNDL2 and regen paddle spoofing for cars in CC_REGEN_PADDLE_CAR and when gas interceptor is enabled - if self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and self.CP.enableGasInterceptor: - steer_phase = self.last_steer_frame % 3 - send_prndl_frame = (self.frame % 3) != steer_phase - - press_regen_paddle = self.regen_paddle_pressed - if send_prndl_frame and CC.longActive: - can_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, press_regen_paddle)) - can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, press_regen_paddle)) - - # Steering (Active: 50Hz, inactive: 10Hz) steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP @@ -185,11 +141,12 @@ class CarController(CarControllerBase): else: # Normal operation if self.CP.carFingerprint in EV_CAR: + self.params.update_ev_gas_brake_threshold(CS.out.vEgo) if frogpilot_toggles.sport_plus: - self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) + self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) else: - self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) else: if frogpilot_toggles.sport_plus: self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) @@ -202,13 +159,12 @@ class CarController(CarControllerBase): 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, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) + interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill: # "Tap" the accelerator pedal to re-engage ACC interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS self.apply_brake = 0 - press_regen_paddle = False self.apply_gas = self.params.INACTIVE_REGEN idx = (self.frame // 4) % 4 @@ -239,7 +195,7 @@ class CarController(CarControllerBase): # 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, acc_engaged, 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)) + idx, CC.enabled, near_stop, at_full_stop, self.CP)) # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index c89c75828..e41b44b1e 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -56,10 +56,6 @@ class CarState(CarStateBase): self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.loopback_lka_steering_cmd_updated: self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] - - # Track timestamps for OEM PRNDL2 and Regen Paddle messages (used to sync spoofing timing) - self.prndl2_ts_nanos = pt_cp.ts_nanos["ECMPRDNL2"]["PRNDL2"] - self.regen_paddle_ts_nanos = pt_cp.ts_nanos["EBCMRegenPaddle"]["RegenPaddle"] if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value: self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] @@ -96,11 +92,16 @@ 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 or (ret.regenBraking and GearShifter.manumatic) + self.single_pedal_mode = ( + ret.gearShifter == GearShifter.low + or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 + or pt_cp.vl["ECMPRDNL2"]["TransmissionState"] == 21 + or (ret.regenBraking and GearShifter.manumatic) + ) if self.CP.enableGasInterceptor: ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. - threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 595 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting. + threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting. ret.gasPressed = ret.gas > threshold else: ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. @@ -173,7 +174,7 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 - + # FrogPilot CarState functions self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint in SDGM_CAR: self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] @@ -234,7 +235,7 @@ class CarState(CarStateBase): ] else: messages += [ - ("ECMPRDNL2", 40), + ("ECMPRDNL2", 10), ("AcceleratorPedal2", 33), ("ECMEngineStatus", 100), ("BCMTurnSignals", 1), @@ -256,7 +257,7 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.direct: messages += [ - ("EBCMRegenPaddle", 40), + ("EBCMRegenPaddle", 50), ("EVDriveMode", 0), ] diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 3bbf2969b..32a0ca761 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -177,33 +177,6 @@ def create_lka_icon_command(bus, active, critical, steer): dat = b"\x00\x00\x00" return make_can_msg(0x104c006c, dat, bus) -def create_prndl2_command(packer, bus, press_regen_paddle): - prndl2_value = 7 if press_regen_paddle else 6 - manual_mode = 1 if press_regen_paddle else 0 - values = { - "Byte0": 0x0C, - "Byte1": 0x0C, - "Byte2": 0x00, - "PRNDL2": prndl2_value, - "Byte4": 0x00, - "ManualMode": manual_mode, - "TransmissionState": 1, - "Byte7": 0x00 - } - return packer.make_can_msg("ECMPRDNL2", bus, values) - -def create_regen_paddle_command(packer, bus, press_regen_paddle): - regen_paddle_value = 2 if press_regen_paddle else 0 - values = { - "RegenPaddle": regen_paddle_value, - "Byte1": 0, - "Byte2": 0, - "Byte3": 0, - "Byte4": 0, - "Byte5": 0, - "Byte6": 0 - } - return packer.make_can_msg("EBCMRegenPaddle", bus, values) def create_gm_cc_spam_command(packer, controller, CS, actuators): if controller.params_.get_bool("IsMetric"): diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 52ab348dd..6780776cf 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -41,7 +41,7 @@ class CarControllerParams: self.ZERO_GAS = 6144 # Coasting self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen - if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV: + if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR: self.MAX_GAS = 7496 self.MAX_GAS_PLUS = 8848 self.MAX_ACC_REGEN = 5610 @@ -53,18 +53,18 @@ class CarControllerParams: elif CP.carFingerprint in SDGM_CAR: self.MAX_GAS = 7496 self.MAX_GAS_PLUS = 7496 - self.MAX_ACC_REGEN = 7110 + self.MAX_ACC_REGEN = 5610 self.INACTIVE_REGEN = 5650 self.max_regen_acceleration = 0. else: self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill. self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max - self.MAX_ACC_REGEN = 7110 # Increased for stronger regen braking + self.MAX_ACC_REGEN = 5500 # Max ACC regen is slightly less than max paddle regen self.INACTIVE_REGEN = 5500 # ICE has much less engine braking force compared to regen in EVs, # lower threshold removes some braking deadzone - self.max_regen_acceleration = -3. if CP.carFingerprint in EV_CAR else -0.1 # More aggressive regen for EVs + self.max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX] self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS] @@ -74,7 +74,18 @@ class CarControllerParams: self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] + # determined by letting Volt regen to a stop in L gear from 89mph, + # and by letting off gas and allowing car to creep, for determining + # the positive threshold values at very low speed + EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s] + EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s] + def update_ev_gas_brake_threshold(self, v_ego): + gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V) + self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS] + self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX] + self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS] + self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold] @dataclass class GMCarDocs(CarDocs): @@ -311,7 +322,6 @@ FW_QUERY_CONFIG = FwQueryConfig( EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC} CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.CHEVROLET_SUBURBAN_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC} -CC_REGEN_PADDLE_CAR = {CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_BOLT_EUV} # CC_ONLY_CAR = set(c for c in CAR if str(c).endswith('_CC')) # We're integrated at the Safety Data Gateway Module on these cars