v1.0 for testing

selfdrive/car/tesla/carcontroller.py

@@ -36,6 +36,9 @@ def __init__(self, dbc_name, CP, VM):
  self.IC_DAS_lane_counter = 0
  self.IC_previous_enabled = False
 
+ self.cruiseDelayFrame = 0
+ self.prevCruiseEnabled = False
+
  if CP.openpilotLongitudinalControl:
  self.lP = messaging.sub_sock('longitudinalPlan')
  self.rS = messaging.sub_sock('radarState')
@@ -47,6 +50,17 @@ def __init__(self, dbc_name, CP, VM):
  def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
  left_line, right_line, lead, left_lane_depart, right_lane_depart):
  can_sends = []
+ #add 1 second delay logic to wait for AP which has a status at 2Hz
+ if CS.cruiseEnabled:
+ if not self.prevCruiseEnabled:
+ self.cruiseDelayFrame = frame
+ if frame - self.cruiseDelayFrame > 30:
+ CS.cruiseDelay = True
+ else:
+ self.cruiseDelayFrame = 0
+ CS.cruiseDelay = False
+ self.prevCruiseEnabled = CS.cruiseEnabled
+
  self.IC_integration_counter = ((self.IC_integration_counter + 1) % 100)
  if self.IC_integration_warning_counter > 0:
  self.IC_integration_warning_counter = self.IC_integration_warning_counter - 1
@@ -243,7 +257,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
  CS.DAS_208_rackDetected, CS.stopSignWarning, CS.stopLightWarning, CAN_CHASSIS[self.CP.carFingerprint]))
 
  # send DAS_status and DAS_status2 at 2Hz
- if self.IC_integration_counter == 40 or self.IC_integration_counter == 90:
+ if self.IC_integration_counter == 40 or self.IC_integration_counter == 90 or (self.IC_previous_enabled and not enabled ):
  if self.CP.carFingerprint in [CAR.AP1_MODELS,CAR.AP2_MODELS]:
  self.IC_DAS_status_counter = -1
  else:
@@ -262,7 +276,8 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
  CS.DAS_fusedSpeedLimit, CAN_CHASSIS[self.CP.carFingerprint], self.IC_DAS_status_counter))
  can_sends.append(self.tesla_can.create_das_status2(CS.msg_autopilot_status2,CS.out.cruiseState.speed * CV.MS_TO_MPH, 
  DAS_collision_warning, DAS_hands_on_state, CAN_CHASSIS[self.CP.carFingerprint], self.IC_DAS_status_counter))
- self.IC_previous_enabled = enabled
+
+ self.IC_previous_enabled = enabled
 
 
  # TODO: HUD control: Autopilot Status, (Status2 also needed for long control),

selfdrive/car/tesla/carstate.py

@@ -72,6 +72,9 @@ def __init__(self, CP):
  self.DAS_fusedSpeedLimit = 0
  self.baseMapSpeedLimitMPS = 0
 
+ self.cruiseEnabled = False
+ self.cruiseDelay = False
+
  #start config section
  self.forcePedalOverCC = False
  self.enableHSO = True
@@ -154,8 +157,8 @@ def update(self, cp, cp_cam):
 
  acc_enabled = (cruise_state in ["ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"])
  autopilot_enabled = (autopilot_status in ["ACTIVE_1", "ACTIVE_2"]) #, "ACTIVE_NAVIGATE_ON_AUTOPILOT"])
-
- ret.cruiseState.enabled = acc_enabled and not autopilot_enabled
+ self.cruiseEnabled = acc_enabled and not autopilot_enabled
+ ret.cruiseState.enabled = self.cruiseEnabled and self.cruiseDelay
  if speed_units == "KPH":
  ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
  elif speed_units == "MPH":

selfdrive/car/tesla/teslacan.py

@@ -167,10 +167,10 @@ def create_das_warningMatrix3 (self, DAS_gas_to_resume, DAS_211_accNoSeatBelt, D
  msg_len = 8
  msg = create_string_buffer(msg_len)
  struct.pack_into('BBBBBBBB', msg, 0,
- (DAS_gas_to_resume * 2) + (stopSignWarning * 8) + (stopLightWarning * 16),
- (DAS_202_noisyEnvironment * 2) + (DAS_207_lkasUnavailable * 64) + (DAS_208_rackDetected * 128),
- (DAS_211_accNoSeatBelt * 4),
- (DAS_219_lcTempUnavailableSpeed * 4) + (DAS_220_lcTempUnavailableRoad * 8) + (DAS_221_lcAborting * 16) + (DAS_222_accCameraBlind * 32),
+ (DAS_gas_to_resume << 1) + (stopSignWarning << 3) + (stopLightWarning << 4),
+ (DAS_202_noisyEnvironment << 1) + (DAS_207_lkasUnavailable << 6) + (DAS_208_rackDetected << 7),
+ (DAS_211_accNoSeatBelt << 2),
+ (DAS_219_lcTempUnavailableSpeed << 2) + (DAS_220_lcTempUnavailableRoad << 3) + (DAS_221_lcAborting << 4) + (DAS_222_accCameraBlind << 5),
  0,0,0,0)
  return [msg_id, 0, msg.raw, bus]