def test_correctness(self):
# Test all cars' commands, randomize the params.
for _ in xrange(1000):
apply_steer = (random.randint(0, 2) % 2 == 0)
frame = random.randint(1, 65536)
steer_step = random.randint(1, 65536)
m_old = subarucan.create_steering_control(self.subaru_cp_old,
subaru_car.IMPREZA,
apply_steer, frame,
steer_step)
m = subarucan.create_steering_control(self.subaru_cp,
subaru_car.IMPREZA,
apply_steer, frame,
steer_step)
self.assertEqual(m_old, m)
m_old = subarucan.create_steering_status(self.subaru_cp_old,
subaru_car.IMPREZA,
apply_steer, frame,
steer_step)
m = subarucan.create_steering_status(self.subaru_cp,
subaru_car.IMPREZA,
apply_steer, frame,
steer_step)
self.assertEqual(m_old, m)
es_distance_msg = {}
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
m_old = subarucan.create_es_distance(self.subaru_cp_old,
es_distance_msg,
pcm_cancel_cmd)
m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg,
pcm_cancel_cmd)
self.assertEqual(m_old, m)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
if not enabled:
apply_steer = 0.
if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
# add noise to prevent lkas fault from constant torque value for over 1s
if enabled and apply_steer == self.apply_steer_last:
self.counter = +1
if self.counter == 50:
apply_steer = round(int(apply_steer * 0.99))
else:
self.counter = 0
can_sends.append(
subarucan.create_steering_control(self.packer,
CS.CP.carFingerprint,
apply_steer, frame,
P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.car_fingerprint == CAR.IMPREZA:
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,
visual_alert, left_line,
right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
if self.car_fingerprint in (CAR.OUTBACK,
CAR.LEGACY) and pcm_cancel_cmd:
can_sends.append(subarucan.create_door_control(self.packer))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, dragonconf):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * 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, P)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
# 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.dpLatCtrl, dragonconf.dpSteeringOnSignal,
blinker_on or frame < self.blinker_end_frame, apply_steer)
self.last_blinker_on = blinker_on
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
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,
visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
""" Controls thread """
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * 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, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled
if not lkas_enabled:
apply_steer = 0
# dp
apply_steer = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer)
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
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, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * 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.steer_torque_driver, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
can_sends.append(
subarucan.create_steering_control(self.packer,
CS.CP.carFingerprint,
apply_steer, frame,
P.STEER_STEP))
self.apply_steer_last = apply_steer
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,
visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
apply_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
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, visual_alert))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = alca_steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
apply_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
# add noise to prevent lkas fault from constant torque value over 1s
if enabled and apply_steer == self.apply_steer_last:
self.counter =+ 1
if self.counter == 50:
apply_steer = round(int(apply_steer * 0.99))
else:
self.counter = 0
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.car_fingerprint not in (CAR.OUTBACK, CAR.LEGACY):
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, visual_alert))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd:
can_sends.append(subarucan.create_door_control(self.packer))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, left_lane_depart,
right_lane_depart, dragonconf):
can_sends = []
# *** steering ***
if (frame % CarControllerParams.STEER_STEP) == 0:
apply_steer = int(
round(actuators.steer * CarControllerParams.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,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
# 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
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer, frame,
CarControllerParams.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(
self.packer, apply_steer, frame,
CarControllerParams.STEER_STEP))
self.apply_steer_last = apply_steer
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_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_es_throttle_control(
self.packer, cruise_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_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,
visual_alert, left_line,
right_line, left_lane_depart,
right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
can_sends = []
# *** steering ***
if (frame % CarControllerParams.STEER_STEP) == 0:
apply_steer = int(
round(actuators.steer * CarControllerParams.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,
CarControllerParams)
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,
CarControllerParams.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(
self.packer, apply_steer, frame,
CarControllerParams.STEER_STEP))
self.apply_steer_last = apply_steer
#---------------------------------------------STOP AND GO---------------------------------------------------
if CS.CP.carFingerprint in PREGLOBAL_CARS:
#PRE-GLOBAL STOP AND GO
#Activate ACC Resume with throttle tap
if (enabled #Cruise must be activated
and CS.car_follow #Must have lead car
and CS.close_distance > CarControllerParams.
SNG_DISTANCE_THRESHOLD_PREGLOBAL #Distance with lead car > 3m (this is due to Preglobal ES's unreliable Close Distance signal)
and CS.close_distance <
4.5 #For safety, SnG will not operate if Close Distance reads more than 4.5m (Pre-global ES's unreliability, sometimes Close Distance shows max-5m when there is a stationary object ahead)
and CS.close_distance > self.
prev_close_distance #Distance with lead car is increasing
and CS.out.standstill #Must be standing still
):
self.sng_resume_acc = True
throttle_cmd = -1 #normally, just forward throttle msg from ECU
if self.sng_resume_acc:
#Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD
if self.sng_throttle_tap_cnt < 5:
throttle_cmd = 5
self.sng_throttle_tap_cnt += 1
else:
self.sng_throttle_tap_cnt = -1
self.sng_resume_acc = False
#Send throttle message
if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(
subarucan.create_preglobal_throttle_control(
self.packer, CS.throttle_msg, throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]
else:
#GLOBAL STOP AND GO
#Car can only be in HOLD state (3) if it is standing still
# => if not in HOLD state car has to be moving or driver has taken action
if CS.cruise_state != 3:
self.sng_throttle_tap_cnt = 0 #Reset throttle tap message count when car starts moving
self.sng_resume_acc = False #Cancel throttle tap when car starts moving
#Reset SnG flags
self.sng_resume_acc = False
self.sng_send_fake_speed = False
#Trigger THROTTLE TAP when in hold and close_distance increases > SNG distance threshold (with deadband)
#false positives caused by pedestrians/cyclists crossing the street in front of car
if (enabled and
CS.cruise_state == 3 #cruise state == 3 => ACC HOLD state
and CS.close_distance > CarControllerParams.
SNG_DISTANCE_THRESHOLD #lead car distance is within SnG operating range
and CS.close_distance < 255 and CS.close_distance > self.
prev_close_distance #Distance with lead car is increasing
and CS.car_follow == 1):
self.sng_resume_acc = True
#If standstill for 1+ seconds and CruiseState is not 3, then this car has no EPB
#Exception: SUBARU ASCENT, for some reason, Ascent even on Global does not resume out of HOLD mode with throttle tap
#so we prevent ASCENTs from entering HOLD in the first place
if (enabled and
(CS.cruise_state != 3 #cruise state == 3 => ACC HOLD state
or CS.CP.carFingerprint == CAR.ASCENT
) #Except for SUBARU ASCENT
and CS.out.standstill #car standstill
and time.time_ns() > self.standstill_transition_timestamp +
CarControllerParams.NON_EPB_STANDSTILL_THRESHOLD
): #for more than 1 second
#send fake speed to ES, because we know this car has no EPB
self.sng_send_fake_speed = True
#default to forward wheel speed reported by ECU
wheel_speed = -1
if self.sng_send_fake_speed:
#only fake wheel speed if ACC engaged and car has come to a full stop for 1 second (to prevent dodgy braking)
wheel_speed = CarControllerParams.NON_EPB_FAKE_SPEED
#Send a throttle tap to resume ACC
throttle_cmd = -1 #normally, just forward throttle msg from ECU
if self.sng_resume_acc:
#Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD
if self.sng_throttle_tap_cnt < CarControllerParams.THROTTLE_TAP_LIMIT:
throttle_cmd = CarControllerParams.THROTTLE_TAP_LEVEL
self.sng_throttle_tap_cnt += 1
else:
self.sng_throttle_tap_cnt = -1
self.sng_resume_acc = False
#TODO: Send cruise throttle to get car up to speed. There is a 2-3 seconds delay after
# throttle tap is sent and car start moving. EDIT: This is standard with Toyota OP's SnG
#pseudo: !!!WARNING!!! Dangerous, proceed with CARE
#if sng_resume_acc is True && has been 1 second since sng_resume_acc turns to True && current ES_Throttle < 2000
# send ES_Throttle = 2000
#Update prev values
self.prev_close_distance = CS.close_distance
self.prev_cruise_state = CS.cruise_state
#Send throttle message
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"]
#Send Brake_Pedal CAN message to fool ES
if self.brake_pedal_cnt != CS.brake_pedal_msg["Counter"]:
can_sends.append(
subarucan.create_brake_pedal(self.packer,
CS.brake_pedal_msg,
wheel_speed))
self.brake_pedal_cnt = CS.brake_pedal_msg["Counter"]
#record standstill time when it transitions from False to True
if CS.out.standstill and not self.prev_standstill:
self.standstill_transition_timestamp = time.time_ns()
self.prev_standstill = CS.out.standstill
#--------------------------------------------------------------------------------------------------------------
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_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:
fake_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
fake_button = 1
else:
fake_button = CS.button
# unstick previous mocked button press
if fake_button == 1 and self.fake_button_prev == 1:
fake_button = 0
self.fake_button_prev = fake_button
can_sends.append(
subarucan.create_es_throttle_control(
self.packer, fake_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_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,
visual_alert, left_line,
right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return 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.OUTBACK:
if apply_steer != 0:
chksm_steer = apply_steer * -1
chksm_engage = 1
else:
chksm_steer = 0
chksm_engage = 0
#counts from 0 to 7 then back to 0
idx = (frame / P.STEER_STEP) % 8
steer2 = (chksm_steer >> 8) & 0x1F
steer1 = chksm_steer - (steer2 << 8)
byte2 = steer2
checksum = (idx + steer2 + steer1 + chksm_engage) % 256
if (self.car_fingerprint == CAR.XV2018):
#counts from 0 to 15 then back to 0 + 16 for enable bit
chksm_steer = apply_steer * -1
if chksm_steer != 0:
left3 = 32
else:
left3 = 0
idx = ((frame / P.STEER_STEP) % 16) + 16
steer2 = (chksm_steer >> 8) & 0x1F
steer1 = chksm_steer - (steer2 << 8)
byte2 = steer2 + left3
checksum = ((idx + steer1 + byte2) % 256) + 35
can_sends.append(
subarucan.create_steering_control(self.packer_pt,
canbus.powertrain,
CS.CP.carFingerprint, idx,
steer1, byte2, checksum))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * 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.steer_torque_driver, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled
if not lkas_enabled:
apply_steer = 0
self.apply_steer_last = apply_steer
if not enabled:
apply_steer = 0.
can_sends.append(
subarucan.create_steering_control(self.packer,
CS.CP.carFingerprint,
apply_steer, frame,
P.STEER_STEP))
### DISENGAGE ###
if self.car_fingerprint == CAR.IMPREZA:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
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))
# button control
if (frame % 5) == 0 and self.car_fingerprint in (CAR.OUTBACK,
CAR.LEGACY,
CAR.FORESTER):
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if (pcm_cancel_cmd):
fake_button = 1
# turn main on if off and past start-up state
elif not CS.main_on and CS.ready:
fake_button = 1
else:
fake_button = CS.button
# unstick previous mocked button press
if fake_button != 0 and fake_button == self.fake_button_prev:
fake_button = 0
self.fake_button_prev = fake_button
can_sends.append(
subarucan.create_es_throttle_control(self.packer, fake_button,
CS.es_accel_msg))
### ALERTS ###
if self.car_fingerprint == CAR.IMPREZA:
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
visual_alert, left_line,
right_line))
return 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
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
can_sends = []
# *** steering ***
if (frame % self.params.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.params.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.params)
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.params.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame,
self.params.STEER_STEP))
self.apply_steer_last = apply_steer
#---------------------------------------------STOP AND GO---------------------------------------------------
if CS.CP.carFingerprint in PREGLOBAL_CARS:
#PRE-GLOBAL STOP AND GO
#Activate ACC Resume with throttle tap
if (enabled #Cruise must be activated
and CS.car_follow #Must have lead car
and CS.close_distance > self.params.
SNG_DISTANCE_THRESHOLD_PREGLOBAL #Distance with lead car > 3m (this is due to Preglobal ES's unreliable Close Distance signal)
and CS.close_distance <
4.5 #For safety, SnG will not operate if Close Distance reads more than 4.5m (Pre-global ES's unreliability, sometimes Close Distance shows max-5m when there is a stationary object ahead)
and CS.close_distance > self.
prev_close_distance #Distance with lead car is increasing
and CS.out.standstill #Must be standing still
):
self.sng_resume_acc = True
throttle_cmd = -1 #normally, just forward throttle msg from ECU
if self.sng_resume_acc:
#Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD
if self.sng_throttle_tap_cnt < 5:
throttle_cmd = 5
self.sng_throttle_tap_cnt += 1
else:
self.sng_throttle_tap_cnt = -1
self.sng_resume_acc = False
#Send throttle message
if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(
subarucan.create_preglobal_throttle_control(
self.packer, CS.throttle_msg, throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]
else:
#GLOBAL STOP AND GO
#Car can only be in HOLD state (3) if it is standing still
# => if not in HOLD state car has to be moving or driver has taken action
if CS.cruise_state != 3:
self.sng_throttle_tap_cnt = 0 #Reset throttle tap message count when car starts moving
self.sng_resume_acc = False #Cancel throttle tap when car starts moving
#Trigger THROTTLE TAP when in hold and close_distance increases > SNG distance threshold (with deadband)
#false positives caused by pedestrians/cyclists crossing the street in front of car
self.sng_resume_acc = False
if (enabled and
CS.cruise_state == 3 #cruise state == 3 => ACC HOLD state
and CS.close_distance > self.params.
SNG_DISTANCE_THRESHOLD #lead car distance is within SnG operating range
and CS.close_distance < 255 and CS.close_distance > self.
prev_close_distance #Distance with lead car is increasing
and CS.car_follow == 1):
self.sng_resume_acc = True
#Send a throttle tap to resume ACC
throttle_cmd = -1 #normally, just forward throttle msg from ECU
if self.sng_resume_acc:
#Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD
if self.sng_throttle_tap_cnt < self.params.THROTTLE_TAP_LIMIT:
throttle_cmd = self.params.THROTTLE_TAP_LEVEL
self.sng_throttle_tap_cnt += 1
else:
self.sng_throttle_tap_cnt = -1
self.sng_resume_acc = False
#TODO: Send cruise throttle to get car up to speed. There is a 2-3 seconds delay after
# throttle tap is sent and car start moving. EDIT: This is standard with Toyota OP's SnG
#pseudo: !!!WARNING!!! Dangerous, proceed with CARE
#if sng_resume_acc is True && has been 1 second since sng_resume_acc turns to True && current ES_Throttle < 2000
# send ES_Throttle = 2000
#Update prev values
self.prev_close_distance = CS.close_distance
self.prev_cruise_state = CS.cruise_state
#Send throttle message
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"]
#--------------------------------------------------------------------------------------------------------------
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_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:
fake_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
fake_button = 1
else:
fake_button = CS.button
# unstick previous mocked button press
if fake_button == 1 and self.fake_button_prev == 1:
fake_button = 0
self.fake_button_prev = fake_button
can_sends.append(
subarucan.create_es_throttle_control(
self.packer, fake_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_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,
visual_alert, left_line,
right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
