VW PQ: update AWV signals control

* make parameter and prefill fall off after halten is active
dkiiv · Nov 2, 2024 · a7d5fa8 · a7d5fa8
1 parent e4d7004
commit a7d5fa8
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 8 deletions.
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
@@ -34,6 +34,8 @@ def __init__(self, dbc_name, CP, VM):
     self.AWV_brake = 0
     self.AWV_enable = 0
     self.AWV_halten = 0
+    self.AWV_freigabe = 0
+    self.AWV_haltenCounter = 0
 
     self.deviationBP = [-0.13, -0.1, -0.05, 0.]     # accel        (m/s squared)
     self.deviationV = [0., 0.08, 0.14, 0.15]        # comfort-band (m/s squared)
@@ -157,24 +159,29 @@ def update(self, CC, CS, now_nanos):
       starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping)
       if self.CCS == pqcan and CC.longActive and actuators.accel <= 0 and CS.out.vEgoRaw <= 6:
         accel = clip(actuators.accel, self.CCP.ACCEL_MIN, 0)
-        self.AWV_enable = 1
+        self.AWV_enable = 1 if self.AWV_haltenCounter <= 5 else 0
+        self.AWV_freigabe = 1
         if self.AWV_halten:
           self.AWV_brake = min(self.AWV_brake + abs(self.AWV_brake) / 7, 0)
+          self.AWV_haltenCounter += 1 if self.AWV_haltenCounter <= 5 else self.AWV_haltenCounter
         else:
           self.AWV_brake = clip(actuators.accel, self.CCP.ACCEL_MIN, 0)
+          self.AWV_haltenCounter = 0
       else:
         accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0
-        self.AWV_enable = 0 if CS.out.vEgoRaw >= 6 else self.AWV_enable
+        self.AWV_enable = 0
+        self.AWV_freigabe = 0 if CS.out.vEgoRaw >= 6 else self.AWV_freigabe
         self.AWV_brake = 0
-      if self.AWV_enable and CS.out.vEgoRaw <= 3:
+        self.AWV_haltenCounter = 0
+      if self.AWV_freigabe and CS.out.vEgoRaw <= 3:
           self.AWV_halten = 1
-      elif CS.out.vEgoRaw >= 4 or not self.AWV_enable:
+      elif CS.out.vEgoRaw >= 4 or not self.AWV_freigabe:
           self.AWV_halten = 0
       self.long_deviation = 0  # TODO: make dynamic again
       self.long_ratelimit = 4  # TODO: make dynamic again
 
       if self.CCS == pqcan:
-        can_sends.append(self.CCS.create_awv_control(self.packer_pt, CANBUS.pt, self.AWV_brake, self.AWV_enable, self.AWV_halten))
+        can_sends.append(self.CCS.create_awv_control(self.packer_pt, CANBUS.pt, self.AWV_brake, self.AWV_enable, self.AWV_freigabe, self.AWV_halten))
       can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, accel,
                                                          acc_control, stopping, starting, CS.esp_hold_confirmation,
                                                          self.long_deviation, self.long_ratelimit))

diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py
@@ -99,13 +99,13 @@ def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping
 
   return commands
 
-def create_awv_control(packer, bus, apply_brake, enabled, halten):
+def create_awv_control(packer, bus, apply_brake, enabled, freigabe, halten):
 
   values = {
     "AWV_1_Parameter": 2 if enabled else 0,
     "AWV_1_Prefill": enabled,
-    "ANB_Teilbremsung_Freigabe": enabled,
-    "ANB_Ziel_Teilbrems_Verz_Anf": apply_brake if enabled else 0,
+    "ANB_Teilbremsung_Freigabe": freigabe,
+    "ANB_Ziel_Teilbrems_Verz_Anf": apply_brake if freigabe else 0,
     "AWV_Halten": halten,  # Hold at stop, configure later? Not needed?
   }