VW PQ: HCA mode switching (#55)

* 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

selfdrive/car/volkswagen/carcontroller.py

@@ -22,6 +22,13 @@ 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_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
     self.hca_frame_timer_running = 0
     self.hca_frame_same_torque = 0
 
@@ -53,6 +60,19 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
             self.hca_frame_same_torque = 0
         else:
           self.hca_frame_same_torque = 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
+
         hca_enabled = abs(apply_steer) > 0
       else:
         hca_enabled = False
@@ -63,7 +83,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,
   }