CC as option for ACC; fix ALCA IC integration

panda/board/safety/safety_tesla.h

@@ -218,6 +218,9 @@ int DAS_high_low_beam_request = 0x00;
 int DAS_high_low_beam_reason = 0x00;
 int DAS_ahb_is_enabled = 0;
 
+//fake DAS - plain CC condition
+int DAS_plain_cc_enabled = 0x00;
+
 
 static int add_tesla_crc(uint32_t MLB, uint32_t MHB , int msg_len) {
   //"""Calculate CRC8 using 1D poly, FF start, FF end"""
@@ -381,6 +384,7 @@ static void reset_DAS_data(void) {
   DAS_LEFT_OBJECT_MHB = 0x03FFFF83;
   DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02;
   DAS_RIGHT_OBJECT_MHB = 0x03FFFF83;
+  DAS_plain_cc_enabled = 0x00;
 }
 
 
@@ -1033,7 +1037,9 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push)
     if ((EON_is_connected == 1) && (DAS_usingPedal == 0) && (DAS_cc_state != 2) && ( acc_state >= 2) && ( acc_state <= 4)) {
       //disable if more than two seconds since last pull, or there was never a stalk pull
       if (((current_car_time >= time_at_last_stalk_pull + 2) && (current_car_time != -1) && (time_at_last_stalk_pull != -1)) || (time_at_last_stalk_pull == -1)) {
-        do_fake_stalk_cancel(to_push->RIR, to_push->RDTR);
+        if (DAS_plain_cc_enabled == 0) {
+          do_fake_stalk_cancel(to_push->RIR, to_push->RDTR);
+        }
       }
     }
   }
@@ -1542,7 +1548,8 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
     int b2 = ((to_send->RDLR >> 16) & 0xFF);
     DAS_high_low_beam_request = b0;
     DAS_high_low_beam_reason = b1;
-    DAS_ahb_is_enabled = b2;
+    DAS_ahb_is_enabled = b2 & 0x01;
+    DAS_plain_cc_enabled = (b2 >> 1) & 0x01;
     //intercept and do not forward
     return false;
   }

selfdrive/car/tesla/ACC_module.py

@@ -16,24 +16,26 @@ class ACCState():
   NOT_READY = 9   # Not ready to be engaged due to the state of the car.
 
 class _Mode():
-  def __init__(self, label, autoresume, state):
+  def __init__(self, label, autoresume, state,adaptive):
     self.label = label
     self.autoresume = autoresume
     self.state = state
+    self.adaptive = adaptive
     self.next = None
 
 class ACCMode():
   # Possible ACC modes, controlling how ACC behaves.
   # This is separate from ACC state. For example, you could
   # have ACC in "Autoresume" mode in "Standby" state.
-  FOLLOW = _Mode(label="follow",   autoresume=False, state=ACCState.STANDBY)
-  AUTO =   _Mode(label="auto", autoresume=True,  state=ACCState.STANDBY)
+  FOLLOW = _Mode(label="follow",   autoresume=False, state=ACCState.STANDBY, adaptive=True)
+  AUTO =   _Mode(label="auto", autoresume=True,  state=ACCState.STANDBY, adaptive=True)
+  PLAINCC = _Mode(label="cc",   autoresume=False, state=ACCState.STANDBY, adaptive=False)
 
   BUTTON_NAME = 'acc'
   BUTTON_ABREVIATION = 'ACC'
 
   # Toggle order: OFF -> ON -> AUTO -> OFF
-  _all_modes = [FOLLOW, AUTO]
+  _all_modes = [FOLLOW, AUTO,PLAINCC]
   for index, mode in enumerate(_all_modes):
     mode.next = _all_modes[(index + 1) % len(_all_modes)]
 
@@ -67,6 +69,7 @@ def __init__(self,carcontroller):
     # Whether to re-engage automatically after being paused due to low speed or
     # user-initated deceleration.
     self.autoresume = False
+    self.adaptive = False
     self.last_brake_press_time = 0
     self.last_cruise_stalk_pull_time = 0
     self.prev_cruise_buttons = CruiseButtons.IDLE
@@ -95,33 +98,37 @@ def update_stat(self, CS, enabled):
     acc_mode = ACCMode.from_label(acc_string)
     CS.cstm_btns.get_button(ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label
     self.autoresume = acc_mode.autoresume
+    self.adaptive = acc_mode.adaptive
     curr_time_ms = _current_time_millis()
     # Handle pressing the enable button.
-    if (CS.cruise_buttons == CruiseButtons.MAIN and
-        self.prev_cruise_buttons != CruiseButtons.MAIN):
+    if (CS.cruise_buttons == CruiseButtons.MAIN  or CS.cruise_buttons == CruiseButtons.DECEL_SET) and self.prev_cruise_buttons != CS.cruise_buttons:
       double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750
       self.last_cruise_stalk_pull_time = curr_time_ms
       ready = (CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) > ACCState.OFF
-               and enabled
+               and (enabled or (CS.cruise_buttons == CruiseButtons.DECEL_SET and not self.adaptive))
                and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
                and CS.v_ego > self.MIN_CRUISE_SPEED_MS)
-      if ready and double_pull:
+      if ready and double_pull and ((self.adaptive and CS.cruise_buttons == CruiseButtons.MAIN) or ((not self.adaptive) and CS.cruise_buttons == CruiseButtons.DECEL_SET)):
         # A double pull enables ACC. updating the max ACC speed if necessary.
         self.enable_adaptive_cruise = True
         # Increase ACC speed to match current, if applicable.
-        self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.speed_limit_kph)
+        if self.adaptive:
+          self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.speed_limit_kph)
+        else:
+          self.acc_speed_kph = CS.v_ego_raw * CV.MS_TO_KPH
         self.user_has_braked = False
         self.has_gone_below_min_speed = False
       else:
         # A single pull disables ACC (falling back to just steering).
-        self.enable_adaptive_cruise = False
+        if CS.cruise_buttons == CruiseButtons.MAIN:
+          self.enable_adaptive_cruise = False
     # Handle pressing the cancel button.
-    elif CS.cruise_buttons == CruiseButtons.CANCEL:
+    if CS.cruise_buttons == CruiseButtons.CANCEL:
       self.enable_adaptive_cruise = False
       self.acc_speed_kph = 0. 
       self.last_cruise_stalk_pull_time = 0
     # Handle pressing up and down buttons.
-    elif (self.enable_adaptive_cruise and
+    elif (CS.cruise_buttons != CruiseButtons.MAIN and self.enable_adaptive_cruise and
           CS.cruise_buttons != self.prev_cruise_buttons):
       self._update_max_acc_speed(CS)
 
@@ -134,9 +141,9 @@ def update_stat(self, CS, enabled):
     if CS.v_ego < self.MIN_CRUISE_SPEED_MS:
       self.has_gone_below_min_speed = True
 
-    # If autoresume is not enabled, manually steering or slowing disables ACC.
+    # If autoresume is not enabled and not in standard CC, disable if we hit the brakes or gone below 18mph
     if not self.autoresume:
-      if not enabled or self.user_has_braked or self.has_gone_below_min_speed:
+      if (self.adaptive and not enabled) or self.user_has_braked or self.has_gone_below_min_speed:
         self.enable_adaptive_cruise = False
 
     # Notify if ACC was toggled
@@ -150,7 +157,7 @@ def update_stat(self, CS, enabled):
 
     # Update the UI to show whether the current car state allows ACC.
     if CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]:
-      if (enabled
+      if ((enabled or not self.adaptive)
           and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
           and CS.v_ego > self.MIN_CRUISE_SPEED_MS):
         CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY)
@@ -211,6 +218,15 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph,
         and (CS.pcm_acc_status == CruiseState.ENABLED)):
       button_to_press = CruiseButtons.CANCEL
 
+    #if non addaptive and we just engaged ACC but pcm is not engaged, then engage
+    if (not self.adaptive) and self.enable_adaptive_cruise and (CS.pcm_acc_status != CruiseState.ENABLED):
+      button_to_press = CruiseButtons.MAIN
+
+    #if plain cc, not adaptive, then just return None or Cancel
+    if not self.adaptive and self.enable_adaptive_cruise:
+      self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH
+      return button_to_press
+
     #disengage if cruise is canceled
     if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and (CS.pcm_acc_status <= 4):
       button_to_press = CruiseButtons.CANCEL 

selfdrive/car/tesla/AHB_module.py

@@ -85,7 +85,7 @@ def update(self, CS, frame,ahbLead1):
       highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
       return self.set_and_return(CS,frame,highLowBeamStatus,highLowBeamReason)
     #if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF
-    if ahbLead1 is not None:
+    if False and ahbLead1 is not None:
       _debug("OP detected car")
       self.time_last_car_detected = tms_now
       highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF

selfdrive/car/tesla/carcontroller.py

@@ -556,12 +556,17 @@ def update(self, enabled, CS, frame, actuators, \
       if (CS.pedal_interceptor_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:
+            cc_state = 3
         CS.speed_control_enabled = 1
       else:
         CS.speed_control_enabled = 0
         if (CS.pcm_acc_status == 4):
           #car CC enabled but not OP, display the HOLD message
           cc_state = 3
+    else:
+      if (CS.pcm_acc_status == 4):
+        cc_state = 3
 
     send_fake_msg = False
     send_fake_warning = False
@@ -593,7 +598,7 @@ 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))
+        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,(not CS.pedal_interceptor_available) and (not self.ACC.adaptive)))
     if send_fake_msg:
       if enable_steer_control and op_status == 3:
         op_status = 0x5
@@ -722,6 +727,8 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS, t
         self.curv1 = 0.0 #straighten the turn for ALCA
         self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps #animas late change on IC
         self.curv0 = clip(self.curv0, -3.5, 3.5)
+        self.lLine = 3
+        self.rLine = 3
       else:
         if self.should_ldw and (CS.enableLdw and (not CS.blinker_on) and (turn_signal_needed == 0)):
           if pp.lProb > LDW_LANE_PROBAB:

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,plainccenabled):
   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) + ((1 if plainccenabled else 0)<<1))
   return [msg_id, 0, msg.raw, 0]