From c89893942374c207cc2b2473984e0700a7de7ec4 Mon Sep 17 00:00:00 2001 From: Jafar Al-Gharaibeh Date: Sun, 10 Oct 2021 16:24:57 -0500 Subject: [PATCH] Mazda: fix disabling cruise main on after cancelling cruise (#22193) On some disengagement events (brake/gas/etc) OP may occasionally ends up disabling the main cruise control where the driver has to press the main cruise button before they can engage ACC again. Try to detect this situation and automatically turn on cruise when that happens. Signed-off-by: Jafar Al-Gharaibeh --- selfdrive/car/mazda/carcontroller.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) 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