From bea6ee6ccbdd701c48f5d875ae19b5ff652b6f38 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 31 Jan 2023 20:26:33 -0800
Subject: [PATCH] GM camera ACC: reliable relay closing (#27164)

* Reliable relay open

* Reliable relay close

* ign in a loop

* fixes

* we need this

* log

* comment to remind me tmrw

* ign fix

* this makes it 2x more reliable, but messyyy

* Revert "this makes it 2x more reliable, but messyyy"

This reverts commit 03401dc4a705cfacbe5a7048d95dccb6fa80d57f.

* revert non-related stuff

* comments, spaces

* a stands for indefinite article

* not applicable for non-camera acc

* something...

* Revert "something..."

This reverts commit de8a158488efd5ef257434fbc3c55aefff800cb7.

* Update ref_commit
old-commit-hash: 022ef679e6d67f0be82ddc19b595bc874d456138
---
 selfdrive/car/gm/carcontroller.py        | 9 +++++++--
 selfdrive/car/gm/carstate.py             | 4 ++++
 selfdrive/test/process_replay/ref_commit | 2 +-
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index c5aae34249..c25de41769 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -50,9 +50,14 @@ class CarController:
 
     # Steering (Active: 50Hz, inactive: 10Hz)
     # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked
-    init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera
+    init_lka_counter = not self.sent_lka_steering_cmd
+    # Also send at 50Hz until we're in sync with camera so counters align when relay closes, preventing a fault
+    # openpilot can subtly drift, so this is activated throughout a drive to stay synced
+    out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.camera_lka_steering_cmd_counter + 1) % 4
+    sync_steer = (init_lka_counter or out_of_sync) and self.CP.networkLocation == NetworkLocation.fwdCamera
+
     steer_step = self.params.INACTIVE_STEER_STEP
-    if CC.latActive or init_lka_counter:
+    if CC.latActive or sync_steer:
       steer_step = self.params.STEER_STEP
 
     # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index cf6d2817ec..8ea8ea8874 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -19,6 +19,7 @@ class CarState(CarStateBase):
     self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
     self.loopback_lka_steering_cmd_updated = False
     self.pt_lka_steering_cmd_counter = 0
+    self.camera_lka_steering_cmd_counter = 0
     self.buttons_counter = 0
 
   def update(self, pt_cp, cam_cp, loopback_cp):
@@ -34,6 +35,7 @@ class CarState(CarStateBase):
     self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
     if self.CP.networkLocation == NetworkLocation.fwdCamera:
       self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
+      self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
 
     ret.wheelSpeeds = self.get_wheel_speeds(
       pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
@@ -113,11 +115,13 @@ class CarState(CarStateBase):
     if CP.networkLocation == NetworkLocation.fwdCamera:
       signals += [
         ("AEBCmdActive", "AEBCmd"),
+        ("RollingCounter", "ASCMLKASteeringCmd"),
         ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"),
         ("ACCCruiseState", "ASCMActiveCruiseControlStatus"),
       ]
       checks += [
         ("AEBCmd", 10),
+        ("ASCMLKASteeringCmd", 10),
         ("ASCMActiveCruiseControlStatus", 25),
       ]
 
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index b598524fd7..b7cb6ceb33 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-d795362593d935767c694c652ecb05ab80dd1914
\ No newline at end of file
+baef183c9602b702756e2fd0781b5d289b61d19a