def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
can_sends = []
# *** steering ***
if (self.frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
self.p)
if not CC.latActive:
apply_steer = 0
if self.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer))
else:
can_sends.append(
subarucan.create_steering_control(self.packer,
apply_steer))
self.apply_steer_last = apply_steer
# *** alerts and pcm cancel ***
if self.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_distance_cnt != CS.es_distance_msg["COUNTER"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
cruise_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
cruise_button = 1
else:
cruise_button = CS.cruise_button
# unstick previous mocked button press
if cruise_button == 1 and self.cruise_button_prev == 1:
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(
subarucan.create_preglobal_es_distance(
self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["COUNTER"]
else:
if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0
can_sends.append(
subarucan.create_es_distance(self.packer,
CS.es_distance_msg, bus,
pcm_cancel_cmd))
self.last_cancel_frame = self.frame
if self.es_dashstatus_cnt != CS.es_dashstatus_msg["COUNTER"]:
can_sends.append(
subarucan.create_es_dashstatus(self.packer,
CS.es_dashstatus_msg))
self.es_dashstatus_cnt = CS.es_dashstatus_msg["COUNTER"]
if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
CC.enabled,
hud_control.visualAlert,
hud_control.leftLaneVisible,
hud_control.rightLaneVisible,
hud_control.leftLaneDepart,
hud_control.rightLaneDepart))
self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"]
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
self.frame += 1
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, left_lane_depart,
right_lane_depart):
can_sends = []
# *** steering ***
if (frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
self.p)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer, frame, self.p.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame,
self.p.STEER_STEP))
self.apply_steer_last = apply_steer
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
cruise_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
cruise_button = 1
else:
cruise_button = CS.cruise_button
# unstick previous mocked button press
if cruise_button == 1 and self.cruise_button_prev == 1:
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(
subarucan.create_preglobal_es_distance(
self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
else:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer,
CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
enabled, visual_alert, left_line,
right_line, left_lane_depart,
right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, left_lane_depart,
right_lane_depart):
can_sends = []
# *** steering ***
if (frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
self.p)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
self.torqueEPS_last = 0
self.stock_apply_last = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer, frame, self.p.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame,
self.p.STEER_STEP))
self.apply_steer_last = apply_steer
self.torqueEPS_last = CS.out.steeringTorqueEps * self.p.STEER_EPS_MULTIPLIER
self.stock_apply_last = CS.out.stockSteeringTorqueRequest * self.p.STOCK_ES_FACTOR
# *** stop and go ***
throttle_cmd = False
speed_cmd = False
if CS.CP.carFingerprint in PREGLOBAL_CARS:
# Initiate the ACC resume sequence if conditions are met
if (enabled # ACC active
and CS.car_follow == 1 # lead car
and CS.out.standstill # must be standing still
and CS.close_distance > 3 # acc resume trigger threshold
and CS.close_distance <
4.5 # max operating distance to filter false positives
and CS.close_distance > self.prev_close_distance
): # distance with lead car is increasing
self.sng_acc_resume = True
# Cancel ACC if stopped, brake pressed and not stopped behind another car
if enabled and CS.out.brakePressed and CS.car_follow == 0 and CS.out.standstill:
pcm_cancel_cmd = True
elif CS.CP.carFingerprint in GLOBAL_CARS_SNG:
if CS.has_epb:
# Record manual hold set while in standstill and no car in front
if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0:
self.manual_hold = True
# Cancel manual hold when car starts moving
if not CS.out.standstill:
self.manual_hold = False
# Initiate the ACC resume sequence if conditions are met
if (enabled # ACC active
and not self.manual_hold and
CS.car_follow == 1 # lead car
and CS.cruise_state == 3 # ACC HOLD (only with EPB)
and
CS.close_distance > 150 # acc resume trigger threshold
and CS.close_distance < 255 # ignore max value
and CS.close_distance > self.prev_close_distance
): # distance with lead car is increasing
self.sng_acc_resume = True
else:
if (enabled # ACC active
and CS.car_follow == 1 # lead car
and
CS.out.standstill and frame > self.standstill_start +
50): # standstill for >0.5 second
speed_cmd = True
if CS.out.standstill and not self.prev_standstill:
self.standstill_start = frame
self.prev_standstill = CS.out.standstill
self.prev_cruise_state = CS.cruise_state
if self.sng_acc_resume:
if self.sng_acc_resume_cnt < 5:
throttle_cmd = True
self.sng_acc_resume_cnt += 1
else:
self.sng_acc_resume = False
self.sng_acc_resume_cnt = -1
if CS.CP.carFingerprint != CAR.CROSSTREK_2020H:
self.prev_close_distance = CS.close_distance
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
cruise_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
cruise_button = 1
else:
cruise_button = CS.cruise_button
# unstick previous mocked button press
if cruise_button == 1 and self.cruise_button_prev == 1:
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(
subarucan.create_preglobal_es_distance(
self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(
subarucan.create_preglobal_throttle(
self.packer, CS.throttle_msg, throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]
else:
if CS.CP.carFingerprint not in [CAR.CROSSTREK_2020H, CAR.OUTBACK]:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer,
CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
# Only cancel ACC using brake_pedal for models that do not support ES_Distance
if pcm_cancel_cmd:
pcm_cancel_cmd = False
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
enabled, visual_alert, left_line,
right_line, left_lane_depart,
right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
if self.es_dashstatus_cnt != CS.es_dashstatus_msg["Counter"]:
can_sends.append(
subarucan.create_es_dashstatus(self.packer,
CS.es_dashstatus_msg))
self.es_dashstatus_cnt = CS.es_dashstatus_msg["Counter"]
if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(
subarucan.create_throttle(self.packer, CS.throttle_msg,
throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]
if self.brake_pedal_cnt != CS.brake_pedal_msg["Counter"]:
can_sends.append(
subarucan.create_brake_pedal(self.packer,
CS.brake_pedal_msg, speed_cmd,
pcm_cancel_cmd))
self.brake_pedal_cnt = CS.brake_pedal_msg["Counter"]
return can_sends