def update_stat(self, CS, enabled, sendcan):
if not self.LoC:
self.LoC = LongControl(CS.CP, tesla_compute_gb)
can_sends = []
if CS.pedal_interceptor_available and not CS.cstm_btns.get_button_status(
"pedal"):
# pedal hardware, enable button
CS.cstm_btns.set_button_status("pedal", 1)
print "enabling pedal"
elif not CS.pedal_interceptor_available:
if CS.cstm_btns.get_button_status("pedal"):
# no pedal hardware, disable button
CS.cstm_btns.set_button_status("pedal", 0)
print "disabling pedal"
print "Pedal unavailable."
return
# check if we had error before
if self.pedal_interceptor_state != CS.pedal_interceptor_state:
self.pedal_interceptor_state = CS.pedal_interceptor_state
CS.cstm_btns.set_button_status(
"pedal", 1 if self.pedal_interceptor_state > 0 else 0)
if self.pedal_interceptor_state > 0:
# send reset command
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx))
sendcan.send(
can_list_to_can_capnp(can_sends,
msgtype='sendcan').to_bytes())
CS.UE.custom_alert_message(
3, "Pedal Interceptor Off (state %s)" %
self.pedal_interceptor_state, 150, 3)
else:
CS.UE.custom_alert_message(3, "Pedal Interceptor On", 150, 3)
# disable on brake
if CS.brake_pressed and self.enable_pedal_cruise:
self.enable_pedal_cruise = False
self.reset(0.)
CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", 1)
print "brake pressed"
prev_enable_pedal_cruise = self.enable_pedal_cruise
# process any stalk movement
curr_time_ms = _current_time_millis()
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.MPH_TO_KPH
if (CS.cruise_buttons == CruiseButtons.MAIN
and self.prev_cruise_buttons != CruiseButtons.MAIN):
double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750
self.last_cruise_stalk_pull_time = curr_time_ms
ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF
and enabled and CruiseState.is_off(CS.pcm_acc_status))
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_pedal_cruise = True
self.LoC.reset(CS.v_ego)
# Increase PCC speed to match current, if applicable.
self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH,
self.pedal_speed_kph)
else:
# A single pull disables PCC (falling back to just steering).
self.enable_pedal_cruise = False
# Handle pressing the cancel button.
elif CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_pedal_cruise = False
self.pedal_speed_kph = 0.
self.last_cruise_stalk_pull_time = 0
# Handle pressing up and down buttons.
elif (self.enable_pedal_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
# Real stalk command while PCC is already enabled. Adjust the max PCC
# speed if necessary.
actual_speed_kph = CS.v_ego * CV.MS_TO_KPH
if CS.cruise_buttons == CruiseButtons.RES_ACCEL:
self.pedal_speed_kph = max(self.pedal_speed_kph,
actual_speed_kph) + speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND:
self.pedal_speed_kph = max(
self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_SET:
self.pedal_speed_kph = min(self.pedal_speed_kph,
actual_speed_kph) - speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_2ND:
self.pedal_speed_kph = min(
self.pedal_speed_kph, actual_speed_kph) - 5 * speed_uom_kph
# Clip PCC speed between 0 and 170 KPH.
self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH,
MAX_PCC_V_KPH)
# If something disabled cruise control, disable PCC too
elif self.enable_pedal_cruise and CS.pcm_acc_status:
self.enable_pedal_cruise = False
# Notify if PCC was toggled
if prev_enable_pedal_cruise and not self.enable_pedal_cruise:
CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
elif self.enable_pedal_cruise and not prev_enable_pedal_cruise:
CS.UE.custom_alert_message(2, "PCC Enabled", 150)
CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED)
# Update the UI to show whether the current car state allows PCC.
if CS.cstm_btns.get_button_status("pedal") in [
PCCState.STANDBY, PCCState.NOT_READY
]:
if enabled and CruiseState.is_off(CS.pcm_acc_status):
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
else:
CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY)
# Update prev state after all other actions.
self.prev_cruise_buttons = CS.cruise_buttons
self.prev_pcm_acc_status = CS.pcm_acc_status
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())
def update(self, 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,leftLaneVisible,rightLaneVisible):
if (not enabled) and (self.ALCA.laneChange_cancelled):
self.ALCA.laneChange_cancelled = False
self.ALCA.laneChange_cancelled_counter = 0
self.warningNeeded = 1
if self.warningCounter > 0:
self.warningCounter = self.warningCounter - 1
if self.warningCounter == 0:
self.warningNeeded = 1
if self.warningCounter == 0 or not enabled:
# when zero reset all warnings
self.DAS_222_accCameraBlind = 0 #we will see what we can use this for
self.DAS_219_lcTempUnavailableSpeed = 0
self.DAS_220_lcTempUnavailableRoad = 0
self.DAS_221_lcAborting = 0
self.DAS_211_accNoSeatBelt = 0
self.DAS_207_lkasUnavailable = 0 #use for manual not in drive?
self.DAS_208_rackDetected = 0 #use for low battery?
self.DAS_202_noisyEnvironment = 0 #use for planner error?
self.DAS_025_steeringOverride = 0 #use for manual steer?
self.DAS_206_apUnavailable = 0 #Ap disabled from CID
if CS.keepEonOff:
if CS.cstm_btns.get_button_status("dsp") != 9:
CS.cstm_btns.set_button_status("dsp", 9)
else:
if CS.cstm_btns.get_button_status("dsp") != 1:
CS.cstm_btns.set_button_status("dsp", 1)
# """ Controls thread """
if not CS.useTeslaMapData:
if self.speedlimit is None:
self.speedlimit = messaging.sub_sock('liveMapData',
conflate=True)
# *** 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 ***
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 100 == 0):
CS.cstm_btns.send_button_info()
#read speed limit params
if CS.hasTeslaIcIntegration:
self.set_speed_limit_active = True
self.speed_limit_offset = CS.userSpeedLimitOffsetKph
else:
self.set_speed_limit_active = (
self.params.get("SpeedLimitOffset")
is not None) and (self.params.get("LimitSetSpeed") == "1")
if self.set_speed_limit_active:
self.speed_limit_offset = float(
self.params.get("SpeedLimitOffset"))
if not self.isMetric:
self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
else:
self.speed_limit_offset = 0.
if CS.useTeslaGPS and (frame % 10 == 0):
if self.gpsLocationExternal is None:
self.gpsLocationExternal = messaging.pub_sock(
'gpsLocationExternal')
sol = gen_solution(CS)
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
self.gpsLocationExternal.send(sol.to_bytes())
#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 (frame % 10 == 0):
self.ALCA.update_status(
(CS.cstm_btns.get_button_status("alca") > 0)
and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or
(CS.hasTeslaIcIntegration and CS.alcaEnabled)))
self.blinker.update_state(CS, frame)
# update PCC module info
pedal_can_sends = self.PCC.update_stat(CS, frame)
if self.PCC.pcc_available:
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# update CS.v_cruise_pcm based on module selected.
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.KPH_TO_MPH
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph
else:
CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph
self.alca_enabled = self.ALCA.update(enabled, CS, actuators,
self.alcaStateData, frame,
self.blinker)
self.should_ldw = self._should_ldw(CS, frame)
apply_angle = -actuators.steerAngle # Tesla is reversed vs OP.
# Update HSO module info.
human_control = self.HSO.update_stat(self, CS, enabled, actuators,
frame)
human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.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)
#BB disable limits to test 0.5.8
# 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
# Send CAN commands.
can_sends = []
#if using radar, we need to send the VIN
if CS.useTeslaRadar and (frame % 100 == 0):
useRadar = 0
if CS.useTeslaRadar:
useRadar = 1
can_sends.append(
teslacan.create_radar_VIN_msg(self.radarVin_idx, CS.radarVIN,
1, 0x108, useRadar,
CS.radarPosition,
CS.radarEpasType))
self.radarVin_idx += 1
self.radarVin_idx = self.radarVin_idx % 3
#First we emulate DAS.
# DAS_longC_enabled (1),DAS_speed_override (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 (3),
# DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
# DAS_acc_speed_limit (8),
# DAS_speed_limit_units(8)
#send fake_das data as 0x553
# TODO: forward collision warning
if frame % 10 == 0:
speedlimitMsg = None
if self.speedlimit is not None:
speedlimitMsg = messaging.recv_one_or_none(self.speedlimit)
icLeadsMsg = self.icLeads.receive(non_blocking=True)
radarStateMsg = messaging.recv_one_or_none(self.radarState)
alcaStateMsg = self.alcaState.receive(non_blocking=True)
pathPlanMsg = messaging.recv_one_or_none(self.pathPlan)
icCarLRMsg = self.icCarLR.receive(non_blocking=True)
trafficeventsMsgs = None
if self.trafficevents is not None:
trafficeventsMsgs = messaging.recv_sock(self.trafficevents)
if CS.hasTeslaIcIntegration:
self.speed_limit_ms = CS.speed_limit_ms
if (speedlimitMsg is not None) and not CS.useTeslaMapData:
lmd = speedlimitMsg.liveMapData
self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0
if icLeadsMsg is not None:
self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg)
if radarStateMsg is not None:
#to show lead car on IC
if self.icLeadsData is not None:
can_messages = self.showLeadCarOnICCanMessage(
radarStateMsg=radarStateMsg)
can_sends.extend(can_messages)
if alcaStateMsg is not None:
self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg)
if pathPlanMsg is not None:
#to show curvature and lanes on IC
if self.alcaStateData is not None:
self.handlePathPlanSocketForCurvatureOnIC(
pathPlanMsg=pathPlanMsg,
alcaStateData=self.alcaStateData,
CS=CS)
if icCarLRMsg is not None:
can_messages = self.showLeftAndRightCarsOnICCanMessages(
icCarLRMsg=tesla.ICCarsLR.from_bytes(icCarLRMsg))
can_sends.extend(can_messages)
if trafficeventsMsgs is not None:
can_messages = self.handleTrafficEvents(
trafficEventsMsgs=trafficeventsMsgs)
can_sends.extend(can_messages)
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
alca_state = 0x00
speed_override = 0
collision_warning = 0x00
speed_control_enabled = 0
accel_min = -15
accel_max = 5
acc_speed_kph = 0
send_fake_warning = False
send_fake_msg = False
if enabled:
#self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
alca_state = 0x01
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
if self.blinker.override_direction > 0:
alca_state = 0x08 + self.blinker.override_direction
elif (self.lLine > 1) and (self.rLine > 1):
alca_state = 0x08
elif (self.lLine > 1):
alca_state = 0x06
elif (self.rLine > 1):
alca_state = 0x07
else:
alca_state = 0x01
#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
#max angle for ALCA
if (abs(actuators.steerAngle) >= CS.CL_MAX_A):
alca_state = 0x15
if not enable_steer_control:
#op_status = 0x08
hands_on_state = 0x02
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
if self.PCC.pcc_available:
acc_speed_kph = self.PCC.pedal_speed_kph
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 (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (
self.ACC.enable_adaptive_cruise):
speed_control_enabled = 1
cc_state = 2
if not self.ACC.adaptive:
cc_state = 3
CS.speed_control_enabled = 1
else:
CS.speed_control_enabled = 0
if (CS.pcm_acc_status == 4):
#car CC enabled but not OP, display the HOLD message
cc_state = 3
else:
if (CS.pcm_acc_status == 4):
cc_state = 3
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 frame % 10 == 0:
can_sends.append(
teslacan.create_fake_DAS_obj_lane_msg(
self.leadDx, self.leadDy, self.leadClass, self.rLine,
self.lLine, self.curv0, self.curv1, self.curv2, self.curv3,
self.laneRange, self.laneWidth))
speed_override = 0
if (CS.pedal_interceptor_value > 10) and (cc_state > 1):
speed_override = 0 #force zero for now
if (not enable_steer_control) and op_status == 3:
#hands_on_state = 0x03
self.DAS_219_lcTempUnavailableSpeed = 1
self.warningCounter = 100
self.warningNeeded = 1
if enabled and self.ALCA.laneChange_cancelled and (
not CS.steer_override) and (
CS.turn_signal_stalk_state
== 0) and (self.ALCA.laneChange_cancelled_counter > 0):
self.DAS_221_lcAborting = 1
self.warningCounter = 300
self.warningNeeded = 1
if CS.hasTeslaIcIntegration:
highLowBeamStatus, highLowBeamReason, ahbIsEnabled = self.AHB.update(
CS, frame, self.ahbLead1)
if frame % 5 == 0:
self.cc_counter = (
self.cc_counter +
1) % 40 #use this to change status once a second
self.fleet_speed_state = 0x00 #fleet speed unavailable
if FleetSpeed.is_available(CS):
if self.ACC.fleet_speed.is_active(
frame) or self.PCC.fleet_speed.is_active(frame):
self.fleet_speed_state = 0x02 #fleet speed enabled
else:
self.fleet_speed_state = 0x01 #fleet speed available
can_sends.append(
teslacan.create_fake_DAS_msg2(highLowBeamStatus,
highLowBeamReason,
ahbIsEnabled,
self.fleet_speed_state))
if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02):
CS.v_cruise_pcm = CS.v_cruise_pcm + 1
send_fake_msg = True
if (self.cc_counter == 3):
send_fake_msg = True
if send_fake_msg:
if enable_steer_control and op_status == 3:
op_status = 0x5
park_brake_request = 0 #experimental; disabled for now
if park_brake_request == 1:
print("Park Brake Request received")
adaptive_cruise = 1 if (
not self.PCC.pcc_available
and self.ACC.adaptive) or self.PCC.pcc_available else 0
can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
acc_speed_kph, \
self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \
cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \
CS.v_cruise_pcm,
CS.DAS_fusedSpeedLimit,
apply_angle,
1 if enable_steer_control else 0,
park_brake_request))
if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (
self.stopSignWarning != self.stopSignWarning_last) or (
self.stopLightWarning != self.stopLightWarning_last) or (
self.warningNeeded == 1) or (frame % 100 == 0):
#if it's time to send OR we have a warning or emergency disable
can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \
self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \
self.stopSignWarning, self.stopLightWarning, \
self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \
self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,CS.useWithoutHarness,CS.usesApillarHarness))
self.stopLightWarning_last = self.stopLightWarning
self.stopSignWarning_last = self.stopSignWarning
self.warningNeeded = 0
# end of DAS emulation """
if frame % 100 == 0: # and CS.hasTeslaIcIntegration:
#IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake
can_sends.append(teslacan.create_fake_IC_msg())
# send enabled ethernet every 0.2 sec
if frame % 20 == 0:
can_sends.append(teslacan.create_enabled_eth_msg(1))
if (not self.PCC.pcc_available
) and frame % 5 == 0: # acc processed at 20Hz
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
self.speed_limit_ms * CV.MS_TO_KPH,
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,
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.pcc_available and frame % 5 == 0: # pedal processed at 20Hz
pedalcan = 2
if CS.useWithoutHarness:
pedalcan = 0
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
self.speed_limit_ms,
self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed), accel_idx,
pedalcan))
self.last_angle = apply_angle
self.last_accel = apply_accel
return pedal_can_sends + can_sends
def update_stat(self, CS, frame):
if not self.LoC:
self.LoC = LongControl(CS.CP, tesla_compute_gb)
# Get v_id from the stored file when initiating the LoC and reset_on_disengage==false
if (not RESET_PID_ON_DISENGAGE):
self.load_pid()
self._update_pedal_state(CS, frame)
can_sends = []
if not self.pcc_available:
timed_out = frame >= self.pedal_timeout_frame
if timed_out or CS.pedal_interceptor_state > 0:
if self.prev_pcc_available:
CS.UE.custom_alert_message(
4, "Pedal Interceptor %s" %
("timed out" if timed_out else "fault (state %s)" %
CS.pedal_interceptor_state), 200, 4)
if frame % 50 == 0:
# send reset command
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
can_sends.append(
teslacan.create_pedal_command_msg(0, 0, idx))
return can_sends
prev_enable_pedal_cruise = self.enable_pedal_cruise
# disable on brake
if CS.brake_pressed and self.enable_pedal_cruise:
self.enable_pedal_cruise = False
self.reset(0.)
# process any stalk movement
curr_time_ms = _current_time_millis()
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.MPH_TO_KPH
if (CS.cruise_buttons == CruiseButtons.MAIN
and self.prev_cruise_buttons != CruiseButtons.MAIN):
self.prev_stalk_pull_time_ms = self.stalk_pull_time_ms
self.stalk_pull_time_ms = curr_time_ms
double_pull = self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms < STALK_DOUBLE_PULL_MS
ready = (CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) >
PCCState.OFF and (CruiseState.is_off(CS.pcm_acc_status))
or CS.forcePedalOverCC)
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_pedal_cruise = True
self.reset(CS.v_ego)
# Increase PCC speed to match current, if applicable.
# We round the target speed in the user's units of measurement to avoid jumpy speed readings
current_speed_kph_uom_rounded = int(CS.v_ego * CV.MS_TO_KPH /
speed_uom_kph +
0.5) * speed_uom_kph
self.pedal_speed_kph = max(current_speed_kph_uom_rounded,
self.speed_limit_kph)
# Handle pressing the cancel button.
elif CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_pedal_cruise = False
self.pedal_speed_kph = 0.
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
# Handle pressing up and down buttons.
elif (self.enable_pedal_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
# Real stalk command while PCC is already enabled. Adjust the max PCC speed if necessary.
# We round the target speed in the user's units of measurement to avoid jumpy speed readings
actual_speed_kph_uom_rounded = int(
CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph
if CS.cruise_buttons == CruiseButtons.RES_ACCEL:
self.pedal_speed_kph = max(
self.pedal_speed_kph,
actual_speed_kph_uom_rounded) + speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND:
self.pedal_speed_kph = max(
self.pedal_speed_kph,
actual_speed_kph_uom_rounded) + 5 * speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_SET:
self.pedal_speed_kph = self.pedal_speed_kph - speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_2ND:
self.pedal_speed_kph = self.pedal_speed_kph - 5 * speed_uom_kph
# Clip PCC speed between 0 and 170 KPH.
self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH,
MAX_PCC_V_KPH)
# If something disabled cruise control, disable PCC too
elif self.enable_pedal_cruise and CS.pcm_acc_status and not CS.forcePedalOverCC:
self.enable_pedal_cruise = False
# A single pull disables PCC (falling back to just steering). Wait some time
# in case a double pull comes along.
elif (self.enable_pedal_cruise
and curr_time_ms - self.stalk_pull_time_ms > STALK_DOUBLE_PULL_MS
and self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms >
STALK_DOUBLE_PULL_MS):
self.enable_pedal_cruise = False
# Notify if PCC was toggled
if prev_enable_pedal_cruise and not self.enable_pedal_cruise:
CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4)
CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME,
PCCState.STANDBY)
self.fleet_speed.reset_averager()
#save the pid parameters to params file
self.save_pid(self.LoC.pid)
elif self.enable_pedal_cruise and not prev_enable_pedal_cruise:
CS.UE.custom_alert_message(2, "PCC Enabled", 150)
CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME,
PCCState.ENABLED)
# Update the UI to show whether the current car state allows PCC.
if CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) in [
PCCState.STANDBY, PCCState.NOT_READY
]:
if CruiseState.is_off(CS.pcm_acc_status) or CS.forcePedalOverCC:
CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME,
PCCState.STANDBY)
else:
CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME,
PCCState.NOT_READY)
# Update prev state after all other actions.
self.prev_cruise_buttons = CS.cruise_buttons
self.prev_pcm_acc_status = CS.pcm_acc_status
return can_sends
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())
def update_stat(self, CS, enabled, sendcan):
if self.pid == None:
CP = CS.CP
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
(CP.longitudinalKiBP, CP.longitudinalKiV),
rate=100.0,
sat_limit=0.8) #,convert=compute_gb)
self.reset(0.)
can_sends = []
#BBTODO: a better way to engage the pedal early and reset its CAN
# on first brake press check if hardware present; being on CAN2 values are not included in fingerprinting
self.pedal_hardware_present = CS.pedal_hardware_present
if not self.pedal_hardware_present:
if (CS.cstm_btns.get_button_status("pedal") > 0):
#no pedal hardware, disable button
CS.cstm_btns.set_button_status("pedal", 0)
print "disabling pedal"
print "no pedal hardware"
return
if self.pedal_hardware_present:
if (CS.cstm_btns.get_button_status("pedal") == 0):
#pedal hardware, enable button
CS.cstm_btns.set_button_status("pedal", 1)
print "enabling pedal"
# check if we had error before
if self.user_pedal_state != CS.user_pedal_state:
self.user_pedal_state = CS.user_pedal_state
CS.cstm_btns.set_button_status(
"pedal", 1 if self.user_pedal_state > 0 else 0)
if self.user_pedal_state > 0:
CS.UE.custom_alert_message(
3, "Pedal Interceptor Error (" +
` self.user_pedal_state ` + ")", 150, 4)
# send reset command
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx))
sendcan.send(
can_list_to_can_capnp(can_sends,
msgtype='sendcan').to_bytes())
# disable on brake
if CS.brake_pressed and self.enable_pedal_cruise:
self.enable_pedal_cruise = False
self.reset(0.)
CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", 1)
print "brake pressed"
prev_enable_pedal_cruise = self.enable_pedal_cruise
# process any stalk movement
curr_time_ms = _current_time_millis()
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.MPH_TO_KPH
if (CS.cruise_buttons == CruiseButtons.MAIN
and self.prev_cruise_buttons != CruiseButtons.MAIN):
double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750
self.last_cruise_stalk_pull_time = curr_time_ms
ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF
and enabled and CruiseState.is_off(CS.pcm_acc_status))
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_pedal_cruise = True
# Increase ACC speed to match current, if applicable.
self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH,
self.pedal_speed_kph)
else:
# A single pull disables ACC (falling back to just steering).
self.enable_pedal_cruise = False
# Handle pressing the cancel button.
elif CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_pedal_cruise = False
self.pedal_speed_kph = 0.
self.last_cruise_stalk_pull_time = 0
# Handle pressing up and down buttons.
elif (self.enable_pedal_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
# Real stalk command while ACC is already enabled. Adjust the max ACC
# speed if necessary. For example if max speed is 50 but you're currently
# only going 30, the cruise speed can be increased without any change to
# max ACC speed. If actual speed is already 50, the code also increases
# the max cruise speed.
if CS.cruise_buttons == CruiseButtons.RES_ACCEL:
requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph
self.pedal_speed_kph = max(self.pedal_speed_kph,
requested_speed_kph)
elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND:
requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph
self.pedal_speed_kph = max(self.pedal_speed_kph,
requested_speed_kph)
elif CS.cruise_buttons == CruiseButtons.DECEL_SET:
self.pedal_speed_kph -= speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_2ND:
self.pedal_speed_kph -= 5 * speed_uom_kph
# Clip ACC speed between 0 and 170 KPH.
self.pedal_speed_kph = min(self.pedal_speed_kph, 170)
self.pedal_speed_kph = max(self.pedal_speed_kph, 1)
# If something disabled cruise control, disable PCC too
elif (self.enable_pedal_cruise == True and CS.pcm_acc_status != 0):
self.enable_pedal_cruise = False
# Notify if PCC was toggled
if prev_enable_pedal_cruise and not self.enable_pedal_cruise:
CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
elif self.enable_pedal_cruise and not prev_enable_pedal_cruise:
CS.UE.custom_alert_message(2, "PCC Enabled", 150)
CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED)
# Update the UI to show whether the current car state allows PCC.
if CS.cstm_btns.get_button_status("pedal") in [
PCCState.STANDBY, PCCState.NOT_READY
]:
if (enabled and CruiseState.is_off(CS.pcm_acc_status)):
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
else:
CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY)
# Update prev state after all other actions.
self.prev_cruise_buttons = CS.cruise_buttons
self.prev_pcm_acc_status = CS.pcm_acc_status
def update(self, 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,leftLaneVisible,rightLaneVisible):
if (not enabled) and (self.ALCA.laneChange_cancelled):
self.ALCA.laneChange_cancelled = False
self.ALCA.laneChange_cancelled_counter = 0
self.warningNeeded = 1
if self.warningCounter > 0:
self.warningCounter = self.warningCounter - 1
if self.warningCounter == 0:
self.warningNeeded = 1
if self.warningCounter == 0 or not enabled:
# when zero reset all warnings
self.DAS_222_accCameraBlind = 0 #we will see what we can use this for
self.DAS_219_lcTempUnavailableSpeed = 0
self.DAS_220_lcTempUnavailableRoad = 0
self.DAS_221_lcAborting = 0
self.DAS_211_accNoSeatBelt = 0
self.DAS_207_lkasUnavailable = 0 #use for manual not in drive?
self.DAS_208_rackDetected = 0 #use for low battery?
self.DAS_202_noisyEnvironment = 0 #use for planner error?
self.DAS_025_steeringOverride = 0 #use for manual steer?
self.DAS_206_apUnavailable = 0 #Ap disabled from CID
if CS.keepEonOff:
if CS.cstm_btns.get_button_status("dsp") != 9:
CS.cstm_btns.set_button_status("dsp",9)
else:
if CS.cstm_btns.get_button_status("dsp") != 1:
CS.cstm_btns.set_button_status("dsp",1)
# """ Controls thread """
if not CS.useTeslaMapData:
if self.speedlimit is None:
self.speedlimit = messaging.sub_sock(service_list['liveMapData'].port, conflate=True, poller=self.poller)
# *** 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 % 100 == 0):
CS.cstm_btns.send_button_info()
#read speed limit params
if CS.hasTeslaIcIntegration:
self.set_speed_limit_active = True
self.speed_limit_offset = CS.userSpeedLimitOffsetKph
self.speed_limit_for_cc = CS.userSpeedLimitKph
#print self.speed_limit_for_cc
else:
self.set_speed_limit_active = (self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1")
if self.set_speed_limit_active:
self.speed_limit_offset = float(self.params.get("SpeedLimitOffset"))
else:
self.speed_limit_offset = 0.
if not self.isMetric:
self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
if CS.useTeslaGPS:
if self.gpsLocationExternal is None:
self.gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
sol = gen_solution(CS)
sol.logMonoTime = int(frame * DT_CTRL * 1e9)
self.gpsLocationExternal.send(sol.to_bytes())
#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 (frame % 10 == 0):
#self.ALCA.update_status(False)
self.ALCA.update_status((CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled)))
pedal_can_sends = []
if CS.pedal_interceptor_available:
#update PCC module info
pedal_can_sends = self.PCC.update_stat(CS, True)
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 = max(0.,CS.v_ego * CV.MS_TO_KPH +0.5) #BB try v_ego to reduce the false FCW warnings; was: vCS.v_cruise_actual
# Get the turn signal from ALCA.
turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators)
apply_angle = -actuators.steerAngle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not self.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 self.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 = []
#if using radar, we need to send the VIN
if CS.useTeslaRadar and (frame % 100 == 0):
useRadar=0
if CS.useTeslaRadar:
useRadar=1
can_sends.append(teslacan.create_radar_VIN_msg(self.radarVin_idx,CS.radarVIN,1,0x108,useRadar,CS.radarPosition,CS.radarEpasType))
self.radarVin_idx += 1
self.radarVin_idx = self.radarVin_idx % 3
#First we emulate DAS.
# DAS_longC_enabled (1),DAS_speed_override (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 CS.hasTeslaIcIntegration:
self.set_speed_limit_active = True
self.speed_limit_offset = CS.userSpeedLimitOffsetKph
# only change the speed limit when we have a valid vaue
if CS.userSpeedLimitKph >= 10:
self.speed_limit_for_cc = CS.userSpeedLimitKph
if CS.useTeslaMapData:
self.speedlimit_ms = CS.speedLimitKph * CV.KPH_TO_MS
self.speedlimit_valid = True
if self.speedlimit_ms == 0:
self.speedlimit_valid = False
self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms)
if frame % 10 == 0:
for socket, _ in self.poller.poll(1):
if socket is self.speedlimit and not CS.useTeslaMapData:
#get speed limit
lmd = messaging.recv_one(socket).liveMapData
self.speedlimit_ms = lmd.speedLimit
self.speedlimit_valid = lmd.speedLimitValid
self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms)
self.speed_limit_for_cc = self.speedlimit_ms * CV.MS_TO_KPH
elif socket is self.icLeads:
self.icLeadsData = tesla.ICLeads.from_bytes(socket.recv())
elif socket is self.radarState:
#to show lead car on IC
if self.icLeadsData is not None:
can_messages = self.showLeadCarOnICCanMessage(radarSocket = socket)
can_sends.extend(can_messages)
elif socket is self.alcaState:
self.alcaStateData = tesla.ALCAState.from_bytes(socket.recv())
elif socket is self.pathPlan:
#to show curvature and lanes on IC
if self.alcaStateData is not None:
self.handlePathPlanSocketForCurvatureOnIC(pathPlanSocket = socket, alcaStateData = self.alcaStateData,CS = CS)
elif socket is self.icCarLR:
can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRSocket = socket)
can_sends.extend(can_messages)
elif socket is self.trafficevents:
can_messages = self.handleTrafficEvents(trafficEventsSocket = socket)
can_sends.extend(can_messages)
if (CS.roadCurvRange > 20) and self.useMap:
if self.useZeroC0:
self.curv0 = 0.
elif self.clipC0:
self.curv0 = -clip(CS.roadCurvC0,-0.5,0.5)
#else:
# self.curv0 = -CS.roadCurvC0
#if CS.v_ego > 9:
# self.curv1 = -CS.roadCurvC1
#else:
# self.curv1 = 0.
self.curv2 = -CS.roadCurvC2
self.curv3 = -CS.roadCurvC3
self.laneRange = CS.roadCurvRange
#else:
# self.curv0 = 0.
# self.curv1 = 0.
# self.curv2 = 0.
# self.curv3 = 0.
# self.laneRange = 0
if (CS.csaRoadCurvRange > 2.) and self.useMap and not self.useMapOnly:
self.curv2 = -CS.csaRoadCurvC2
self.curv3 = -CS.csaRoadCurvC3
#if self.laneRange > 0:
# self.laneRange = min(self.laneRange,CS.csaRoadCurvRange)
#else:
self.laneRange = CS.csaRoadCurvRange
elif (CS.csaOfframpCurvRange > 2.) and self.useMap and not self.useMapOnly:
#self.curv2 = -CS.csaOfframpCurvC2
#self.curv3 = -CS.csaOfframpCurvC3
#self.curv0 = 0.
#self.curv1 = 0.
#if self.laneRange > 0:
# self.laneRange = min(self.laneRange,CS.csaOfframpCurvRange)
#else:
self.laneRange = CS.csaOfframpCurvRange
else:
self.laneRange = 50
self.laneRange = int(clip(self.laneRange,0,159))
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
speed_override = 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
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)
acc_speed_kph = self.PCC.pedal_speed_kph
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
CS.speed_control_enabled = 1
else:
CS.speed_control_enabled = 0
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 frame % 10 == 0:
can_sends.append(teslacan.create_fake_DAS_obj_lane_msg(self.leadDx,self.leadDy,self.leadClass,self.rLine,self.lLine,self.curv0,self.curv1,self.curv2,self.curv3,self.laneRange,self.laneWidth))
speed_override = 0
if (CS.pedal_interceptor_value > 10) and (cc_state > 1):
speed_override = 0 #force zero for now
if (not enable_steer_control) and op_status == 3:
#hands_on_state = 0x03
self.DAS_219_lcTempUnavailableSpeed = 1
self.warningCounter = 100
self.warningNeeded = 1
if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (not CS.blinker_on) and (self.ALCA.laneChange_cancelled_counter > 0):
self.DAS_221_lcAborting = 1
self.warningCounter = 300
self.warningNeeded = 1
if send_fake_msg:
if enable_steer_control and op_status == 3:
op_status = 0x5
can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_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,
CS.v_cruise_pcm * CV.KPH_TO_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) or (self.stopSignWarning != self.stopSignWarning_last) or (self.stopLightWarning != self.stopLightWarning_last) or (self.warningNeeded == 1) or (frame % 100 == 0):
#if it's time to send OR we have a warning or emergency disable
can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \
self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \
self.stopSignWarning, self.stopLightWarning, \
self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \
self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,0,CS.useWithoutHarness))
self.stopLightWarning_last = self.stopLightWarning
self.stopSignWarning_last = self.stopSignWarning
self.warningNeeded = 0
# end of DAS emulation """
if frame % 100 == 0: # and CS.hasTeslaIcIntegration:
#IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake
can_sends.append(teslacan.create_fake_IC_msg())
idx = frame % 16
cruise_btn = None
# send enabled ethernet every 0.2 sec
if frame % 20 == 0:
can_sends.append(teslacan.create_enabled_eth_msg(1))
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.speed_limit_for_cc, 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.speed_limit_for_cc * CV.KPH_TO_MS, self.speedlimit_valid, \
self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
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
return pedal_can_sends + can_sends
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())
