diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index d6aa4a4d2ad1f1..0d4a90e6a65a3c 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -23,6 +23,13 @@ def __init__(self, dbc_name, CP, VM): self.frame = 0 self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 + self.hca_mode = 5 # init in (active)status 5 + 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.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 def update(self, CC, CS, ext_bus, now_nanos): actuators = CC.actuators @@ -62,12 +69,24 @@ 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 self.apply_steer_last = apply_steer - can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled)) + can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled, self.hca_mode)) # **** Acceleration Controls ******************************************** # diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 2e8e14f77164b6..982efd2f24aa03 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -1,8 +1,9 @@ -def create_steering_control(packer, bus, apply_steer, lkas_enabled): +def create_steering_control(packer, bus, apply_steer, lkas_enabled, hca_mode): + values = { "LM_Offset": abs(apply_steer), "LM_OffSign": 1 if apply_steer < 0 else 0, - "HCA_Status": 7 if (lkas_enabled and apply_steer != 0) else 3, + "HCA_Status": hca_mode if (lkas_enabled and apply_steer != 0) else 3, "Vib_Freq": 16, }