Skip to content

Commit

Permalink
VW PQ: update HCA flipping (#56)
Browse files Browse the repository at this point in the history
* switch apply_steer to new_steer in HCA mode switching

* change HCA7 return to center ramp up rate to same as HCA5

* try more conditionals on ramp rate change for HCA7. drop requested steer on first hca7 frame

* refactor new_steer conditional
  • Loading branch information
dkiiv committed May 11, 2024
1 parent a889d4e commit fc102d0
Showing 1 changed file with 25 additions and 12 deletions.
37 changes: 25 additions & 12 deletions selfdrive/car/volkswagen/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,13 @@ def __init__(self, dbc_name, CP, VM):
self.hcaSameTorqueCount = 0
self.hcaEnabledFrameCount = 0
self.hca_mode = 5 # init in (active)status 5
self.hca_mode_last = 0 # init last HCA mode
self.hca_centerDeadbandHigh = 13 # init center dead band high
self.hca_centerDeadbandLow = 4 # init center dead band low
self.hca_deadbandNM_switch = 150 # init dead band NM switch
self.steeringAngle = 0 # init our own steeringAngle
self.steeringAngle_last = 0 # init our own steeringAngle_last
self.steeringRate = 0 # init our own steeringRate
self.steerDeltaUpHCA5 = self.CCP.STEER_DELTA_UP # init HCA 5 delta up ramp rate
self.steerDeltaUpHCA7 = self.CCP.STEER_DELTA_UP / 2 # init HCA 7 delta up ramp rate, adjust "/" value to change ramp rate difference

Expand All @@ -53,6 +56,28 @@ def update(self, CC, CS, ext_bus, now_nanos):

if CC.latActive:
new_steer = int(round(actuators.steer * self.CCP.STEER_MAX))

if self.CCS == pqcan: # Custom HCA mode switching (PQ only)
self.steeringAngle = CS.out.steeringAngleDeg if CS.out.steeringAngleDeg >= 0 else CS.out.steeringAngleDeg * -1
self.steeringRate = CS.out.steeringRateDeg if CS.out.steeringRateDeg >= 0 else CS.out.steeringRateDeg *-1
if (((self.steeringAngle >= self.hca_centerDeadbandHigh and abs(new_steer) <= self.hca_deadbandNM_switch) or \
(self.steeringAngle >= self.hca_centerDeadbandLow and abs(new_steer) >= self.hca_deadbandNM_switch)) or \
(self.hca_mode == 7 and ((abs(new_steer) >= 25 and self.steeringAngle <= self.hca_centerDeadbandLow) or \
self.steeringAngle >= self.hca_centerDeadbandLow))):

if self.steeringAngle_last > self.steeringAngle and self.steeringRate > 100:
self.CCP.STEER_DELTA_UP = self.steerDeltaUpHCA5
else:
self.CCP.STEER_DELTA_UP = self.steerDeltaUpHCA7

new_steer = int(round(new_steer * 0.7)) if self.steeringRate >= 5 and self.hca_mode_last == 5 else new_steer
self.hca_mode = 7
else:
self.hca_mode = 5
self.CCP.STEER_DELTA_UP = self.steerDeltaUpHCA5
self.hca_mode_last = self.hca_mode
self.steeringAngle_last = self.steeringAngle

apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP)
if apply_steer == 0:
hcaEnabled = False
Expand All @@ -71,18 +96,6 @@ def update(self, CC, CS, ext_bus, now_nanos):
self.hcaSameTorqueCount = 0
else:
self.hcaSameTorqueCount = 0

if self.CCS == pqcan: # Custom HCA mode switching (PQ only)
self.steeringAngle = CS.out.steeringAngleDeg if CS.out.steeringAngleDeg >= 0 else CS.out.steeringAngleDeg * -1
if (((self.steeringAngle >= self.hca_centerDeadbandHigh and abs(apply_steer) <= self.hca_deadbandNM_switch) or \
(self.steeringAngle >= self.hca_centerDeadbandLow and abs(apply_steer) >= self.hca_deadbandNM_switch)) or \
(self.hca_mode == 7 and ((abs(apply_steer) >= 25 and self.steeringAngle <= self.hca_centerDeadbandLow) or \
self.steeringAngle >= self.hca_centerDeadbandLow))):
self.hca_mode = 7
self.CCP.STEER_DELTA_UP = self.steerDeltaUpHCA7
else:
self.hca_mode = 5
self.CCP.STEER_DELTA_UP = self.steerDeltaUpHCA5
else:
hcaEnabled = False
apply_steer = 0
Expand Down

0 comments on commit fc102d0

Please sign in to comment.