VW PQ: Add HCA 5 / 7 mode flipping (#58)

* initial hca_mode flipping commit

* hot fix**ing**

* bump hysteresis to 5 deg

* increase hysteresis to 13 degree's

* change HCA flipping logic.

* logic change worked, dropping hysteresis limits to test

* switch to NM torque blending

* temporarily bump panda maxsteer to 400

* revert panda edit, refactor torque blend

* refactor, again!

* try ramping the 1NM drop

* bugfix for pseudo STEER_MAX?

* another hotfix

* change rate limit in interpolation, removed apply_driver_steer_torque_limit

* throw out >3nm. switch to HCA7 when requested torque >2nm

* make HCA7 switching a wider band, and add some torque interpolation

* change flipping logic

* fix typo

* bugfix: negative steering angle

* switch to hca7 based on NM rate too

* change from NM rate to steeringRate

* revert back to no rate based hca7 switch

* up deadzone to 13

* 13 up for HCA7 worked, dropping HCA5 down to 8

* fix bug, > not < !

* bugfix: hca7 lock and unlock logic. fixed for reals now!

* change delta up ramp rate when HCA7. refactor HCA switch logic a bit

* added dynamic HCA7 lock behavior, based on NM and angle

* tweak values for HCA mode switching

* 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

* adjust deadband high, and adjust lock logic

* incorporate turn signal into logic

* add deadbandLow back to hca7 lock

selfdrive/car/volkswagen/carcontroller.py

@@ -22,6 +22,16 @@ def __init__(self, dbc_name, CP, VM):
     self.gra_acc_counter_last = None
     self.frame = 0
     self.eps_timer_soft_disable_alert = False
+    self.hca_mode = 5                                       # init in (active)status 5
+    self.hca_mode_last = 0                                  # init last HCA mode
+    self.hca_centerDeadbandHigh = 10.5                      # 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
     self.hca_frame_timer_running = 0
     self.hca_frame_same_torque = 0
 
@@ -44,6 +54,30 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
 
       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) or \
+               ((CS.out.leftBlinker or CS.out.rightBlinker) and abs(new_steer >= self.hca_deadbandNM_switch)) or \
+               (abs(new_steer) >= (self.hca_deadbandNM_switch / 3) and not (CS.out.leftBlinker or CS.out.rightBlinker) and \
+                                        self.steeringAngle >= self.hca_centerDeadbandLow) 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)
         self.hca_frame_timer_running += self.CCP.STEER_STEP
         if self.apply_steer_last == apply_steer:
@@ -63,7 +97,7 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
 
       self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
       self.apply_steer_last = apply_steer
-      can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hca_enabled))
+      can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hca_enabled, self.hca_mode))
 
       if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
         # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque

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,
   }