brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pedal interceptor sng

Browse Source 
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent fc3b8ead19
commit 3671544735
3 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
selfdrive/car/gm/carcontroller.py
View File

@@ -117,6 +117,8 @@ class CarController:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
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
# Pitch compensated acceleration;
@@ -130,7 +132,11 @@ class CarController:
# ASCM sends max regen when not enabled
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = 0
elif near_stop and stopping and not CC.cruiseControl.resume:
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE))
else:
# Normal operation
brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
if frogpilot_variables.sport_plus:
self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
@@ -145,10 +151,14 @@ class CarController:
# gas interceptor only used for full long control on cars without ACC
interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive)
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
self.apply_gas = self.params.INACTIVE_REGEN
idx = (self.frame // 4) % 4
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
if self.CP.flags & GMFlags.CC_LONG.value:
if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed:
# Using extend instead of append since the message is only sent intermittently
@@ -163,6 +173,10 @@ class CarController:
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
if self.CP.autoResumeSng:
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
at_full_stop = at_full_stop and not resume
# 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, CC.enabled, at_full_stop))
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,