diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 5d9e9c9bb3..bda5378dd6 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -8,6 +8,7 @@ class CarController(): self.apply_steer_last = 0 self.packer = CANPacker(dbc_name) self.steer_rate_limited = False + self.brake_counter = 0 def update(self, enabled, CS, frame, actuators): """ Controls thread """ @@ -31,10 +32,17 @@ class CarController(): else: apply_steer = 0 self.steer_rate_limited = False - if CS.out.cruiseState.enabled and frame % 20 == 0: - # Cancel Stock ACC if it's enabled while OP is disengaged - # Send at a rate of 5hz until we sync with stock ACC state - can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL)) + if CS.out.cruiseState.enabled: + # if brake is pressed, let us wait >20ms before trying to disable crz to avoid + # a race condition with the stock system, where the second cancel from openpilot + # will disable the crz 'main on' + self.brake_counter = self.brake_counter + 1 + if frame % 20 == 0 and not (CS.out.brakePressed and self.brake_counter < 3): + # Cancel Stock ACC if it's enabled while OP is disengaged + # Send at a rate of 5hz until we sync with stock ACC state + can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL)) + else: + self.brake_counter = 0 self.apply_steer_last = apply_steer