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 <to.jafar@gmail.com>

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