diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 977d20c5b3..7c883dacc0 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -21,7 +21,8 @@ class CarController: self.frame = 0 self.last_button_frame = 0 - self.lka_steering_cmd_counter_last = -1 + self.lka_steering_cmd_counter = 0 + self.sent_lka_steering_cmd = False self.lka_icon_status_last = (False, False) self.params = CarControllerParams() @@ -44,9 +45,14 @@ class CarController: # 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 + if CS.loopback_lka_steering_cmd_updated: + self.lka_steering_cmd_counter += 1 + self.sent_lka_steering_cmd = True elif (self.frame % self.params.STEER_STEP) == 0: + # Initialize ASCMLKASteeringCmd counter using the camera + if not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera: + self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 + if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -54,10 +60,7 @@ class CarController: apply_steer = 0 self.apply_steer_last = apply_steer - # 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 - + idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) if self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f96a234dbd..6d1e2b9d44 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -15,7 +15,8 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] - self.lka_steering_cmd_counter = 0 + self.loopback_lka_steering_cmd_updated = False + self.camera_lka_steering_cmd_counter = 0 self.buttons_counter = 0 def update(self, pt_cp, cam_cp, loopback_cp): @@ -25,6 +26,11 @@ class CarState(CarStateBase): self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] + # Variables used for avoiding LKAS faults + self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]) > 0 + if self.CP.networkLocation == NetworkLocation.fwdCamera: + self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], @@ -59,7 +65,6 @@ 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"] @@ -94,8 +99,14 @@ class CarState(CarStateBase): signals = [] checks = [] if CP.networkLocation == NetworkLocation.fwdCamera: - signals.append(("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus")) - checks.append(("ASCMActiveCruiseControlStatus", 25)) + signals += [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"), + ] + checks += [ + ("ASCMLKASteeringCmd", 10), + ("ASCMActiveCruiseControlStatus", 25), + ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.CAMERA) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index db3684fb0b..6b64fcb5aa 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -02c6922762fef41c8188a5a4f1f3267b76651330 \ No newline at end of file +5fa6a3743f2678eef13267fb946d7a03f2af5824