diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index c94f693c72..8f4d0f27ca 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -14,6 +14,7 @@ class CarController(): def __init__(self, dbc_name, CP, VM): self.start_time = 0. self.apply_steer_last = 0 + self.lka_steering_cmd_counter_last = -1 self.lka_icon_status_last = (False, False) self.steer_rate_limited = False @@ -31,8 +32,12 @@ class CarController(): # Send CAN commands. can_sends = [] - # STEER - if (frame % P.STEER_STEP) == 0: + # Steering (50Hz) + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the + # next Panda loopback confirmation in the current CS frame. + if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: + self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter + elif (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) @@ -42,7 +47,9 @@ class CarController(): apply_steer = 0 self.apply_steer_last = apply_steer - idx = (frame // P.STEER_STEP) % 4 + # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the + # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. + idx = (CS.lka_steering_cmd_counter + 1) % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f16f5e542a..00f25c5fdb 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -13,8 +13,9 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"] + self.lka_steering_cmd_counter = 0 - def update(self, pt_cp): + def update(self, pt_cp, loopback_cp): ret = car.CarState.new_message() self.prev_cruise_buttons = self.cruise_buttons @@ -42,6 +43,7 @@ class CarState(CarStateBase): ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] # 0 inactive, 1 active, 2 temporarily limited, 3 failed self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] @@ -131,3 +133,15 @@ class CarState(CarStateBase): ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) + + @staticmethod + def get_loopback_can_parser(CP): + signals = [ + ("RollingCounter", "ASCMLKASteeringCmd", 0), + ] + + checks = [ + ("ASCMLKASteeringCmd", 50), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4719a5db5a..e9b5a13e7a 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -144,10 +144,11 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): self.cp.update_strings(can_strings) + self.cp_loopback.update_strings(can_strings) - ret = self.CS.update(self.cp) + ret = self.CS.update(self.cp, self.cp_loopback) - ret.canValid = self.cp.can_valid + ret.canValid = self.cp.can_valid and self.cp_loopback.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False buttonEvents = [] diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 85be9334c0..d475b4bd67 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -68,6 +68,7 @@ class CanBus: OBSTACLE = 1 CHASSIS = 2 SW_GMLAN = 3 + LOOPBACK = 128 FINGERPRINTS = { # Astra BK MY17, ASCM unplugged diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ada906367e..6e148b96a2 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -39,6 +39,7 @@ class CarInterfaceBase(): self.cp = self.CS.get_can_parser(CP) self.cp_cam = self.CS.get_cam_can_parser(CP) self.cp_body = self.CS.get_body_can_parser(CP) + self.cp_loopback = self.CS.get_loopback_can_parser(CP) self.CC = None if CarController is not None: @@ -254,3 +255,7 @@ class CarStateBase: @staticmethod def get_body_can_parser(CP): return None + + @staticmethod + def get_loopback_can_parser(CP): + return None diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 009b94c106..767b66c691 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -58674945e8c265e2c1f53500fc9cab2ed9a815b0 \ No newline at end of file +4ad66f0fea4c42e79ba347a73ff839e5369c0411 \ No newline at end of file