GreatMergeTorqueTune

This commit is contained in:
firestar5683
2026-02-08 12:51:23 -06:00
parent 2b5a613c65
commit cf606c06aa
5 changed files with 89 additions and 43 deletions
+31 -16
View File
@@ -1,5 +1,7 @@
from typing import Tuple
import time
import math
from openpilot.common.swaglog import cloudlog
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
@@ -9,7 +11,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, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, CC_REGEN_PADDLE_CAR
from openpilot.selfdrive.car.gm.values import CAR, DBC, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, CC_REGEN_PADDLE_CAR
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
@@ -64,13 +66,20 @@ class CarController(CarControllerBase):
self.params = CarControllerParams(self.CP)
self.params_ = Params()
self.mass = CP.mass
self.tireRadius = 0.075 * CP.wheelbase + 0.1453
self.frontalArea = 1.05 * CP.wheelbase + 0.0679
self.coeffDrag = 0.30
self.airDensity = 1.225
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])
# FrogPilot variables
self.accel_g = 0.0
self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz
self.accel_g = 0.0
self.regen_paddle_pressed = False
@@ -334,14 +343,6 @@ class CarController(CarControllerBase):
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
# Pitch compensated acceleration;
# TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
if frogpilot_toggles.long_pitch and len(CC.orientationNED) > 1:
self.pitch.update(CC.orientationNED[1])
self.accel_g = ACCELERATION_DUE_TO_GRAVITY * apply_deadzone(self.pitch.x, PITCH_DEADZONE) # driving uphill is positive pitch
accel += self.accel_g
brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
interceptor_gas_cmd = 0
@@ -353,9 +354,26 @@ class CarController(CarControllerBase):
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = int(min(-100 * frogpilot_toggles.stopAccel, self.params.MAX_BRAKE))
else:
# Normal operation
self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
if len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
else:
accel_due_to_pitch = 0.0
gas_max = self.params.MAX_GAS
accel_max = self.params.ACCEL_MAX
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)))
brake_accel = min((scaled_torque - BRAKE_SWITCH)/(self.tireRadius*self.mass), 0)
self.apply_gas = int(round(apply_gas_torque))
self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
if self.apply_brake > 0:
self.apply_gas = self.params.INACTIVE_REGEN
# Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop
if stopping:
@@ -452,10 +470,7 @@ class CarController(CarControllerBase):
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
if self.frame % 20 == 0:
pscm_status = CS.pscm_status.copy()
if pscm_status["LKATorqueDeliveredStatus"] == 3:
pscm_status["LKATorqueDeliveredStatus"] = 1
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, pscm_status))
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
new_actuators = actuators.as_builder()
new_actuators.accel = accel
+12 -5
View File
@@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import mean
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR, CC_REGEN_PADDLE_CAR
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR, CC_REGEN_PADDLE_CAR, CAR
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
@@ -96,7 +96,10 @@ 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
(ret.regenBraking and GearShifter.manumatic) or
(self.CP.carFingerprint in (CAR.CHEVROLET_BOLT_ACC_2022_2023, CAR.CHEVROLET_BOLT_CC_2022_2023) and self.CP.enableGasInterceptor))
if self.CP.enableGasInterceptor:
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
@@ -115,7 +118,7 @@ class CarState(CarStateBase):
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
ret.steerFaultTemporary = self.lkas_status == 2
ret.steerFaultPermanent = False # self.lkas_status == 3
ret.steerFaultPermanent = self.lkas_status == 3
if self.CP.carFingerprint not in SDGM_CAR:
# 1 - open, 0 - closed
@@ -163,7 +166,11 @@ class CarState(CarStateBase):
if self.CP.carFingerprint in CC_ONLY_CAR:
ret.accFaulted = False
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
# Try ECM first for cars that might have it (like most GMs), fall back to ASCM
try:
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
except:
ret.cruiseState.enabled = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCmdActive"] != 0
if self.CP.enableBsm:
if self.CP.carFingerprint not in SDGM_CAR:
@@ -173,7 +180,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"]
-1
View File
@@ -66,7 +66,6 @@ def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
"GasRegenFullStopActive": at_full_stop,
"GasRegenAlwaysOne": 1,
"GasRegenAlwaysOne2": 1,
"GasRegenAlwaysOne3": 1,
}
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
+17 -8
View File
@@ -48,8 +48,8 @@ NON_LINEAR_TORQUE_PARAMS = {
"right": [4.78003305, 1.0, 0.3122, 0.05591772],
},
CAR.CHEVROLET_SILVERADO: {
"left": [3.8, 0.81, 0.21, 0.0465122],
"right": [3.8, 0.81, 0.21, 0.0465122],
"left": [3.8, 0.81, 0.24, 0.0465122],
"right": [3.8, 0.81, 0.24, 0.0465122],
},
}
@@ -150,11 +150,14 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [5., 35., 60.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
# For ACC models with pedal interceptor, behave like CC_ONLY_CAR
ret.experimentalLongitudinalAvailable = (candidate not in CC_ONLY_CAR) and not ret.enableGasInterceptor
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
# Only use pcmCruise if no pedal interceptor (bolt_cc style behavior)
ret.pcmCruise = not ret.enableGasInterceptor
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
# Use default minEnableSpeed for ACC models (will be overridden by pedal interceptor section if present)
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
@@ -260,7 +263,13 @@ class CarInterface(CarInterfaceBase):
# ACC Bolts use pedal for full longitudinal control, not just sng
ret.flags |= GMFlags.PEDAL_LONG.value
elif candidate == CAR.CHEVROLET_SILVERADO:
# Enable pedal interceptor for ACC models when detected
if candidate in CAMERA_ACC_CAR and ret.enableGasInterceptor:
# ACC models with pedal interceptor get full pedal longitudinal control
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
if candidate == CAR.CHEVROLET_SILVERADO:
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
@@ -320,7 +329,7 @@ class CarInterface(CarInterfaceBase):
ret.stoppingControl = True
ret.autoResumeSng = True
if candidate in CC_ONLY_CAR: #pedal interceptor tuning
if candidate in CC_ONLY_CAR or (candidate in CAMERA_ACC_CAR and ret.enableGasInterceptor): #pedal interceptor tuning
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
@@ -346,8 +355,8 @@ class CarInterface(CarInterfaceBase):
if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 5., 2.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.9] # == 2 km/h/s, 1.25 mph/s
ret.longitudinalTuning.deadzoneBP = [0., 1.]
ret.longitudinalTuning.deadzoneV = [0.9, 0.9] # == 2 km/h/s, 1.25 mph/s
ret.longitudinalActuatorDelay = 1. # TODO: measure this
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.1]
+29 -13
View File
@@ -64,14 +64,16 @@ class CarControllerParams:
self.INACTIVE_REGEN = 5650
# Camera ACC vehicles have no regen while enabled.
# Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly
self.max_regen_acceleration = 0.
max_regen_acceleration = 0.
self.BRAKE_SWITCH_MAX = self.MAX_ACC_REGEN if CP.carFingerprint in EV_CAR else self.ZERO_GAS
elif CP.carFingerprint in SDGM_CAR:
self.MAX_GAS = 8191
self.MAX_GAS_PLUS = 8191
self.MAX_ACC_REGEN = 5500
self.INACTIVE_REGEN = 5500
self.max_regen_acceleration = 0.
max_regen_acceleration = 0.
self.BRAKE_SWITCH = self.ZERO_GAS
else:
self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill.
@@ -80,17 +82,32 @@ class CarControllerParams:
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
max_regen_acceleration = -3. if CP.carFingerprint in EV_CAR else -0.1
self.BRAKE_SWITCH_MAX = self.MAX_ACC_REGEN if CP.carFingerprint in EV_CAR else self.ZERO_GAS
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]
self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX]
self.GAS_LOOKUP_BP_PLUS = [max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS]
self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS]
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration]
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, 0.]
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
self.BRAKE_SWITCH_LOOKUP_BP = [0.5, 10]
self.BRAKE_SWITCH_LOOKUP_V = [self.ZERO_GAS, self.BRAKE_SWITCH_MAX]
# 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):
@@ -174,7 +191,7 @@ class CAR(Platforms):
GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"),
GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"),
],
GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0),
GMCarSpecs(mass=2994, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0),
)
CHEVROLET_EQUINOX = GMPlatformConfig(
[GMCarDocs("Chevrolet Equinox 2019-22")],
@@ -218,15 +235,15 @@ class CAR(Platforms):
CHEVROLET_SUBURBAN.specs,
)
GMC_YUKON_CC = GMPlatformConfig(
[GMCarDocs("GMC Yukon No ACC")],
[GMCarDocs("GMC Yukon - No-ACC")],
CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4),
)
CADILLAC_CT6_CC = GMPlatformConfig(
[GMCarDocs("Cadillac CT6 No ACC")],
[GMCarDocs("Cadillac CT6 - No-ACC")],
CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
)
CHEVROLET_TRAILBLAZER_CC = GMPlatformConfig(
[GMCarDocs("Chevrolet Trailblazer 2021-22")],
[GMCarDocs("Chevrolet Trailblazer 2021-22 - No-ACC")],
CHEVROLET_TRAILBLAZER.specs,
)
CADILLAC_XT4 = GMPlatformConfig(
@@ -234,7 +251,7 @@ class CAR(Platforms):
CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
)
CADILLAC_XT5_CC = GMPlatformConfig(
[GMCarDocs("Cadillac XT5 No ACC")],
[GMCarDocs("Cadillac XT5 - No-ACC")],
CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5),
)
CHEVROLET_TRAVERSE = GMPlatformConfig(
@@ -246,7 +263,7 @@ class CAR(Platforms):
CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5),
)
CHEVROLET_MALIBU_CC = GMPlatformConfig(
[GMCarDocs("Chevrolet Malibu 2023 No ACC")],
[GMCarDocs("Chevrolet Malibu 2023 - No-ACC")],
CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4),
)
CHEVROLET_TRAX = GMPlatformConfig(
@@ -338,7 +355,6 @@ FW_QUERY_CONFIG = FwQueryConfig(
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_ACC_2022_2023, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC_2019_2021, CAR.CHEVROLET_BOLT_CC_2022_2023, CAR.CHEVROLET_BOLT_CC_2017}
CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC_2019_2021, CAR.CHEVROLET_BOLT_CC_2022_2023, CAR.CHEVROLET_BOLT_CC_2017, 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_2019_2021, CAR.CHEVROLET_BOLT_CC_2022_2023, CAR.CHEVROLET_BOLT_CC_2017}
# 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
SDGM_CAR = {CAR.CADILLAC_XT4, CAR.CHEVROLET_TRAVERSE, CAR.BUICK_BABYENCLAVE}