VW PQ: bugfix OEM ACC erroring during OP long?

* bump panda * set GRA_Status of Motor_2 to 0 when OP long is active
dkiiv · Oct 15, 2024 · aa468d5 · aa468d5
1 parent be3ca77
commit aa468d5
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 7 deletions.
diff --git a/panda b/panda
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
@@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos):
 
     # **** Stock ACC Button Controls **************************************** #
 
-    gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
+    gra_send_ready = CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
     if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
       can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values,
                                                            cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
@@ -215,6 +215,14 @@ def update(self, CC, CS, now_nanos):
           self.send_count = 0
         self.last_cruise_button = self.cruise_button
 
+    # **** Blinding Motor_2 for PQ radar ************ #
+    if VolkswagenFlags.PQ and self.ext_bus == CANBUS.cam and self.CP.openpilotLongitudinalControl:
+      if getattr(self, "motor2_frame", 0) % 2 or CS.motor2_stock != getattr(self, 'motor2_last', CS.motor2_stock):  # 50hz / 20ms
+        can_sends.append(self.CCS.create_motor2_control(self.packer_pt, CANBUS.cam, CS.motor2_stock))
+        self.motor2_frame = 0
+      self.motor2_last = CS.motor2_stock
+      self.motor2_frame += 1
+
     new_actuators = actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last

diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
@@ -253,6 +253,8 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
     if ret.cruiseState.speed > 70:  # 255 kph in m/s == no current setpoint
       ret.cruiseState.speed = 0
 
+    self.motor2_stock = pt_cp.vl["Motor_2"]
+
     # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
     ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"],
                                                                                                                       pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])

diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py
@@ -46,11 +46,11 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0
 
   values.update({
     "COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
-    "GRA_Abbrechen": cancel,
-    "GRA_Recall": resume or resume_cruise,
-    "GRA_Neu_Setzen": set_cruise,
-    "GRA_Down_kurz": decel_cruise,
-    "GRA_Up_kurz": accel_cruise,
+    "GRA_Abbrechen": cancel if custom_stock_long else 0,
+    "GRA_Recall": resume or resume_cruise if custom_stock_long else 0,
+    "GRA_Neu_Setzen": set_cruise if custom_stock_long else 0,
+    "GRA_Down_kurz": decel_cruise if custom_stock_long else 0,
+    "GRA_Up_kurz": accel_cruise if custom_stock_long else 0,
   })
 
   return packer.make_can_msg("GRA_Neu", bus, values)
@@ -112,3 +112,10 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance
   }
 
   return packer.make_can_msg("ACC_GRA_Anzeige", bus, values)
+
+def create_motor2_control(packer, bus, motor2_stock):
+  values = motor2_stock
+  values.update = {
+    "GRA_Status": 0,
+  }
+  return packer.make_can_msg("Motor_2", bus, values)