diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index f5347b9d6..8882eecb6 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/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 - 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.brake_counter = 0 self.apply_steer_last = apply_steer diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 7b7a1d50b..0a1815948 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/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), diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index a5400e886..e2ee93e02 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/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)