Mazda: fix up cruise cancellation (#22750)

* also cancel on cruiseControl.cancel

* set counter in button msg
old-commit-hash: 40b7962b93

selfdrive/car/mazda/carcontroller.py

@@ -15,7 +15,9 @@ class CarController():
 
   def update(self, c, CS, frame):
     can_sends = []
+
     apply_steer = 0
+    self.steer_rate_limited = False
 
     if c.enabled:
       # calculate steer and also set limits due to driver torque
@@ -28,20 +30,19 @@ class CarController():
         # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
         # Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
         # TODO: improve the resume trigger logic by looking at actual radar data
-        can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME))
+        can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
+
+    if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.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, CS.crz_btns_counter, Buttons.CANCEL))
     else:
-      self.steer_rate_limited = False
+      self.brake_counter = 0
-      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
 

selfdrive/car/mazda/carstate.py

@@ -12,6 +12,7 @@ class CarState(CarStateBase):
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["GEAR"]["GEAR"]
 
+    self.crz_btns_counter = 0
     self.acc_active_last = False
     self.low_speed_alert = False
     self.lkas_allowed = False
@@ -84,6 +85,7 @@ class CarState(CarStateBase):
 
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
+    self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
     ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     return ret
@@ -134,9 +136,6 @@ class CarState(CarStateBase):
         ("BR", "DOORS", 0),
         ("PEDAL_GAS", "ENGINE_DATA", 0),
         ("SPEED", "ENGINE_DATA", 0),
-        ("RES", "CRZ_BTNS", 0),
-        ("SET_P", "CRZ_BTNS", 0),
-        ("SET_M", "CRZ_BTNS", 0),
         ("CTR", "CRZ_BTNS", 0),
         ("LEFT_BS1", "BSM", 0),
         ("RIGHT_BS1", "BSM", 0),

selfdrive/car/mazda/mazdacan.py

@@ -79,7 +79,7 @@ def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool)
   return packer.make_can_msg("CAM_LANEINFO", 0, values)
 
 
-def create_button_cmd(packer, car_fingerprint, button):
+def create_button_cmd(packer, car_fingerprint, counter, button):
 
   can = int(button == Buttons.CANCEL)
   res = int(button == Buttons.RESUME)
@@ -113,7 +113,7 @@ def create_button_cmd(packer, car_fingerprint, button):
       "BIT1": 1,
       "BIT2": 1,
       "BIT3": 1,
-      "CTR": 0
+      "CTR": (counter + 1) % 16,
     }
 
     return packer.make_can_msg("CRZ_BTNS", 0, values)