From 8e6400bc903afb8cf8ae47c4763c7920606f3550 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 19 Oct 2022 17:46:31 -0700
Subject: [PATCH] try sending everything again (+1 counter)

---
 cereal                            |  2 +-
 selfdrive/car/gm/carcontroller.py |  4 ++--
 selfdrive/car/gm/carstate.py      |  5 +++++
 selfdrive/car/gm/gmcan.py         | 33 +++++++++++++++++++------------
 4 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/cereal b/cereal
index 1e3dd70a39..3d233033be 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 1e3dd70a391bc1bbe437d3eea8be30947f929a75
+Subproject commit 3d233033bec17cd18c1a910a8aca0c9060ed7a18
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index e6ba7390a5..86fc7e349d 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -112,8 +112,8 @@ class CarController:
 
     else:
       # Silence "Take Steering" alert sent by camera, send PSCMStatus with HandsOffSWlDetectionStatus=1
-      if self.frame % 500 == 0:
-        can_sends.append(gmcan.create_pscm_status(self.packer_pt, CS.pscm_status_counter))
+      if self.frame % 100 == 0:
+        can_sends.append(gmcan.create_pscm_status(self.packer_pt, CS.pscm_status, CS.pscm_status_counter))
 
       # Stock longitudinal, integrated at camera
       if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 9183991113..7cb6aaea30 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -76,6 +76,7 @@ class CarState(CarStateBase):
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
 
     # 0 inactive, 1 active, 2 temporarily limited, 3 failed
+    self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
     self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
     ret.steerFaultTemporary = self.lkas_status == 2
     ret.steerFaultPermanent = self.lkas_status == 3
@@ -149,6 +150,10 @@ class CarState(CarStateBase):
       ("LKADriverAppldTrq", "PSCMStatus"),
       ("LKATorqueDelivered", "PSCMStatus"),
       ("LKATorqueDeliveredStatus", "PSCMStatus"),
+      ("HandsOffSWlDetectionStatus", "PSCMStatus"),
+      ("HandsOffSWDetectionMode", "PSCMStatus"),
+      ("LKATotalTorqueDelivered", "PSCMStatus"),
+      ("PSCMStatusChecksum", "PSCMStatus"),
       ("RollingCounter", "PSCMStatus"),
       ("TractionControlOn", "ESPStatus"),
       ("ParkBrake", "VehicleIgnitionAlt"),
diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py
index 4ef72b8c7f..0a14425bb1 100644
--- a/selfdrive/car/gm/gmcan.py
+++ b/selfdrive/car/gm/gmcan.py
@@ -7,19 +7,26 @@ def create_buttons(packer, bus, idx, button):
   }
   return packer.make_can_msg("ASCMSteeringButton", bus, values)
 
-def create_pscm_status(packer, idx):
-  values = {
-    "HandsOffSWDetectionMode": 1,
-    "HandsOffSWlDetectionStatus": 1,
-    "LKATorqueDeliveredStatus": 0,
-    "RollingCounter": int(idx + 1) & 0xf,
-  }
-  values["PSCMStatusChecksum"] = (0x30 + (values['HandsOffSWlDetectionStatus'] << 5) +
-                                  (values['LKATorqueDeliveredStatus'] << 3) +
-                                  (values['HandsOffSWDetectionMode'] << 3) +
-                                  values['RollingCounter'])
-
-  return packer.make_can_msg("PSCMStatus", 2, values)
+def create_pscm_status(packer, pscm_status, idx):
+  checksum_mod = int(1 - pscm_status["HandsOffSWlDetectionStatus"]) << 5
+  checksum_mod += ((pscm_status["RollingCounter"] + 1) & 0xf) - pscm_status["RollingCounter"]
+  pscm_status["HandsOffSWlDetectionStatus"] = 1
+  pscm_status["RollingCounter"] += 1  # wraps around
+  pscm_status["PSCMStatusChecksum"] += checksum_mod
+  return packer.make_can_msg("PSCMStatus", 2, pscm_status)
+
+  # values = {
+  #   "HandsOffSWDetectionMode": 1,
+  #   "HandsOffSWlDetectionStatus": 1,
+  #   "LKATorqueDeliveredStatus": 0,
+  #   "RollingCounter": int(idx + 1) & 0xf,
+  # }
+  # values["PSCMStatusChecksum"] = (0x30 + (values['HandsOffSWlDetectionStatus'] << 5) +
+  #                                 (values['LKATorqueDeliveredStatus'] << 3) +
+  #                                 (values['HandsOffSWDetectionMode'] << 3) +
+  #                                 values['RollingCounter'])
+  #
+  # return packer.make_can_msg("PSCMStatus", 2, values)
 
 def create_steering_control(packer, bus, apply_steer, idx, lkas_active):