automate bitbang, handle failed entry

dkiiv · Aug 6, 2024 · b8e99c5 · b8e99c5
1 parent 7a123ae
commit b8e99c5
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
@@ -44,6 +44,8 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
         self.PLA_status = 6 if self.PLA_entryCounter >= 11 else 4
         self.PLA_ESP_status = 6 if self.PLA_entryCounter >= 32 else 4
         self.PLA_entryCounter += 1 if self.PLA_entryCounter <= 32 else self.PLA_entryCounter
+        if CS.LH2_steeringState != 64 and self.PLA_entryCounter >= 30:
+          self.PLA_entryCounter = 0
       else:
         self.PLA_status = 8
         self.PLA_ESP_status = 8
@@ -57,6 +59,11 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
       can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.br, apply_angle, self.PLA_status, self.PLA_ESP_status, self.CSLH3_SignLast))
       self.CSLH3_SignLast = CS.LH_3_Sign
 
+    # **** Gate_Komf Spammer ************************************************ #
+    if self.PLA_entryCounter <= 30 and CC.latActive:
+      can_sends.append(self.CCS.create_gk_spam(self.packer_pt, CANBUS.br, CS.Gate_Komf_stock))
+
+
     # **** Acceleration Controls ******************************************** #
 
     if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl:
@@ -97,8 +104,6 @@ def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables):
       can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values,
                                                            cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
 
-    # **** Gate_Komf Spammer ************************************************ #
-    can_sends.append(self.CCS.create_gk_spam(self.packer_pt, CANBUS.br, CS.Gate_Komf_stock))
 
     new_actuators = actuators.copy()
     new_actuators.steeringAngleDeg = self.apply_angle_last

diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
@@ -185,6 +185,7 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables):
     self.LH_3_Sign = pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"]
 
     self.Gate_Komf_stock = pt_cp.vl["Gate_Komf_1"]
+    self.LH2_steeringState = pt_cp.vl["Lenkhilfe_2"]["LH2_aktLenkeingriff"]
 
     # Update gas, brakes, and gearshift.
     ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0