def _update_pedal_state(self, CS, frame):
if CS.pedal_idx != CS.prev_pedal_idx:
# time out pedal after 500ms without receiving a new CAN message from it
self.pedal_timeout_frame = frame + 50
self.prev_pcc_available = self.pcc_available
pedal_ready = frame < self.pedal_timeout_frame and CS.pedal_interceptor_state == 0
acc_disabled = CS.forcePedalOverCC or CruiseState.is_off(CS.pcm_acc_status)
# Mark pedal unavailable while traditional cruise is on.
self.pcc_available = pedal_ready and acc_disabled
if self.pcc_available != self.prev_pcc_available:
CS.config_ui_buttons(self.pcc_available, pedal_ready and not acc_disabled)
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_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_stat(self, CS, enabled):
# Check if the cruise stalk was double pulled, indicating that adaptive
# cruise control should be enabled. Twice in .75 seconds counts as a double
# pull.
self.prev_enable_adaptive_cruise = self.enable_adaptive_cruise
acc_string = CS.cstm_btns.get_button_label2(ACCMode.BUTTON_NAME)
acc_mode = ACCMode.from_label(acc_string)
CS.cstm_btns.get_button(
ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label
self.autoresume = acc_mode.autoresume
curr_time_ms = _current_time_millis()
# Handle pressing the enable button.
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(
ACCMode.BUTTON_NAME) > ACCState.OFF and enabled
and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
and CS.v_ego > self.MIN_CRUISE_SPEED_MS)
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_adaptive_cruise = True
# Increase ACC speed to match current, if applicable.
self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH,
self.acc_speed_kph)
self.user_has_braked = False
self.has_gone_below_min_speed = False
else:
# A single pull disables ACC (falling back to just steering).
self.enable_adaptive_cruise = False
# Handle pressing the cancel button.
elif CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_adaptive_cruise = False
self.acc_speed_kph = 0.
self.last_cruise_stalk_pull_time = 0
# Handle pressing up and down buttons.
elif (self.enable_adaptive_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
self._update_max_acc_speed(CS)
if CS.brake_pressed:
self.user_has_braked = True
if not self.autoresume:
self.enable_adaptive_cruise = False
if CS.v_ego < self.MIN_CRUISE_SPEED_MS:
self.has_gone_below_min_speed = True
# If autoresume is not enabled, manually steering or slowing disables ACC.
if not self.autoresume:
if not enabled or self.user_has_braked or self.has_gone_below_min_speed:
self.enable_adaptive_cruise = False
# Notify if ACC was toggled
if self.prev_enable_adaptive_cruise and not self.enable_adaptive_cruise:
CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4)
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.STANDBY)
elif self.enable_adaptive_cruise:
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.ENABLED)
if not self.prev_enable_adaptive_cruise:
CS.UE.custom_alert_message(2, "ACC Enabled", 150)
# Update the UI to show whether the current car state allows ACC.
if CS.cstm_btns.get_button_status(
ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]:
if (enabled
and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
and CS.v_ego > self.MIN_CRUISE_SPEED_MS):
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.STANDBY)
else:
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.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_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