class CarController(object):
def __init__(self, dbc_name, enable_camera=True):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
#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)
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame / 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx))
elif CS.stopped:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
hondacan.create_gas_command(self.packer, apply_gas,
idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera):
self.apply_steer_last = 0
self.turning_signal_timer = 0
self.car_fingerprint = car_fingerprint
self.lkas11_cnt = 0
self.mdps12_cnt = 0
self.cnt = 0
self.last_resume_cnt = 0
self.map_speed = 0
self.enable_camera = enable_camera
# True when camera present, and we need to replace all the camera messages
# otherwise we forward the camera msgs and we just replace the lkas cmd signals
self.camera_disconnected = False
self.packer = CANPacker(dbc_name)
context = zmq.Context()
self.params = Params()
self.map_data_sock = messaging.sub_sock(
context, service_list['liveMapData'].port, conflate=True)
self.speed_conv = 3.6
self.speed_adjusted = False
self.checksum = "NONE"
self.checksum_learn_cnt = 0
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.lkas = False
self.spas_present = True # TODO Make Automatic
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd,
hud_alert):
if not self.enable_camera:
return
if CS.camcan > 0:
if self.checksum == "NONE":
self.checksum = learn_checksum(self.packer, CS.lkas11)
print("Discovered Checksum", self.checksum)
if self.checksum == "NONE":
return
elif CS.steer_error == 1:
if self.checksum_learn_cnt > 200:
self.checksum_learn_cnt = 0
if self.checksum == "NONE":
print("Testing 6B Checksum")
self.checksum == "6B"
elif self.checksum == "6B":
print("Testing 7B Checksum")
self.checksum == "7B"
elif self.checksum == "7B":
print("Testing CRC8 Checksum")
self.checksum == "crc8"
else:
self.checksum == "NONE"
else:
self.checksum_learn_cnt += 1
force_enable = False
# I don't care about your opinion, deal with it!
if (CS.cstm_btns.get_button_status("alwon") > 0) and CS.acc_active:
enabled = True
force_enable = True
if (self.car_fingerprint in FEATURES["soft_disable"]
and CS.v_wheel < 16.8):
enabled = False
force_enable = False
if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1):
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (self.cnt % 100 == 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)
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, self.cnt, actuators)
if force_enable and not CS.acc_active:
apply_steer = int(
round(actuators.steer * SteerLimitParams.STEER_MAX))
else:
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
# SPAS limit angle extremes for safety
apply_steer_ang_req = np.clip(actuators.steerAngle,
-1 * (SteerLimitParams.STEER_ANG_MAX),
SteerLimitParams.STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang - apply_steer_ang_req) > 0.6:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += 0.5
else:
self.apply_steer_ang -= 0.5
else:
self.apply_steer_ang = apply_steer_ang_req
# Limit steer rate for safety
apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last,
SteerLimitParams,
CS.steer_torque_driver)
if alca_enabled:
self.turning_signal_timer = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer = self.turning_signal_timer - 1
turning_signal = 1
else:
turning_signal = 0
# Use LKAS or SPAS
if CS.mdps11_stat == 7 or CS.v_wheel > 2.7:
self.lkas = True
elif CS.v_wheel < 0.1:
self.lkas = False
if self.spas_present:
self.lkas = True
# If ALCA is disabled, and turning indicators are turned on, we do not want OP to steer,
if not enabled or (turning_signal and not alca_enabled):
if self.lkas:
apply_steer = 0
else:
self.apply_steer_ang = 0.0
self.en_cnt = 0
steer_req = 1 if enabled and self.lkas else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
self.mdps12_cnt = self.cnt % 0x100
self.spas_cnt = self.cnt % 0x200
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, \
enabled if self.lkas else False, False if CS.camcan == 0 else CS.lkas11, hud_alert, (CS.cstm_btns.get_button_status("cam") > 0), \
(False if CS.camcan == 0 else True), self.checksum))
if CS.camcan > 0:
can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \
CS.camcan, self.checksum))
# SPAS11 50hz
if (self.cnt % 2) == 0 and not self.spas_present:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas == 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and enabled and not self.lkas:
self.en_spas = 4
elif self.en_cnt >= 8 and enabled and not self.lkas:
self.en_spas = 5
if self.lkas or not enabled:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(
create_spas11(self.packer, (self.spas_cnt / 2), self.en_spas,
self.apply_steer_ang, self.checksum))
# SPAS12 20Hz
if (self.cnt % 5) == 0 and not self.spas_present:
can_sends.append(create_spas12(self.packer))
# Force Disable
if pcm_cancel_cmd and (not force_enable):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0))
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0))
# Speed Limit Related Stuff Lot's of comments for others to understand!
# Run this twice a second
if (self.cnt % 50) == 0:
if self.params.get("LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None:
# If Not Enabled, or cruise not set, allow auto speed adjustment again
if not (enabled and CS.acc_active_real):
self.speed_adjusted = False
# Attempt to read the speed limit from zmq
map_data = messaging.recv_one_or_none(self.map_data_sock)
# If we got a message
if map_data != None:
# See if we use Metric or dead kings extremeties for measurements, and set a variable to the conversion value
if bool(self.params.get("IsMetric")):
self.speed_conv = CV.MS_TO_KPH
else:
self.speed_conv = CV.MS_TO_MPH
# If the speed limit is valid
if map_data.liveMapData.speedLimitValid:
last_speed = self.map_speed
# Get the speed limit, and add the offset to it,
v_speed = (map_data.liveMapData.speedLimit +
float(self.params.get("SpeedLimitOffset")))
## Stolen curvature code from planner.py, and updated it for us
v_curvature = 45.0
if map_data.liveMapData.curvatureValid:
v_curvature = math.sqrt(
1.85 /
max(1e-4, abs(map_data.liveMapData.curvature)))
# Use the minimum between Speed Limit and Curve Limit, and convert it as needed
#self.map_speed = min(v_speed, v_curvature) * self.speed_conv
self.map_speed = v_speed * self.speed_conv
# Compare it to the last time the speed was read. If it is different, set the flag to allow it to auto set our speed
if last_speed != self.map_speed:
self.speed_adjusted = False
else:
# If it is not valid, set the flag so the cruise speed won't be changed.
self.map_speed = 0
self.speed_adjusted = True
else:
self.speed_adjusted = True
# Ensure we have cruise IN CONTROL, so we don't do anything dangerous, like turn cruise on
# Ensure the speed limit is within range of the stock cruise control capabilities
# Do the spamming 10 times a second, we might get from 0 to 10 successful
# Only do this if we have not yet set the cruise speed
if CS.acc_active_real and not self.speed_adjusted and self.map_speed > (
8.5 * self.speed_conv) and (self.cnt % 9 == 0
or self.cnt % 9 == 1):
# Use some tolerance because of Floats being what they are...
if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed *
1.005):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL,
(1 if self.cnt % 9 == 1 else 0)))
elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed /
1.005):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL,
(1 if self.cnt % 9 == 1 else 0)))
# If nothing needed adjusting, then the speed has been set, which will lock out this control
else:
self.speed_adjusted = True
### If Driver Overrides using accelerator (or gas for the antiquated), cancel auto speed adjustment
if CS.pedal_gas:
self.speed_adjusted = True
### Send messages to canbus
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
self.cnt += 1
class CarController(object):
def __init__(self, canbus, car_fingerprint, allow_controls):
self.pedal_steady = 0.
self.start_time = sec_since_boot()
self.chime = 0
self.steer_idx = 0
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
self.allow_controls = allow_controls
self.lka_icon_status_last = (False, False)
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
# Setup detection helper. Routes commands to
# an appropriate CAN bus number.
self.canbus = canbus
self.params = CarControllerParams(car_fingerprint)
self.packer_pt = CANPacker(DBC[car_fingerprint]['pt'])
self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
#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)
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
if lkas_enabled:
apply_steer = alca_steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if not CS.lane_departure_toggle_on:
apply_steer = 0
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
else:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
### GAS/BRAKE ###
if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
acc_enabled = enabled
if CS.cstm_btns.get_button_status("stop") > 0:
# Auto-resume from full stop by resetting ACC control
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, acc_enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
enable_apg):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.last_fault_frame = -200
self.fake_ecus = set()
if enable_camera: self.fake_ecus.add(ECU.CAM)
if enable_dsu: self.fake_ecus.add(ECU.DSU)
if enable_apg: self.fake_ecus.add(ECU.APGS)
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead):
#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)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# 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)
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, enable_camera=True):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.epas_disabled = True
self.last_angle = 0.
self.last_accel = 0.
self.ALCA = ALCAController(self, True,
True) # Enabled and SteerByAngle both True
self.ACC = ACCController()
self.PCC = PCCController(self)
self.HSO = HSOController(self)
self.sent_DAS_bootID = False
context = zmq.Context()
self.poller = zmq.Poller()
self.speedlimit = messaging.sub_sock(context,
service_list['speedLimit'].port,
conflate=True,
poller=self.poller)
self.speedlimit_mph = 0
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
if not self.enable_camera:
return
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
STEER_MAX = 420
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
# Update statuses for custom buttons 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)
#print CS.cstm_btns.get_button_status("alca")
if CS.pedal_interceptor_available:
#update PCC module info
self.PCC.update_stat(CS, True, sendcan)
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
human_control = False
# update CS.v_cruise_pcm based on module selected.
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph
else:
CS.v_cruise_pcm = CS.v_cruise_actual
# Get the angle from ALCA.
alca_enabled = False
turn_signal_needed = 0
alca_steer = 0.
apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_angle = -apply_angle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not alca_enabled
enable_steer_control = (enabled and not human_lane_changing
and not human_control)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# If blinker is on send the actual angle.
#if (changing_lanes and (CS.laneChange_enabled < 2)):
# apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
send_step = 5
if (True):
#First we emulate DAS.
#send DAS_bootID
if not self.sent_DAS_bootID:
can_sends.append(teslacan.create_DAS_bootID_msg())
self.sent_DAS_bootID = True
else:
#get speed limit
for socket, _ in self.poller.poll(0):
if socket is self.speedlimit:
self.speedlimit_mph = ui.SpeedLimitData.from_bytes(
socket.recv()).speed * CV.MS_TO_MPH
#send DAS_info
if frame % 100 == 0:
can_sends.append(
teslacan.create_DAS_info_msg(CS.DAS_info_msg))
CS.DAS_info_msg += 1
CS.DAS_info_msg = CS.DAS_info_msg % 10
#send DAS_status
if frame % 50 == 0:
op_status = 0x02
hands_on_state = 0x00
speed_limit_kph = int(self.speedlimit_mph)
alca_state = 0x08
if enabled:
op_status = 0x03
alca_state = 0x08 + turn_signal_needed
#if not enable_steer_control:
#op_status = 0x04
#hands_on_state = 0x03
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
can_sends.append(
teslacan.create_DAS_status_msg(CS.DAS_status_idx,
op_status,
speed_limit_kph,
alca_state,
hands_on_state))
CS.DAS_status_idx += 1
CS.DAS_status_idx = CS.DAS_status_idx % 16
#send DAS_status2
if frame % 50 == 0:
collision_warning = 0x00
acc_speed_limit_mph = CS.v_cruise_pcm * CV.KPH_TO_MPH
if hud_alert == AH.FCW:
collision_warning = 0x01
can_sends.append(
teslacan.create_DAS_status2_msg(
CS.DAS_status2_idx, acc_speed_limit_mph,
collision_warning))
CS.DAS_status2_idx += 1
CS.DAS_status2_idx = CS.DAS_status2_idx % 16
#send DAS_bodyControl
if frame % 50 == 0:
can_sends.append(
teslacan.create_DAS_bodyControls_msg(
CS.DAS_bodyControls_idx, turn_signal_needed))
CS.DAS_bodyControls_idx += 1
CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
#send DAS_control
if frame % 4 == 0:
acc_speed_limit_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
accel_min = -15
accel_max = 5
speed_control_enabled = enabled and (acc_speed_limit_kph >
0)
can_sends.append(
teslacan.create_DAS_control(CS.DAS_control_idx,
speed_control_enabled,
acc_speed_limit_kph,
accel_min, accel_max))
CS.DAS_control_idx += 1
CS.DAS_control_idx = CS.DAS_control_idx % 8
#send DAS_lanes
if frame % 10 == 0:
can_sends.append(
teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
CS.DAS_lanes_idx += 1
CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
#send DAS_pscControl
if frame % 4 == 0:
can_sends.append(
teslacan.create_DAS_pscControl_msg(
CS.DAS_pscControl_idx))
CS.DAS_pscControl_idx += 1
CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
#send DAS_telemetryPeriodic
if frame % 4 == 0:
can_sends.append(
teslacan.create_DAS_telemetryPeriodic(
CS.DAS_telemetryPeriodic1_idx,
CS.DAS_telemetryPeriodic2_idx))
CS.DAS_telemetryPeriodic2_idx += 1
CS.DAS_telemetryPeriodic2_idx = CS.DAS_telemetryPeriodic2_idx % 10
if CS.DAS_telemetryPeriodic2_idx == 0:
CS.DAS_telemetryPeriodic1_idx += 2
CS.DAS_telemetryPeriodic1_idx = CS.DAS_telemetryPeriodic1_idx % 16
#send DAS_telemetryEvent
if frame % 10 == 0:
#can_sends.append(teslacan.create_DAS_telemetryEvent(CS.DAS_telemetryEvent1_idx,CS.DAS_telemetryEvent2_idx))
CS.DAS_telemetryEvent2_idx += 1
CS.DAS_telemetryEvent2_idx = CS.DAS_telemetryEvent2_idx % 10
if CS.DAS_telemetryEvent2_idx == 0:
CS.DAS_telemetryEvent1_idx += 2
CS.DAS_telemetryEvent1_idx = CS.DAS_telemetryEvent1_idx % 16
#send DAS_visualDebug
if (frame + 1) % 10 == 0:
can_sends.append(teslacan.create_DAS_visualDebug_msg())
#send DAS_chNm
if (frame + 2) % 10 == 0:
can_sends.append(teslacan.create_DAS_chNm())
#send DAS_objects
if frame % 3 == 0:
can_sends.append(
teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
CS.DAS_objects_idx += 1
CS.DAS_objects_idx = CS.DAS_objects_idx % 16
#send DAS_warningMatrix0
if frame % 6 == 0:
can_sends.append(
teslacan.create_DAS_warningMatrix0(
CS.DAS_warningMatrix0_idx))
CS.DAS_warningMatrix0_idx += 1
CS.DAS_warningMatrix0_idx = CS.DAS_warningMatrix0_idx % 16
#send DAS_warningMatrix3
if (frame + 3) % 6 == 0:
driverResumeRequired = 0
if enabled and not enable_steer_control:
driverResumeRequired = 1
can_sends.append(
teslacan.create_DAS_warningMatrix3(
CS.DAS_warningMatrix3_idx, driverResumeRequired))
CS.DAS_warningMatrix3_idx += 1
CS.DAS_warningMatrix3_idx = CS.DAS_warningMatrix3_idx % 16
#send DAS_warningMatrix1
if frame % 100 == 0:
can_sends.append(
teslacan.create_DAS_warningMatrix1(
CS.DAS_warningMatrix1_idx))
CS.DAS_warningMatrix1_idx += 1
CS.DAS_warningMatrix1_idx = CS.DAS_warningMatrix1_idx % 16
# end of DAS emulation """
idx = frame % 16
can_sends.append(
teslacan.create_steering_control(enable_steer_control,
apply_angle, idx))
can_sends.append(teslacan.create_epb_enable_signal(idx))
cruise_btn = None
if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available:
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
pcm_speed)
#add fake carConfig to trigger IC to display AP
if frame % 2 == 0:
carConfig_msg = teslacan.create_GTW_carConfig_msg(
real_carConfig_data=CS.real_carConfig,
dasHw=1,
autoPilot=1,
fRadarHw=1)
#can_sends.append(carConfig_msg)
if cruise_btn or (turn_signal_needed > 0 and frame % 2 == 0):
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat=0, #turn_signal_needed,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
enabled, CS, frame, actuators, pcm_speed)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed),
accel_idx))
self.last_angle = apply_angle
self.last_accel = apply_accel
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, car_fingerprint):
self.start_time = sec_since_boot()
self.lkas_active = False
self.steer_idx = 0
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
self.es_distance_cnt = -1
self.es_lkas_cnt = -1
self.counter = 0
self.ALCA = ALCAController(self,True,False) # Enabled True and SteerByAngle only False
# Setup detection helper. Routes commands to
# an appropriate CAN bus number.
self.params = CarControllerParams(car_fingerprint)
print(DBC)
self.packer = CANPacker(DBC[car_fingerprint]['pt'])
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'))
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.blindspot_poll_counter = 0
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.last_fault_frame = -200
self.blindspot_debug_enabled_left = False
self.blindspot_debug_enabled_right = False
self.fake_ecus = set()
if enable_camera: self.fake_ecus.add(ECU.CAM)
if enable_dsu: self.fake_ecus.add(ECU.DSU)
if enable_apg: self.fake_ecus.add(ECU.APGS)
self.ALCA = ALCAController(self,True,False) # Enabled True and SteerByAngle only False
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_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)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only send brake values if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# 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)
apply_steer = int(round(alca_steer * STEER_MAX))
# steer torque
#apply_steer = int(round(actuators.steer * STEER_MAX))
max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)
apply_steer = clip(apply_steer, min_lim, max_lim)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
#apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
#if CS.standstill and not self.last_standstill:
# self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# Enable blindspot debug mode once
if BLINDSPOTDEBUG:
self.blindspot_poll_counter += 1
if self.blindspot_poll_counter > 1000: # 10 seconds after start
if CS.left_blinker_on:
self.blindspot_blink_counter_left += 1
#print "debug Left Blinker on"
elif CS.right_blinker_on:
self.blindspot_blink_counter_right += 1
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
#print "debug Left Blinker off"
if self.blindspot_debug_enabled_left:
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
#print "debug Left blindspot debug disabled"
if self.blindspot_debug_enabled_right:
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print "debug Right blindspot debug disabled"
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, True))
#print "debug Left blindspot debug enabled"
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.v_ego > 6: #polling at low speeds switches camera off
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print "debug Right blindspot debug enabled"
self.blindspot_debug_enabled_right = True
if self.blindspot_debug_enabled_left:
if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if self.blindspot_debug_enabled_right:
if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
if CS.lane_departure_toggle_on:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
else:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
# rav4h with dsu disconnected
if self.angle_control:
if CS.lane_departure_toggle_on:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
else:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
if CS.cstm_btns.get_button_status("tr") > 0:
distance = 0b01110011 #x73 comma with toggle - toggle is 5th bit from right
CS.cstm_btns.set_button_status("tr", 0)
else:
distance = 0b01100011 #x63 comma with toggle - toggle is 5th bit from right
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, distance))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, distance))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame/10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2))
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame/100)%0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, enable_camera=True):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.epas_disabled = True
self.last_angle = 0.
self.last_accel = 0.
self.ALCA = ALCAController(self, True,
True) # Enabled and SteerByAngle both True
self.ACC = ACCController()
self.PCC = PCCController(self)
self.HSO = HSOController(self)
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
if not self.enable_camera:
return
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
STEER_MAX = 420
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
# Update statuses for custom buttons 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)
#print CS.cstm_btns.get_button_status("alca")
if CS.pedal_hardware_present:
#update PCC module info
self.PCC.update_stat(CS, True, sendcan)
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
human_control = False
# update CS.v_cruise_pcm based on module selected.
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph
else:
CS.v_cruise_pcm = CS.v_cruise_actual
# Get the angle from ALCA.
alca_enabled = False
turn_signal_needed = 0
alca_steer = 0.
apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_angle = -apply_angle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not alca_enabled
enable_steer_control = (enabled and not human_lane_changing
and not human_control)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# If blinker is on send the actual angle.
#if (changing_lanes and (CS.laneChange_enabled < 2)):
# apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
send_step = 5
if (True):
"""#First we emulate DAS.
if (CS.DAS_info_frm == -1):
#initialize all frames
CS.DAS_info_frm = frame # 1.00 s interval
CS.DAS_status_frm = (frame + 10) % 100 # 0.50 s interval
CS.DAS_status2_frm = (frame + 35) % 100 # 0.50 s interval in between DAS_status
CS.DAS_bodyControls_frm = (frame + 40) % 100 # 0.50 s interval
CS.DAS_lanes_frm = (frame + 5) % 100 # 0.10 s interval
CS.DAS_objects_frm = (frame + 2) % 100 # 0.03 s interval
CS.DAS_pscControl_frm = (frame + 3) % 100 # 0.04 s interval
if (CS.DAS_info_frm == frame):
can_sends.append(teslacan.create_DAS_info_msg(CS.DAS_info_msg))
CS.DAS_info_msg += 1
CS.DAS_info_msg = CS.DAS_info_msg % 10
if (CS.DAS_status_frm == frame):
can_sends.append(teslacan.create_DAS_status_msg(CS.DAS_status_idx))
CS.DAS_status_idx += 1
CS.DAS_status_idx = CS.DAS_status_idx % 16
CS.DAS_status_frm = (CS.DAS_status_frm + 50) % 100
if (CS.DAS_status2_frm == frame):
can_sends.append(teslacan.create_DAS_status2_msg(CS.DAS_status2_idx))
CS.DAS_status2_idx += 1
CS.DAS_status2_idx = CS.DAS_status2_idx % 16
CS.DAS_status2_frm = (CS.DAS_status2_frm + 50) % 100
if (CS.DAS_bodyControls_frm == frame):
can_sends.append(teslacan.create_DAS_bodyControls_msg(CS.DAS_bodyControls_idx))
CS.DAS_bodyControls_idx += 1
CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
CS.DAS_bodyControls_frm = (CS.DAS_bodyControls_frm + 50) % 100
if (CS.DAS_lanes_frm == frame):
can_sends.append(teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
CS.DAS_lanes_idx += 1
CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
CS.DAS_lanes_frm = (CS.DAS_lanes_frm + 10) % 100
if (CS.DAS_pscControl_frm == frame):
can_sends.append(teslacan.create_DAS_pscControl_msg(CS.DAS_pscControl_idx))
CS.DAS_pscControl_idx += 1
CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
CS.DAS_pscControl_frm = (CS.DAS_pscControl_frm + 4) % 100
if (CS.DAS_objects_frm == frame):
can_sends.append(teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
CS.DAS_objects_idx += 1
CS.DAS_objects_idx = CS.DAS_objects_idx % 16
CS.DAS_objects_frm = (CS.DAS_objects_frm + 3) % 100
# end of DAS emulation """
idx = frame % 16
can_sends.append(
teslacan.create_steering_control(enable_steer_control,
apply_angle, idx))
can_sends.append(teslacan.create_epb_enable_signal(idx))
cruise_btn = None
if self.ACC.enable_adaptive_cruise and not self.PCC.pedal_hardware_present:
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
pcm_speed)
if (cruise_btn != None) or ((turn_signal_needed > 0) and
(frame % 2 == 0)):
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat=turn_signal_needed,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if self.PCC.pedal_hardware_present: # and (frame % 10) == 0:
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
enabled, CS, frame, actuators, pcm_speed)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed),
accel_idx))
self.last_angle = apply_angle
self.last_accel = apply_accel
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
enable_apg):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.angle_send = 0
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.last_fault_frame = -200
self.fake_ecus = set()
if enable_camera: self.fake_ecus.add(ECU.CAM)
if enable_dsu: self.fake_ecus.add(ECU.DSU)
if enable_apg: self.fake_ecus.add(ECU.APGS)
self.ALCA = ALCAController(self, True,
True) # Enabled True and SteerByAngle True
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_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)
# *** compute control surfaces ***
#Leave this here, will someday use accel...
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque - leave minimum for steer torque incase it's needed
apply_steer = int(round(actuators.steer * STEER_MAX))
# steer angle - Currently using steer angle
if enabled:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
# 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)
apply_steer = int(round(alca_steer * STEER_MAX))
apply_angle = alca_angle
#Disable if not enabled
if not enabled:
apply_angle = 0
apply_steer_req = 0
else:
apply_steer_req = 1
#Multply by 100 to allow 2 decmals sent over CAN. Arduino will divde by 100.
angle_send = apply_angle * 100
#update desired angle for safety loop
CS.desired_angle = apply_angle
#Reset enabled time if blinker pressed to not diable during lane change
if CS.left_blinker_on or CS.right_blinker_on or CS.brake_pressed:
CS.enabled_time = (
sec_since_boot() * 1e3
) #reset time to not trigger safety after releaaed
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#Always send CAN steer message
can_sends.append(
create_steer_command(self.packer, angle_send, apply_steer_req,
frame))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd,
False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, enable_camera=True):
self.params = Params()
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.epas_disabled = True
self.last_angle = 0.
self.last_accel = 0.
self.ALCA = ALCAController(self,True,True) # Enabled and SteerByAngle both True
self.ACC = ACCController()
self.PCC = PCCController(self)
self.HSO = HSOController(self)
self.GYRO = GYROController()
self.sent_DAS_bootID = False
context = zmq.Context()
self.poller = zmq.Poller()
self.speedlimit = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller)
self.speedlimit_ms = 0
self.speedlimit_valid = False
self.speedlimit_units = 0
self.opState = 0 # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
self.accPitch = 0.
self.accRoll = 0.
self.accYaw = 0.
self.magPitch = 0.
self.magRoll = 0.
self.magYaw = 0.
self.gyroPitch = 0.
self.gyroRoll = 0.
self.gyroYaw = 0.
self.set_speed_limit_active = False
self.speed_limit_offset = 0.
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
if not self.enable_camera:
return
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
STEER_MAX = 420
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
#read speed limit params
self.set_speed_limit_active = self.params.get("SpeedLimitOffset") is not None #self.params.get("LimitSetSpeed") == "1" and
if self.set_speed_limit_active:
self.speed_limit_offset = float(self.params.get("SpeedLimitOffset"))
else:
self.speed_limit_offset = 0.
#get pitch/roll/yaw every 0.1 sec
if (frame %10 == 0):
(self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers)
CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw)
# Update statuses for custom buttons 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 and CS.enableALCA)
#print CS.cstm_btns.get_button_status("alca")
if CS.pedal_interceptor_available:
#update PCC module info
self.PCC.update_stat(CS, True, sendcan)
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
human_control = False
# update CS.v_cruise_pcm based on module selected.
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph
else:
CS.v_cruise_pcm = CS.v_cruise_actual
# Get the angle from ALCA.
alca_enabled = False
turn_signal_needed = 0
alca_steer = 0.
apply_angle, alca_steer,alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
apply_angle = -apply_angle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not alca_enabled
enable_steer_control = (enabled
and not human_lane_changing
and not human_control
and vehicle_moving)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR )
if alca_enabled or not CS.enableSpeedVariableDesAngle:
des_angle_factor = 1.
#BB disable limits to test 0.5.8
# apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
#First we emulate DAS.
# DAS_longC_enabled (1),DAS_gas_to_resume (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4)
# DAS_speed_kph(8),
# DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (4),
# DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
# DAS_acc_speed_limit_mph (8),
# DAS_speed_limit_units(8)
#send fake_das data as 0x553
# TODO: forward collission warning
if frame % 10 == 0:
#get speed limit
for socket, _ in self.poller.poll(0):
if socket is self.speedlimit:
lmd = messaging.recv_one(socket).liveMapData
self.speedlimit_ms = lmd.speedLimit
self.speedlimit_valid = lmd.speedLimitValid
if (self.params.get("IsMetric") == "1"):
self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_KPH + 0.5
else:
self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_MPH + 0.5
op_status = 0x02
hands_on_state = 0x00
forward_collision_warning = 0 #1 if needed
if hud_alert == AH.FCW:
forward_collision_warning = hud_alert[1]
if forward_collision_warning > 1:
forward_collision_warning = 1
#cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
cc_state = 1
speed_limit_to_car = int(self.speedlimit_units)
alca_state = 0x00
apUnavailable = 0
gas_to_resume = 0
collision_warning = 0x00
acc_speed_limit_mph = 0
speed_control_enabled = 0
accel_min = -15
accel_max = 5
acc_speed_kph = 0
if enabled:
#self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
if self.opState == 0:
op_status = 0x02
if self.opState == 1:
op_status = 0x03
if self.opState == 2:
op_status = 0x08
if self.opState == 3:
op_status = 0x01
if self.opState == 5:
op_status = 0x03
alca_state = 0x08 + turn_signal_needed
#canceled by user
if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0):
alca_state = 0x14
#min speed for ALCA
if CS.CL_MIN_V > CS.v_ego:
alca_state = 0x05
if not enable_steer_control:
#op_status = 0x08
hands_on_state = 0x02
apUnavailable = 1
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1)
if CS.pedal_interceptor_available:
acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1)
if hud_alert == AH.FCW:
collision_warning = hud_alert[1]
if collision_warning > 1:
collision_warning = 1
#use disabling for alerts/errors to make them aware someting is goin going on
if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW):
op_status = 0x08
if self.ACC.enable_adaptive_cruise:
acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
if (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise):
speed_control_enabled = 1
cc_state = 2
else:
if (CS.pcm_acc_status == 4):
#car CC enabled but not OP, display the HOLD message
cc_state = 3
send_fake_msg = False
send_fake_warning = False
if enabled:
if frame % 2 == 0:
send_fake_msg = True
if frame % 25 == 0:
send_fake_warning = True
else:
if frame % 23 == 0:
send_fake_msg = True
if frame % 60 == 0:
send_fake_warning = True
if send_fake_msg:
can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,gas_to_resume,apUnavailable, collision_warning, op_status, \
acc_speed_kph, \
turn_signal_needed,forward_collision_warning,hands_on_state, \
cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \
acc_speed_limit_mph,
speed_limit_to_car,
apply_angle,
1 if enable_steer_control else 0))
if send_fake_warning or (self.opState == 2) or (self.opState == 5):
#if it's time to send OR we have a warning or emergency disable
can_sends.append(teslacan.create_fake_DAS_warning(CS.DAS_noSeatbelt, CS.DAS_canErrors, \
CS.DAS_plannerErrors, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation))
# end of DAS emulation """
idx = frame % 16
cruise_btn = None
if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available:
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
self.speedlimit_ms * CV.MS_TO_KPH,self.speedlimit_valid, \
self.set_speed_limit_active, self.speed_limit_offset)
if cruise_btn:
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat= 0, #turn_signal_needed,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
self.speedlimit_ms * CV.MS_TO_KPH, self.speedlimit_valid, \
self.set_speed_limit_active, self.speed_limit_offset)
can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx))
self.last_angle = apply_angle
self.last_accel = apply_accel
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController():
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
enable_apg):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.last_fault_frame = -200
self.fake_ecus = set()
if enable_camera: self.fake_ecus.add(ECU.CAM)
if enable_dsu: self.fake_ecus.add(ECU.DSU)
if enable_apg: self.fake_ecus.add(ECU.APGS)
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# 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)
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus, False))
return can_sends
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.apply_steer_last = 0
self.ccframe = 0
self.prev_frame = -1
self.hud_count = 0
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.send_chime = False
self.gone_fast_yet = False
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
self.fake_ecus = set()
if enable_camera:
self.fake_ecus.add(ECU.CAM)
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_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)
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = alca_steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_motor,
SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_lkas_heartbit(self.packer, CS.lkas_status_ok)
can_sends.append(new_msg)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
enable_apg):
self.braking = False
# redundant safety check with the board
self.controls_allowed = True
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.car_fingerprint = car_fingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.fake_ecus = set()
if enable_camera: self.fake_ecus.add(ECU.CAM)
if enable_dsu: self.fake_ecus.add(ECU.DSU)
if enable_apg: self.fake_ecus.add(ECU.APGS)
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_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)
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# 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)
apply_steer = int(round(alca_steer * STEER_MAX))
max_lim = min(
max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX),
STEER_MAX)
min_lim = max(
min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX),
-STEER_MAX)
apply_steer = clip(apply_steer, min_lim, max_lim)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(
apply_steer,
max(self.last_steer - STEER_DELTA_DOWN, -STEER_DELTA_UP),
self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.last_steer - STEER_DELTA_UP,
min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
# only cut torque when steer state is a known fault
if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd,
False))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2))
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())