v1.0 for testing

BogGyver · Apr 19, 2021 · 21674c3 · 21674c3
1 parent 5258636
commit 21674c3
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 9 deletions.
diff --git a/panda b/panda
diff --git a/selfdrive/car/tesla/carcontroller.py b/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),

diff --git a/selfdrive/car/tesla/carstate.py b/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":

diff --git a/selfdrive/car/tesla/teslacan.py b/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]