PCC fixes (commaai#136)

* fixed effectively unreachable code in PCC module
* fixed silent pcc disengagements
* added/improved on screen error messages
* refactoring and cleanup (removed unused code)

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.

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)

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,19 +423,18 @@ 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]
     btn.btn_label = self.btns_init[1][1]
     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

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),