Mazda: fix up cruise cancellation (#22750)
* also cancel on cruiseControl.cancel * set counter in button msgpull/22753/head
parent
6bca73edd8
commit
40b7962b93
|
@ -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
|
||||
|
||||
|
|
|
@ -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),
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue