diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 1354c748ad985c..eb23aeec573422 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -2,12 +2,10 @@ from selfdrive.car.tesla import teslacan from selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState from common.numpy_fast import clip, interp -from selfdrive.services import service_list from selfdrive.car.tesla.values import CruiseState, CruiseButtons from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.planner import calc_cruise_accel_limits -from common.realtime import sec_since_boot import selfdrive.messaging as messaging import time import math @@ -129,8 +127,9 @@ class PCCController(): def __init__(self,carcontroller): self.CC = carcontroller self.human_cruise_action_time = 0 - self.pedal_state = False - self.prev_pedal_state = False + self.pcc_available = self.prev_pcc_available = False + self.pedal_timeout_frame = 0 + self.accelerator_pedal_pressed = self.prev_accelerator_pedal_pressed = False self.automated_cruise_action_time = 0 self.last_angle = 0. self.radarState = messaging.sub_sock('radarState', conflate=True) @@ -149,7 +148,6 @@ def __init__(self,carcontroller): 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 @@ -161,7 +159,6 @@ def __init__(self,carcontroller): #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 @@ -228,46 +225,37 @@ def reset(self, v_pid): if self.LoC and RESET_PID_ON_DISENGAGE: self.LoC.reset(v_pid) - def update_stat(self, CS, enabled): + 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 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 can_sends - - # 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: + if self.pcc_available and CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) == PCCState.OFF: + CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.STANDBY) + elif not self.pcc_available: + if CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) != PCCState.OFF: + CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.OFF) + if CS.pedal_interceptor_state > 0: + CS.UE.custom_alert_message(4, "Pedal Interceptor fault (state %s)" % CS.pedal_interceptor_state, 200, 4) + elif frame >= self.pedal_timeout_frame: + CS.UE.custom_alert_message(4, "Pedal Interceptor timed out", 200, 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)) - 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) + return can_sends + # 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, "PCC Disabled", 150, 4) - CS.cstm_btns.set_button_status("pedal", 1) - print ("brake pressed") + CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, 1) prev_enable_pedal_cruise = self.enable_pedal_cruise # process any stalk movement @@ -280,8 +268,7 @@ def update_stat(self, CS, enabled): 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 + 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. @@ -326,17 +313,17 @@ def update_stat(self, CS, enabled): # 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) + CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, 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) + 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("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) + 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("pedal", PCCState.NOT_READY) + 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 @@ -345,7 +332,6 @@ def update_stat(self, CS, enabled): return can_sends def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, speed_limit_valid, set_speed_limit_active, speed_limit_offset,alca_enabled): - cur_time = sec_since_boot() idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph @@ -374,7 +360,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s self.maxsuggestedspeed_avg.reset() self.pedal_idx = (self.pedal_idx + 1) % 16 - if not CS.pedal_interceptor_available or not enabled: + if not self.pcc_available or not enabled: return 0., 0, idx # Alternative speed decision logic that uses the lead car's distance # and speed more directly. @@ -422,13 +408,13 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping] # determine if pedal is pressed by human - self.prev_pedal_state = self.pedal_state - self.pedal_state = CS.pedal_interceptor_value > 10 + self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed + self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10 #reset PID if we just lifted foot of accelerator - if (not self.pedal_state) and self.prev_pedal_state: + if (not self.accelerator_pedal_pressed) and self.prev_accelerator_pedal_pressed: self.reset(v_pid=CS.v_ego) - if self.enable_pedal_cruise and (not self.pedal_state): + if self.enable_pedal_cruise and not self.accelerator_pedal_pressed: jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, self.v_pid, @@ -442,7 +428,6 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s self.v_acc = self.v_cruise self.a_acc = self.a_cruise self.v_acc_future = self.v_pid - self.acc_start_time = cur_time # Interpolation of trajectory self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * (self.a_acc - self.a_acc_start) @@ -640,6 +625,20 @@ def pedal_hysteresis(self, pedal, enabled): self.pedal_steady = pedal + PEDAL_HYST_GAP return self.pedal_steady + 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 _visual_radar_adjusted_dist_m(m, CS): # visual radar sucks at short distances. It rarely shows readings below 7m. # So rescale distances with 7m -> 0m. Maxes out at 1km, if that matters. diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 3b78faaec848b5..a056fef310c5b6 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -354,11 +354,9 @@ def update(self, enabled, CS, frame, actuators, \ 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))) - pedal_can_sends = [] - - if CS.pedal_interceptor_available: - #update PCC module info - pedal_can_sends = self.PCC.update_stat(CS, True) + # 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. @@ -539,7 +537,7 @@ def update(self, enabled, CS, frame, actuators, \ 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: + if self.PCC.pcc_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: @@ -551,7 +549,7 @@ def update(self, enabled, CS, frame, actuators, \ 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): + 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: @@ -616,11 +614,11 @@ def update(self, enabled, CS, frame, actuators, \ park_brake_request = int(CS.ahbEnabled) if park_brake_request == 1: print("Park Brake Request received") - adaptive_cruise = 1 if (not CS.pedal_interceptor_available and self.ACC.adaptive) or CS.pedal_interceptor_available else 0 + 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, \ turn_signal_needed,forward_collision_warning, adaptive_cruise, hands_on_state, \ - cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \ + cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \ CS.v_cruise_pcm, #acc_speed_limit_mph, CS.speedLimitToIc, #speed_limit_to_car, apply_angle, @@ -644,7 +642,7 @@ def update(self, enabled, CS, frame, actuators, \ # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) - if (not CS.pedal_interceptor_available) and frame % 5 == 0: # acc processed at 20Hz + 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_for_cc, self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset) @@ -656,7 +654,7 @@ def update(self, enabled, CS, frame, actuators, \ # 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 + if self.PCC.pcc_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) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index e838944225a50a..261fef0538f830 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -307,10 +307,10 @@ def __init__(self, CP): self.user_gas = 0. self.user_gas_pressed = False - self.pedal_interceptor_state = 0 + self.pedal_interceptor_state = self.prev_pedal_interceptor_state = 0 self.pedal_interceptor_value = 0. self.pedal_interceptor_value2 = 0. - self.pedal_interceptor_missed_counter = 0 + self.pedal_idx = self.prev_pedal_idx = 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 @@ -363,9 +363,6 @@ def __init__(self, CP): #BB variables for pedal CC self.pedal_speed_kph = 0. - # Pedal mode is ready, i.e. hardware is present and normal cruise is off. - self.pedal_interceptor_available = False - self.prev_pedal_interceptor_available = False #BB UIEvents self.UE = UIEvents(self) @@ -426,11 +423,10 @@ def __init__(self, CP): self.ahbHiBeamOn = 0 self.ahbNightMode = 0 - def config_ui_buttons(self, pedalAvailable, pedalPresent): - if pedalAvailable: + def config_ui_buttons(self, pcc_available, pcc_blocked_by_acc_mode): + if pcc_available: self.btns_init[1] = [PCCModes.BUTTON_NAME, PCCModes.BUTTON_ABREVIATION, PCCModes.labels()] else: - # we don't have pedal interceptor self.btns_init[1] = [ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()] btn = self.cstm_btns.btns[1] btn.btn_name = self.btns_init[1][0] @@ -438,7 +434,7 @@ def config_ui_buttons(self, pedalAvailable, pedalPresent): btn.btn_label2 = self.btns_init[1][2][0] btn.btn_status = 1 - if (not pedalAvailable) and pedalPresent: + if (not pcc_available) and pcc_blocked_by_acc_mode: btn.btn_label2 = self.btns_init[1][2][1] self.cstm_btns.update_ui_buttons(1, 1) @@ -596,7 +592,7 @@ def update(self, cp, epas_cp, pedal_cp): self.v_wheel = 0 #JCT self.v_weight = 0 #JCT speed = (cp.vl["DI_torque2"]['DI_vehicleSpeed']) * CV.MPH_TO_KPH/3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S - speed = speed * 1.01 # To match car's displayed speed + speed = speed * 1.02 # To match car's displayed speed if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.x = [[speed], [0.0]] @@ -608,9 +604,12 @@ def update(self, cp, epas_cp, pedal_cp): #BB use this set for pedal work as the user_gas_xx is used in other places + self.prev_pedal_interceptor_state = self.pedal_interceptor_state self.pedal_interceptor_state = pedal_cp.vl["GAS_SENSOR"]['STATE'] self.pedal_interceptor_value = pedal_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] self.pedal_interceptor_value2 = pedal_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2'] + self.prev_pedal_idx = self.pedal_idx + self.pedal_idx = pedal_cp.vl["GAS_SENSOR"]['IDX'] can_gear_shifter = cp.vl["DI_torque2"]['DI_gear'] @@ -634,7 +633,6 @@ def update(self, cp, epas_cp, pedal_cp): self.DI_cruiseSet = cp.vl["DI_state"]['DI_cruiseSet'] if self.imperial_speed_units: self.DI_cruiseSet = self.DI_cruiseSet * CV.MPH_TO_KPH - self.cruise_speed_offset = 0. self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint) self.pedal_gas = 0. # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1 @@ -656,25 +654,7 @@ def update(self, cp, epas_cp, pedal_cp): self.regenLight = cp.vl["DI_state"]['DI_regenLight'] == 1 - self.prev_pedal_interceptor_available = self.pedal_interceptor_available - pedal_has_value = bool(self.pedal_interceptor_value) or bool(self.pedal_interceptor_value2) - pedal_interceptor_present = self.pedal_interceptor_state in [0, 5] and pedal_has_value - # Add loggic if we just miss some CAN messages so we don't immediately disable pedal - if pedal_has_value: - self.pedal_interceptor_missed_counter = 0 - if pedal_interceptor_present: - self.pedal_interceptor_missed_counter = 0 - else: - self.pedal_interceptor_missed_counter += 1 - pedal_interceptor_present = pedal_interceptor_present and (self.pedal_interceptor_missed_counter < 10) - # Mark pedal unavailable while traditional cruise is on. - self.pedal_interceptor_available = pedal_interceptor_present and (self.forcePedalOverCC or not bool(self.pcm_acc_status)) - if self.pedal_interceptor_available != self.prev_pedal_interceptor_available: - self.config_ui_buttons(self.pedal_interceptor_available, pedal_interceptor_present) - self.v_cruise_actual = self.DI_cruiseSet - self.hud_lead = 0 #JCT - self.cruise_speed_offset = 0. # carstate standalone tester diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index bbe62194c0e21e..a1f91b0ff06509 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -141,7 +141,7 @@ def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled,fleet_speed_ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ turn_signal_needed,forward_collission_warning,adaptive_cruise, hands_on_state, \ - cc_state, pedal_state, alca_state, \ + cc_state, pcc_available, alca_state, \ acc_speed_limit_mph, legal_speed_limit, apply_angle, @@ -155,7 +155,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll struct.pack_into('BBBBBBBB', msg, 0,int((speed_control_enabled << 7) + (speed_override << 6) + (apUnavailable << 5) + (collision_warning << 4) + op_status), \ int(acc_speed_kph), \ int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4) + (adaptive_cruise << 3) + hands_on_state), \ - int((cc_state << 6) + (pedal_state << 5) + alca_state), \ + int((cc_state << 6) + (pcc_available << 5) + alca_state), \ int(acc_speed_limit_mph), int((legal_speed_limit & 0x1F) + ((park_brake_request << 5) & 0x20)), #positions 7 and 6 not used yet int(c_apply_steer & 0xFF),