diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 8de42d598..3528e98d1 100755 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -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 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b77a8e8f1..879cd265e 100755 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -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: diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 32a0ca761..d8d50cfd4 100755 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -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] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 86ddd42d4..300e04df9 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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] diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 6780776cf..891e52498 100755 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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