improve ALCA IC integration of sgown lanes

panda/board/safety/safety_tesla.h

@@ -221,6 +221,10 @@ int DAS_ahb_is_enabled = 0;
 //fake DAS - plain CC condition
 int DAS_plain_cc_enabled = 0x00;
 
+//fake DAS - emergency brakes use
+int DAS_emergency_brake_request = 0x00;
+int DAS_fleet_speed_state = 0x00;
+
 
 static int add_tesla_crc(uint32_t MLB, uint32_t MHB , int msg_len) {
   //"""Calculate CRC8 using 1D poly, FF start, FF end"""
@@ -385,6 +389,8 @@ static void reset_DAS_data(void) {
   DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02;
   DAS_RIGHT_OBJECT_MHB = 0x03FFFF83;
   DAS_plain_cc_enabled = 0x00;
+  DAS_emergency_brake_request = 0x00;
+  DAS_fleet_speed_state = 0x00;
 }
 
 
@@ -786,7 +792,7 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) {
     //send DAS_status - 0x399
     MLB = DAS_op_status + 0xF0 + (DAS_speed_limit_kph << 8) + (((DAS_collision_warning << 6) + DAS_speed_limit_kph) << 16);
     MHB = ((DAS_cc_state & 0x03) << 3) + (DAS_ldwStatus << 5) + 
-        (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6)) << 8) +
+        (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6) + DAS_fleet_speed_state) << 8) +
        ((( DAS_status_idx << 4) + (DAS_alca_state >> 2)) << 16);
     int cksm = add_tesla_cksm2(MLB, MHB, 0x399, 7);
     MHB = MHB + (cksm << 24);
@@ -809,10 +815,14 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) {
         lcw = 0x01;
     }
     int b4 = 0x80;
-    int b5 = 0x13;
+    int b5 = 0x13; 
     if (DAS_cc_state > 1) { //enabled or hold
-        b4 = 0x60;
-        b5 = 0x12;
+        b4 = 0x84;
+        b5 = 0x25; //BB lssState 0x 0x03 should be LSS_STATE_ELK enhanced LK
+        //DAS_RobState active 0x02
+        //DAS_radarTelemetry normal 0x01
+        //DAS_lssState 0x03
+        //DAS_acc_report ACC_report_target_CIPV 0x01
     }
     MLB = MLB + (b4 << 24);
     MHB = 0x8000 + b5 + (DAS_status2_idx << 20) + (lcw << 16);
@@ -1549,6 +1559,7 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
     DAS_high_low_beam_request = b0;
     DAS_high_low_beam_reason = b1;
     DAS_ahb_is_enabled = b2 & 0x01;
+    DAS_fleet_speed_state = (b2 >> 1) & 0x03;
     //intercept and do not forward
     return false;
   }
@@ -1583,7 +1594,8 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
     DAS_cc_state = ((b3 & 0xC0)>>6);
     DAS_usingPedal = ((b3 & 0x20) >> 5);
     DAS_alca_state = (b3 & 0x1F);
-    DAS_speed_limit_kph = b5;
+    DAS_speed_limit_kph = (b5 & 0x1F);
+    DAS_emergency_brake_request = ((b5 & 0x20)  >> 5);
     time_last_DAS_data = current_car_time;
     DAS_present = 1;
     DAS_steeringAngle = ((b7 << 8) + b6) & 0x7FFF;

selfdrive/car/tesla/carcontroller.py

@@ -495,6 +495,7 @@ def update(self, enabled, CS, frame, actuators, \
     acc_speed_kph = 0
     if enabled:
       #self.opState  0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
+      alca_state = 0x01
       if self.opState == 0:
         op_status = 0x02
       if self.opState == 1:
@@ -505,13 +506,25 @@ def update(self, enabled, CS, frame, actuators, \
         op_status = 0x01
       if self.opState == 5:
         op_status = 0x03
-      alca_state = 0x08 + turn_signal_needed
+      if turn_signal_needed > 0:
+        alca_state = 0x08 + turn_signal_needed
+      elif (self.lLine > 1) and (self.rLine > 1):
+        alca_state = 0x08
+      elif (self.lLine > 1):
+        alca_state = 0x06
+      elif (self.rLine > 1): 
+        alca_state = 0x07
+      else:
+        alca_state = 0x01
       #canceled by user
       if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0):
         alca_state = 0x14
       #min speed for ALCA
-      if CS.CL_MIN_V > CS.v_ego:
+      if (CS.CL_MIN_V > CS.v_ego):
         alca_state = 0x05
+      #max angle for ALCA
+      if (abs(actuators.steerAngle) >= CS.CL_MAX_A):
+        alca_state = 0x15
       if not enable_steer_control:
         #op_status = 0x08
         hands_on_state = 0x02
@@ -578,10 +591,18 @@ def update(self, enabled, CS, frame, actuators, \
     if CS.useTeslaRadar and CS.hasTeslaIcIntegration:
       highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1)
       if frame % 5 == 0:
-        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled))
+        fleet_speed_state = 0x00 #fleet speed unavailable
+        if CS.medianFleetSpeedMPS > 0:
+          fleet_speed_state = 0x01 #fleet speed available
+        if (CS.maxdrivespeed > 0) and CS.useTeslaMapData and (CS.mapAwareSpeed or (CS.baseMapSpeedLimitMPS <2.7)):
+          fleet_speed_state = 0x02 #fleet speed active
+        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,fleet_speed_state))
     if send_fake_msg:
       if enable_steer_control and op_status == 3:
         op_status = 0x5
+      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
       can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
             acc_speed_kph, \
@@ -590,7 +611,8 @@ def update(self, enabled, CS, frame, actuators, \
             CS.v_cruise_pcm, #acc_speed_limit_mph,
             CS.speedLimitToIc, #speed_limit_to_car,
             apply_angle,
-            1 if enable_steer_control else 0))
+            1 if enable_steer_control else 0,
+            park_brake_request))
     if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (self.stopSignWarning != self.stopSignWarning_last) or (self.stopLightWarning != self.stopLightWarning_last) or (self.warningNeeded == 1) or (frame % 100 == 0):
       #if it's time to send OR we have a warning or emergency disable
       can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \

selfdrive/car/tesla/carstate.py

@@ -132,8 +132,7 @@ def get_can_signals(CP):
       ("UI_baseMapSpeedLimitMPS", "UI_driverAssistRoadSign", 0),
       ("UI_bottomQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0),
       ("UI_rampType", "UI_driverAssistRoadSign", 0),
-
-
+      ("UI_autoSummonEnable","UI_driverAssistControl",0),
   ]
 
   checks = [
@@ -354,6 +353,8 @@ def __init__(self, CP):
     self.DAS_doorOpen = 0
     self.DAS_notInDrive = 0
 
+    self.summonButton = 0
+
 
 
     #BB variables for pedal CC
@@ -520,6 +521,7 @@ def update(self, cp, epas_cp, pedal_cp):
 
     if (self.hasTeslaIcIntegration):
       self.apEnabled = (cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1)
+      self.summonButton = int(cp.vl["UI_driverAssistControl"]["UI_autoSummonEnable"])
       self.apFollowTimeInS =  1 + cp.vl["MCU_chassisControl"]["MCU_fcwSensitivity"] * 0.5
       self.keepEonOff = cp.vl["MCU_chassisControl"]["MCU_ldwEnable"] == 1
       self.alcaEnabled = cp.vl["MCU_chassisControl"]["MCU_pedalSafetyEnable"] == 1

selfdrive/car/tesla/teslacan.py

@@ -130,11 +130,11 @@ def create_DAS_LR_object_msg(lane,v1Class,v1Id,v1Dx,v1Dy,v1V,v2Class,v2Id,v2Dx,v
     int((v2y >> 5) & 0x03) + ((v2Id << 2) & 0xFC))
   return [msg_id, 0, msg.raw, 0]
 
-def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled):
+def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled,fleet_speed_state):
   msg_id = 0x65A
   msg_len = 3
   msg = create_string_buffer(msg_len)
-  struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0))
+  struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0) + (fleet_speed_state << 1))
   return [msg_id, 0, msg.raw, 0]
 
 
@@ -145,8 +145,9 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll
                  acc_speed_limit_mph,
                  legal_speed_limit,
                  apply_angle,
-                 enable_steer_control):
-  msg_id = 0x659 #0x553 //we will try to use DAS_udsRequest to send this info to IC
+                 enable_steer_control, 
+                 park_brake_request):
+  msg_id = 0x659 #we will use DAS_udsRequest to send this info to IC
   msg_len = 8
   msg = create_string_buffer(msg_len)
   units_included = 1
@@ -156,7 +157,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll
       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(acc_speed_limit_mph),
-      int(legal_speed_limit),
+      int((legal_speed_limit & 0x1F) + ((park_brake_request << 5) & 0x20)), #positions 7 and 6 not used yet
       int(c_apply_steer & 0xFF),
       int((c_apply_steer >> 8) & 0xFF))
   return [msg_id, 0, msg.raw, 0]