mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-31 13:13:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

GM: Accel to Wheel Torque Control

Browse Source 
Fix Sport Plus
This commit is contained in:
garrettpall
2024-10-18 07:31:00 -04:00
committed by firestar5683
parent a4052b545d
commit efd37043d8
5 changed files with 43 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc/gm_global_a_powertrain_generated.dbc
-1
View File
@@ -221,7 +221,6 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM
SG_ GasRegenFullStopActive : 13|1@0+ (1,0) [0|0] "" NEO
SG_ GasRegenCmdActive : 0|1@0+ (1,0) [0|0] "" NEO
SG_ RollingCounter : 7|2@0+ (1,0) [0|0] "" NEO
SG_ GasRegenAlwaysOne3 : 23|1@0+ (1,0) [0|1] "" NEO
SG_ GasRegenCmd : 8|14@0+ (1,0) [0|0] "" NEO
BO_ 717 ASCM_2CD: 5 K124_ASCM
selfdrive/car/gm/carcontroller.py
+31 -21
View File
@@ -1,3 +1,4 @@
import math
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
@@ -48,6 +49,12 @@ 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'])
@@ -121,14 +128,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
@@ -140,20 +139,31 @@ class CarController(CarControllerBase):
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE))
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.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
else:
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)))
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:
if frogpilot_toggles.sport_plus:
self.apply_gas = int(round(interp(accel, self.params.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)))
accel_due_to_pitch = 0.0
if frogpilot_toggles.sport_plus:
gas_max = self.params.MAX_GAS_PLUS
accel_max = self.params.ACCEL_MAX_PLUS
else:
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:
selfdrive/car/gm/gmcan.py
-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]
selfdrive/car/gm/interface.py
+4 -3
View File
@@ -132,16 +132,17 @@ class CarInterface(CarInterfaceBase):
# Tuning for experimental long
if use_new_api:
ret.longitudinalTuning.kiV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.5, 0.5]
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
else:
ret.longitudinalTuning.kpV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.72]
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.stoppingDecelRate = 1.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stopAccel = -0.25
if experimental_long:
ret.pcmCruise = False
@@ -170,7 +171,7 @@ class CarInterface(CarInterfaceBase):
# Tuning
if use_new_api:
ret.longitudinalTuning.kiV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.5, 0.5]
else:
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
selfdrive/car/gm/values.py
+8 -2
View File
@@ -38,7 +38,7 @@ class CarControllerParams:
def __init__(self, CP):
# Gas/brake lookups
self.ZERO_GAS = 6144 # Coasting
self.ZERO_GAS = 6150 # 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:
@@ -49,6 +49,7 @@ class CarControllerParams:
# 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.
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 = 7496
@@ -56,6 +57,7 @@ class CarControllerParams:
self.MAX_ACC_REGEN = 5610
self.INACTIVE_REGEN = 5650
self.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.
@@ -65,15 +67,19 @@ class CarControllerParams:
# ICE has much less engine braking force compared to regen in EVs,
# lower threshold removes some braking deadzone
self.max_regen_acceleration = -1. 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_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