def update(self, enabled, CS, frame, actuators):
""" Controls thread """
can_sends = []
### STEER ###
if enabled:
# calculate steer and also set limits due to driver torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if CS.out.standstill and frame % 5 == 0:
# 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))
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))
self.apply_steer_last = apply_steer
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
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
new_steer = int(
round(c.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if CS.out.standstill and frame % 5 == 0:
# 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,
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.brake_counter = 0
self.apply_steer_last = apply_steer
# send HUD alerts
if frame % 50 == 0:
ldw = c.hudControl.visualAlert == VisualAlert.ldw
steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired
can_sends.append(
mazdacan.create_alert_command(self.packer, CS.cam_laneinfo,
ldw, steer_required))
# send steering command
can_sends.append(
mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
def update(self, enabled, CS, frame, actuators, dragonconf):
""" Controls thread """
can_sends = []
### STEER ###
if enabled:
# calculate steer and also set limits due to driver torque
new_steer = int(round(actuators.steer *
SteerLimitParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
if CS.out.standstill and frame % 20 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 5hz 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))
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))
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
can_sends.append(
mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
def update(self, CC, CS):
can_sends = []
apply_steer = 0
self.steer_rate_limited = False
if CC.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms 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'. crz ctrl msg runs at 50hz. 70ms allows us to
# read 3 messages and most likely sync state before we attempt cancel.
self.brake_counter = self.brake_counter + 1
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
self.apply_steer_last = apply_steer
# send HUD alerts
if self.frame % 50 == 0:
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
# TODO: find a way to silence audible warnings so we can add more hud alerts
steer_required = steer_required and CS.lkas_allowed_speed
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
self.frame, apply_steer, CS.cam_lkas))
new_actuators = CC.actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
self.frame += 1
return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = final_steer * P.STEER_MAX
# limits due to driver torque
driver_max_torque = P.STEER_MAX + (
P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
driver_min_torque = -P.STEER_MAX + (
-P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0)
min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0)
apply_steer = clip(apply_steer, min_steer_allowed,
max_steer_allowed)
# slow rate if steer torque increases in magnitude
if self.apply_steer_last > 0:
apply_steer = clip(
apply_steer,
max(self.apply_steer_last - P.STEER_DELTA_DOWN,
-P.STEER_DELTA_UP),
self.apply_steer_last + P.STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.apply_steer_last - P.STEER_DELTA_UP,
min(self.apply_steer_last + P.STEER_DELTA_DOWN,
P.STEER_DELTA_UP))
apply_steer = int(round(apply_steer))
self.apply_steer_last = apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
if self.car_fingerprint == CAR.CX5:
#counts from 0 to 15 then back to 0
ctr = (frame / P.STEER_STEP) % 16
#ctr = CS.CAM_LKAS.ctr #(frame / P.STEER_STEP) % 16
if ctr != -1 and self.last_cam_ctr != ctr:
self.last_cam_ctr = ctr
#line_not_visible = CS.CAM_LKAS.lnv
if CS.v_ego_raw > 4:
line_not_visible = 0
else:
line_not_visible = 1
e1 = CS.CAM_LKAS.err1
e2 = CS.CAM_LKAS.err2
# assuming controlsd runs at 83 hz
# 2000ms => 166.6
# 1000ms => 83.3
# 500ms => 41.65
tsec = 167
osec = 83
hsec = 42
qsec = 21
q3sec = 62
if CS.steer_lkas.handsoff == 1 and self.ldw == 0:
self.handsoff_ctr += 1
if self.handsoff_ctr > tsec:
self.ldw_ctr = osec
self.handsoff_ctr = 0
self.ldw = 1
if apply_steer > 0:
self.ldwr = 1
self.ldwl = 0
else:
self.ldwr = 0
self.ldwl = 1
#else:
# self.handsoff_ctr = 0
if self.ldw == 0 and CS.steer_lkas.block == 1 and CS.v_ego_raw > 54 and apply_steer != 0:
self.ldw = 1
self.ldw_ctr = osec
if apply_steer > 0:
self.ldwr = 1
self.ldwl = 0
else:
self.ldwr = 0
self.ldwl = 1
ldw = 0
if self.ldw_ctr > 0:
self.ldw_ctr -= 1
if self.ldw_ctr == 0:
self.ldw = 0
self.ldwl = 0
self.ldwr = 0
elif self.ldw_ctr < q3sec and self.ldw_ctr > qsec:
ldw = 1
can_sends.append(
mazdacan.create_steering_control(
self.packer_pt, canbus.powertrain,
CS.CP.carFingerprint, ctr, apply_steer,
line_not_visible, 1, 1, e1, e2, ldw))
# send lane info msgs at 1/8 rate of steer msgs
if (ctr % 8 == 0):
can_sends.append(
mazdacan.create_cam_lane_info(
self.packer_pt, canbus.powertrain,
CS.CP.carFingerprint, line_not_visible,
CS.cam_laneinfo, CS.steer_lkas, self.ldwr,
self.ldwl))
#can_sends.append(mazdacan.create_lkas_msg(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LKAS))
#can_sends.append(mazdacan.create_lane_track(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LT))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())