diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index c5aae3424..c25de4176 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 cf6d2817e..8ea8ea887 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 b598524fd..b7cb6ceb3 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