class PCCController(object):
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.automated_cruise_action_time = 0
self.last_angle = 0.
context = zmq.Context()
self.poller = zmq.Poller()
self.live20 = messaging.sub_sock(context,
service_list['live20'].port,
conflate=True,
poller=self.poller)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.last_cruise_stalk_pull_time = 0
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.
self.pedal_idx = 0
self.accel_steady = 0.
self.prev_tesla_accel = 0.
self.prev_tesla_pedal = 0.
self.pedal_interceptor_state = 0
self.torqueLevel_last = 0.
self.prev_v_ego = 0.
self.PedalForZeroTorque = 18. #starting number, works on my S85
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.
self.a_pid = 0.
self.last_output_gb = 0.
self.last_speed_kph = 0.
#for smoothing the changes in speed
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.acc_start_time = sec_since_boot()
self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
#Long Control
self.LoC = None
#when was radar data last updated?
self.last_md_ts = None
self.last_l100_ts = None
self.md_ts = None
self.l100_ts = None
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
def reset(self, v_pid):
if self.LoC:
self.LoC.reset(v_pid)
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_pdl(self, enabled, CS, frame, actuators, pcm_speed):
cur_time = sec_since_boot()
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
if not CS.pedal_interceptor_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
l20 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
l20 = messaging.recv_one(socket)
break
if l20 is not None:
self.lead_1 = l20.live20.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
self.md_ts = l20.live20.mdMonoTime
self.l100_ts = l20.live20.l100MonoTime
brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS)
# cruise speed can't be negative even is user is distracted
self.v_pid = max(self.v_pid, 0.)
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
if self.enable_pedal_cruise:
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.pedal_speed_kph,
self.lead_last_seen_time_ms)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, self.v_pid, accel_max,
brake_max, jerk_max, jerk_min, _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (
self.a_acc_sol + self.a_acc_start) / 2.0
# we will try to feed forward the pedal position.... we might want to feed the last output_gb....
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_pid)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_pid, vTarget,
self.vTargetFuture, self.a_acc_sol, CS.CP, None)
output_gb = t_go - t_brake
#print "Output GB Follow:", output_gb
else:
self.LoC.reset(CS.v_ego)
print "PID reset"
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
##############################################################
# This is an experimental mode that is probably broken.
#
# Don't use it.
#
# Ratios are centered at 1. They can be multiplied together.
# Factors are centered around 0. They can be multiplied by constants.
# For example +9% is a 1.06 ratio or 0.09 factor.
##############################################################
elif PCCModes.is_selected(ExperimentalMode(), CS.cstm_btns):
output_gb = 0.0
if enabled and self.enable_pedal_cruise:
MAX_DECEL_RATIO = 0
MAX_ACCEL_RATIO = 1.1
available_speed_kph = self.pedal_speed_kph - CS.v_ego * CV.MS_TO_KPH
# Hold accel if radar gives intermittent readings at great distance.
# Makes the car less skittish when first making radar contact.
if (_is_present(self.lead_1)
and self.continuous_lead_sightings < 8
and _sec_til_collision(self.lead_1) > 3
and self.lead_1.dRel > 60):
output_gb = self.last_output_gb
# Hold speed in turns if no car is seen
elif CS.angle_steers >= 5.0 and not _is_present(self.lead_1):
pass
# Try to stay 2 seconds behind lead, matching their speed.
elif _is_present(self.lead_1):
weighted_d_ratio = _weighted_distance_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
weighted_v_ratio = _weighted_velocity_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
# Don't bother decelerating if the lead is already pulling away
if weighted_d_ratio < 1 and weighted_v_ratio > 1.01:
gas_brake_ratio = max(1, self.last_output_gb + 1)
else:
gas_brake_ratio = weighted_d_ratio * weighted_v_ratio
# rescale around 0 rather than 1.
output_gb = gas_brake_ratio - 1
# If no lead has been seen recently, accelerate to max speed.
else:
# An acceleration factor that drops off as we aproach max speed.
max_speed_factor = min(available_speed_kph, 3) / 3
# An acceleration factor that increases as time passes without seeing
# a lead car.
time_factor = (_current_time_millis() -
self.lead_last_seen_time_ms) / 3000
time_factor = clip(time_factor, 0, 1)
output_gb = 0.14 * max_speed_factor * time_factor
# If going above the max configured PCC speed, slow. This should always
# be in force so it is not part of the if/else block above.
if available_speed_kph < 0:
# linearly brake harder, hitting -1 at 10kph over
speed_limited_gb = max(available_speed_kph, -10) / 10.0
# This is a minimum braking amount. The logic above may ask for more.
output_gb = min(output_gb, speed_limited_gb)
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) <
abs(self.lastTorqueForPedalForZeroTorque)):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)
#print "Torque level at detection %s" % (CS.torqueLevel)
#print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., accel_max)
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.)
# if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0,
MAX_PEDAL_VALUE - tesla_brake)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal, self.accel_steady = accel_hysteresis(
tesla_pedal, self.accel_steady, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
self.last_md_ts = self.md_ts
self.last_l100_ts = self.l100_ts
return self.prev_tesla_pedal, enable_pedal, idx
# function to calculate the cruise speed based on a safe follow distance
def calc_follow_speed_ms(self, CS):
# Make sure we were able to populate lead_1.
if self.lead_1 is None:
return None, None, None
# dRel is in meters.
lead_dist_m = _visual_radar_adjusted_dist_m(self.lead_1.dRel)
# Grab the relative speed.
rel_speed_kph = self.lead_1.vRel * CV.MS_TO_KPH
# v_ego is in m/s, so safe_distance is in meters.
safe_dist_m = _safe_distance_m(CS.v_ego)
# Current speed in kph
actual_speed_kph = CS.v_ego * CV.MS_TO_KPH
# speed and brake to issue
new_speed_kph = self.last_speed_kph
### Logic to determine best cruise speed ###
if self.enable_pedal_cruise:
# If no lead is present, accel up to max speed
if lead_dist_m == 0:
new_speed_kph = self.pedal_speed_kph
elif lead_dist_m > 0:
lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph
if lead_dist_m < MIN_SAFE_DIST_M:
new_speed_kph = MIN_PCC_V_KPH
else:
desired_speeds = OrderedDict([
# (distance in m, desired speed in kph)
# if too close, make sure we're falling back.
(0.5 * safe_dist_m,
clip(actual_speed_kph, lead_absolute_speed_kph - 20,
lead_absolute_speed_kph - 1)),
# if at the comfort point, match lead speed.
(1.0 * safe_dist_m,
clip(actual_speed_kph, lead_absolute_speed_kph - 7,
lead_absolute_speed_kph + 6)),
(1.5 * safe_dist_m,
clip(actual_speed_kph, lead_absolute_speed_kph - 6,
lead_absolute_speed_kph + 7)),
# if too far, make sure we're closing.
(3.5 * safe_dist_m,
clip(actual_speed_kph, lead_absolute_speed_kph + 1,
lead_absolute_speed_kph + 20))
])
new_speed_kph = _interp_map(lead_dist_m, desired_speeds)
# Enforce limits on speed in the presence of a lead car.
new_speed_kph = min(
new_speed_kph, _max_safe_speed_kph(lead_dist_m),
lead_absolute_speed_kph - _min_safe_vrel_kph(lead_dist_m))
# Enforce limits on speed
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH)
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH,
self.pedal_speed_kph)
if CS.blinker_on:
# Don't accelerate during manual turns.
new_speed_kph = min(new_speed_kph, self.last_speed_kph)
self.last_speed_kph = new_speed_kph
return new_speed_kph * CV.KPH_TO_MS
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
class PCCController(object):
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.automated_cruise_action_time = 0
self.last_angle = 0.
context = zmq.Context()
self.poller = zmq.Poller()
self.live20 = messaging.sub_sock(context,
service_list['live20'].port,
conflate=True,
poller=self.poller)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.
self.pedal_idx = 0
self.pedal_steady = 0.
self.prev_tesla_accel = 0.
self.prev_tesla_pedal = 0.
self.pedal_interceptor_state = 0
self.torqueLevel_last = 0.
self.prev_v_ego = 0.
self.PedalForZeroTorque = 18. #starting number, works on my S85
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.
self.a_pid = 0.
self.last_output_gb = 0.
self.last_speed_kph = 0.
#for smoothing the changes in speed
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.acc_start_time = sec_since_boot()
self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
#Long Control
self.LoC = None
#when was radar data last updated?
self.last_md_ts = None
self.last_l100_ts = None
self.md_ts = None
self.l100_ts = None
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
def reset(self, v_pid):
if self.LoC:
self.LoC.reset(v_pid)
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):
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("pedal") > PCCState.OFF
and enabled 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.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)
# 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.
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 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("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)
or CS.forcePedalOverCC):
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_pdl(self, enabled, CS, frame, actuators, pcm_speed):
cur_time = sec_since_boot()
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
if not CS.pedal_interceptor_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
l20 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
l20 = messaging.recv_one(socket)
break
if l20 is not None:
self.lead_1 = l20.live20.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
self.md_ts = l20.live20.mdMonoTime
self.l100_ts = l20.live20.l100MonoTime
brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS)
# cruise speed can't be negative even is user is distracted
self.v_pid = max(self.v_pid, 0.)
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
if self.enable_pedal_cruise:
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.pedal_speed_kph,
self.lead_last_seen_time_ms)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, self.v_pid, accel_max,
brake_max, jerk_max, jerk_min, _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (
self.a_acc_sol + self.a_acc_start) / 2.0
# we will try to feed forward the pedal position.... we might want to feed the last output_gb....
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_pid)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_pid, vTarget,
self.vTargetFuture, self.a_acc_sol, CS.CP, None)
output_gb = t_go - t_brake
#print "Output GB Follow:", output_gb
else:
self.LoC.reset(CS.v_ego)
#print "PID reset"
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) <
abs(self.lastTorqueForPedalForZeroTorque)):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)
#print "Torque level at detection %s" % (CS.torqueLevel)
#print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., accel_max)
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.)
# if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0,
MAX_PEDAL_VALUE - tesla_brake)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
self.last_md_ts = self.md_ts
self.last_l100_ts = self.l100_ts
return self.prev_tesla_pedal, enable_pedal, idx
# function to calculate the cruise speed based on a safe follow distance
def calc_follow_speed_ms(self, CS):
# Make sure we were able to populate lead_1.
if self.lead_1 is None:
return None, None, None
# dRel is in meters.
lead_dist_m = _visual_radar_adjusted_dist_m(self.lead_1.dRel)
# Grab the relative speed.
rel_speed_kph = self.lead_1.vRel * CV.MS_TO_KPH
# v_ego is in m/s, so safe_distance is in meters.
safe_dist_m = _safe_distance_m(CS.v_ego)
# Current speed in kph
actual_speed_kph = CS.v_ego * CV.MS_TO_KPH
# speed and brake to issue
new_speed_kph = self.last_speed_kph
### Logic to determine best cruise speed ###
if self.enable_pedal_cruise:
# If no lead is present, accel up to max speed
if lead_dist_m == 0:
new_speed_kph = self.pedal_speed_kph
elif lead_dist_m > 0:
lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph
if lead_dist_m < MIN_SAFE_DIST_M:
new_speed_kph = MIN_PCC_V_KPH
else:
# Force speed into a band that is generally slower than lead if too
# close, and faster than lead if too far. Allow a range of speeds at
# any given distance, to prevent continuous jerky adjustments.
min_vrel_kph_map = OrderedDict([
# (distance in m, min allowed relative kph)
(0.5 * safe_dist_m, 2),
(1.0 * safe_dist_m, -6),
(1.5 * safe_dist_m, -10),
(3.0 * safe_dist_m, -20)
])
min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map)
max_vrel_kph_map = OrderedDict([
# (distance in m, max allowed relative kph)
(0.5 * safe_dist_m, 8),
(1.0 * safe_dist_m, 1),
(1.5 * safe_dist_m, 0),
# With visual radar the relative velocity is 0 until the confidence
# gets high. So even a small negative number here gives constant
# accel until lead lead car gets close enough to read.
(2.0 * safe_dist_m, -2)
])
max_vrel_kph = _interp_map(lead_dist_m, max_vrel_kph_map)
min_kph = lead_absolute_speed_kph - max_vrel_kph
max_kph = lead_absolute_speed_kph - min_vrel_kph
# In the special case were we are going faster than intended but it's
# still an acceptable speed, accept it. This could happen if the
# driver manually accelerates, or if we roll down a hill. In either
# case, don't fight the extra velocity unless necessary.
if actual_speed_kph > new_speed_kph and min_kph < actual_speed_kph < max_kph:
new_speed_kph = actual_speed_kph
new_speed_kph = clip(new_speed_kph, min_kph, max_kph)
# Enforce limits on speed in the presence of a lead car.
new_speed_kph = min(
new_speed_kph, _max_safe_speed_kph(lead_dist_m),
lead_absolute_speed_kph - _min_safe_vrel_kph(lead_dist_m))
# Enforce limits on speed
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH)
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH,
self.pedal_speed_kph)
if CS.blinker_on:
# Don't accelerate during manual turns.
new_speed_kph = min(new_speed_kph, self.last_speed_kph)
self.last_speed_kph = new_speed_kph
return new_speed_kph * CV.KPH_TO_MS
def pedal_hysteresis(self, pedal, enabled):
# for small accel oscillations within PEDAL_HYST_GAP, don't change the command
if not enabled:
# send 0 when disabled, otherwise acc faults
self.pedal_steady = 0.
elif pedal > self.pedal_steady + PEDAL_HYST_GAP:
self.pedal_steady = pedal - PEDAL_HYST_GAP
elif pedal < self.pedal_steady - PEDAL_HYST_GAP:
self.pedal_steady = pedal + PEDAL_HYST_GAP
return self.pedal_steady