work for AP1 cars with AP disabled

Author: BogGyver

panda

@@ -397,9 +397,9 @@ TeslaTogglesPanel::TeslaTogglesPanel(SettingsWindow *parent) : ListWidget(parent
       "",
       0.0,0.0,0.0,0.0,TINKLA_STRING
     },
-    {"TinklaUseAEBevent",
-    "Use AEB Events (Experimental)",
-    "Use AEB Events to control speed on cars with AP disabled. This is an EXPERIMENTAL feature.",
+    {"TinklaAutopilotDisabled",
+    "Autopilot feature disabled",
+    "Use when car has the autopilot feature disabled.",
     "../assets/offroad/icon_settings.png",
     "","","",0.0,0.0,0.0,0.0, TINKLA_TOGGLE
     },

selfdrive/car/modules/qt/tsettings.cc

@@ -232,7 +232,7 @@ def update(self, controls_state, enabled, CS, frame, actuators, cruise_cancel, h
 
         # send DAS_status and DAS_status2 at 2Hz
         if (self.IC_integration_counter %20 == 0) or (self.IC_previous_enabled and not enabled ):
-            should_send = enabled or (self.IC_previous_enabled and not enabled )#not(self.prev_autopilot_enabled and CS.autopilot_enabled)
+            should_send = enabled or (self.IC_previous_enabled and not enabled ) or CS.autopilot_disabled_det 
             self.prev_autopilot_enabled = CS.autopilot_enabled
             if CS.enableICIntegration and should_send:
                 messages.append(self.tesla_can.create_das_status(DAS_op_status, DAS_collision_warning,
@@ -242,7 +242,7 @@ def update(self, controls_state, enabled, CS, frame, actuators, cruise_cancel, h
                 messages.append(self.tesla_can.create_das_status2(DAS_csaState, cruise_speed, 
                     DAS_collision_warning, CAN_CHASSIS[self.CP.carFingerprint], 1))
 
-        if (enabled or CS.autopilot_enabled or self.IC_previous_enabled or self.CP.carFingerprint == CAR.PREAP_MODELS) and (self.IC_integration_counter % 10 == 0):
+        if (enabled or CS.autopilot_disabled_det or self.IC_previous_enabled or self.CP.carFingerprint == CAR.PREAP_MODELS) and (self.IC_integration_counter % 10 == 0):
 
             # send DAS_bodyControls
             if (self.IC_integration_counter in [20,70]) or (self.IC_previous_enabled and not enabled):
@@ -282,9 +282,9 @@ def update(self, controls_state, enabled, CS, frame, actuators, cruise_cancel, h
                         CS.DAS_219_lcTempUnavailableSpeed, CS.DAS_220_lcTempUnavailableRoad, CS.DAS_221_lcAborting, CS.DAS_222_accCameraBlind,
                         CS.DAS_208_rackDetected, CS.DAS_216_driverOverriding, CS.stopSignWarning, CS.stopLightWarning, CAN_CHASSIS[self.CP.carFingerprint]))
 
-            #send message for TB/Panda if preAP
-            if self.CP.carFingerprint == CAR.PREAP_MODELS:
-                if CS.enableICIntegration:
+            #send message for TB/Panda if preAP or AP disabled 
+            if self.CP.carFingerprint == CAR.PREAP_MODELS or CS.autopilot_disabled:
+                if CS.enableICIntegration and self.CP.carFingerprint == CAR.PREAP_MODELS:
                     messages.append(
                         self.tesla_can.create_ap1_long_control(
                             not CS.carNotInDrive, 
@@ -314,10 +314,11 @@ def update(self, controls_state, enabled, CS, frame, actuators, cruise_cancel, h
                         1 if CS.pcc_available else 0, 
                         DAS_alca_state,
                         v_cruise_pcm,
-                        CS.DAS_fusedSpeedLimit,
+                        int(CS.DAS_fusedSpeedLimit),
                         apply_angle,
                         1 if enabled else 0,
                         1 if CS.enablePedal else 0, #is pedal enabled
+                        1 if CS.autopilot_disabled else 0, #autopilot feature disabled
                         CAN_CHASSIS[self.CP.carFingerprint],
                     )
                 )

selfdrive/car/tesla/HUD_module.py

@@ -48,6 +48,7 @@ def __init__(self,CP,packer, tesla_can, pedalcan):
         self.adjustSpeedWithSpeedLimit = load_bool_param("TinklaAdjustAccWithSpeedLimit",True)
         self.adjustSpeedRelative = load_bool_param("TinklaSpeedLimitUseRelative",False)
         self.useLongControlData = load_bool_param("TinklaUseLongControlData",False)
+        self.autopilot_disabled = load_bool_param("TinklaAutopilotDisabled",False)
         average_speed_over_x_suggestions = 5  # 1 second @ 5Hz
         self.speed_limit_uom = 0.
         self.prev_speed_limit_uom = 0.
@@ -60,7 +61,8 @@ def __init__(self,CP,packer, tesla_can, pedalcan):
         self.ap1_adjusting_speed = False
         self.ap1_speed_target = 0.
         self.set_speed_limit_active = False
-        if (CP.carFingerprint == CAR.PREAP_MODELS):
+        self.longPlan = None
+        if (CP.carFingerprint == CAR.PREAP_MODELS) or self.autopilot_disabled:
             self.ACC = ACCController(self)
             self.PCC = PCCController(self,tesla_can,pedalcan,CP )
 
@@ -74,6 +76,19 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
         if my_accel < 0:
             my_accel = my_accel * BRAKE_FACTOR 
         #update options 
+        
+        target_accel = clip(my_accel, TESLA_MIN_ACCEL,TESLA_MAX_ACCEL)
+        if self.CP.carFingerprint == CAR.PREAP_MODELS or CS.autopilot_disabled:
+            target_accel = clip(my_accel * interp(CS.out.vEgo, BRAKE_FACTOR_BP, BRAKE_FACTOR_V ), TESLA_MIN_ACCEL,TESLA_MAX_ACCEL)
+
+        max_accel = 0 if target_accel < 0 else target_accel
+        min_accel = 0 if target_accel > 0 else target_accel
+
+        max_jerk = CarControllerParams.JERK_LIMIT_MAX
+        min_jerk = CarControllerParams.JERK_LIMIT_MIN
+
+        tesla_jerk_limits = [min_jerk,max_jerk]
+        tesla_accel_limits = [min_accel,max_accel]
 
         if enabled and not self.prev_enabled:
             self.fleet_speed.reset_averager()
@@ -103,7 +118,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
         if not self.CP.openpilotLongitudinalControl:
             return messages
         #preAP ModelS
-        if self.CP.carFingerprint == CAR.PREAP_MODELS:
+        if self.CP.carFingerprint == CAR.PREAP_MODELS or CS.autopilot_disabled:
             # update PCC module info
             pedal_can_sends = self.PCC.update_stat(CS, frame)
             if len(pedal_can_sends) > 0:
@@ -183,14 +198,12 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
             if self.PCC.pcc_available and frame % 1 == 0:  # pedal processed at 100Hz, we get speed at 50Hz from ESP_B
                 #following = False
                 #TODO: see what works best for these
-                self.v_target = self.longPlan.speeds[0]
-                if long_plan and long_plan.longitudinalPlan:
-                    self.v_target = long_plan.longitudinalPlan.speeds[0]
-                    self.a_target = long_plan.longitudinalPlan.accels[0]
+                if self.longPlan:
+                    self.v_target = self.longPlan.speeds[0]
+                    self.a_target = self.longPlan.accels[0]
 
                 if self.v_target is None:
                     self.v_target = CS.out.vEgo
-                target_accel = clip(my_accel * interp(CS.out.vEgo, BRAKE_FACTOR_BP, BRAKE_FACTOR_V ), TESLA_MIN_ACCEL,TESLA_MAX_ACCEL)
                 target_speed = max(self.v_target, 0)                    
 
                 apply_accel, self.apply_brake, accel_needed, accel_idx = self.PCC.update_pdl(
@@ -225,6 +238,23 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
                     )
                     self.ibooster_idx = (self.ibooster_idx + 1) % 16
 
+                if self.autopilot_disabled:
+                    target_speed = target_speed * CV.MS_TO_KPH
+                    if enabled:
+                        if self.CP.carFingerprint == CAR.AP1_MODELS:
+                            messages.append(self.tesla_can.create_ap1_long_control(not CS.carNotInDrive, False, CS.pcc_enabled ,target_speed, tesla_accel_limits, tesla_jerk_limits, CAN_POWERTRAIN[self.CP.carFingerprint], self.long_control_counter))
+                    else:
+                        if CS.carNotInDrive:
+                            CS.cc_state = 0
+                        else:
+                            CS.cc_state = 1
+                        #send this values so we can enable at 0 km/h
+                        tesla_accel_limits = [-1.4000000000000004,1.8000000000000007]
+                        tesla_jerk_limits = [-0.46000000000000085,0.47600000000000003]
+                        if self.CP.carFingerprint == CAR.AP1_MODELS:
+                            messages.append(self.tesla_can.create_ap1_long_control(not CS.carNotInDrive, False, False , 0, tesla_accel_limits, tesla_jerk_limits, CAN_POWERTRAIN[self.CP.carFingerprint], self.long_control_counter))
+
+                
         #AP ModelS with OP Long and enabled
         elif enabled and self.CP.openpilotLongitudinalControl and (frame %1 == 0) and (self.CP.carFingerprint in [CAR.AP1_MODELS,CAR.AP2_MODELS]):
             #we use the same logic from planner here to get the speed
@@ -299,9 +329,6 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
                 self.j_target = 8
 
             #following = False
-            #TODO: see what works best for these
-            target_accel = clip(my_accel, TESLA_MIN_ACCEL,TESLA_MAX_ACCEL)
-            #target_jerk = 0.
             target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
 
             #if accel pedal pressed send 0 for target_accel
@@ -316,15 +343,6 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel,pcm_speed,pcm_over
                 if self.fleet_speed_ms < target_speed and self.fleet_speed_ms > 0:
                     target_accel = min(target_accel,FLEET_SPEED_ACCEL)
                     target_speed = min(target_speed,self.fleet_speed_ms,CS.out.cruiseState.speed)
-            
-            max_accel = 0 if target_accel < 0 else target_accel
-            min_accel = 0 if target_accel > 0 else target_accel
-
-            max_jerk = CarControllerParams.JERK_LIMIT_MAX
-            min_jerk = CarControllerParams.JERK_LIMIT_MIN
-
-            tesla_jerk_limits = [min_jerk,max_jerk]
-            tesla_accel_limits = [min_accel,max_accel]
 
             target_speed = target_speed * CV.MS_TO_KPH
 

selfdrive/car/tesla/LONG_module.py

@@ -387,5 +387,5 @@ def _update_pedal_state(self, CS, frame):
             frame < self.pedal_timeout_frame and CS.pedal_interceptor_state == 0
         )
         # Mark pedal unavailable while traditional cruise is on.
-        self.pcc_available = pedal_ready and CS.enablePedal
+        self.pcc_available = (pedal_ready and CS.enablePedal) or (CS.autopilot_disabled)
 

selfdrive/car/tesla/PCC_module.py

@@ -48,7 +48,7 @@ def update(self, c, enabled, CS, frame, actuators, cruise_cancel, pcm_speed, pcm
       CS.autoresumeAcc = load_bool_param("TinklaAutoResumeACC",False)
     can_sends = []
     #add 1 second delay logic to wait for AP which has a status at 2Hz
-    if self.CP.carFingerprint != CAR.PREAP_MODELS:
+    if self.CP.carFingerprint != CAR.PREAP_MODELS and not CS.autopilot_disabled:
       if CS.cruiseEnabled:
         if not self.prevCruiseEnabled:
           self.cruiseDelayFrame = frame

selfdrive/car/tesla/carcontroller.py

@@ -25,7 +25,8 @@ def __init__(self, CP):
     # Needed by carcontroller
     self.msg_stw_actn_req = None
 
-    self.autopilot_disabled = False
+    self.autopilot_disabled = load_bool_param("TinklaAutopilotDisabled",False)
+    self.autopilot_disabled_det = False
 
     self.das_steeringControl_counter = -1
     self.das_status_counter = -1
@@ -108,7 +109,7 @@ def __init__(self, CP):
     self.pcc_enabled = False
     self.torqueLevel = 0.0
     self.cruise_state = None
-    self.enableHumanLongControl = load_bool_param("TinklaForceTeslaPreAP", False)
+    self.enableHumanLongControl = load_bool_param("TinklaForceTeslaPreAP", False) or self.autopilot_disabled
     self.enableICIntegration = load_bool_param("TinklaHasIcIntegration", False)
     self.pedalcanzero = load_bool_param("TinklaPedalCanZero",False)
     self.has_ibooster_ecu = load_bool_param("TinklaHasIBooster",False)
@@ -231,7 +232,6 @@ def update(self, cp, cp_cam):
 
     self.esp_long_acceleration = cp.vl["ESP_ACC"]["Long_Acceleration"]
     self.esp_lat_acceleration = cp.vl["ESP_ACC"]["Lat_Acceleration"]
-    self.autopilot_disabled = (cp.vl["GTW_carConfig"]["GTW_autopilot"] == 0)
 
     #Detect car model - Needs more work when we do 3/Y
     #can look like S/SD/SP/SPD XD/XPD
@@ -244,6 +244,7 @@ def update(self, cp, cp_cam):
       if (self.teslaModelDetected == 0) or (prev_teslaModel != self.teslaModel):
         self.teslaModelDetected = 1
 
+    self.autopilot_disabled_det = (cp.vl["GTW_carConfig"]["GTW_autopilot"] == 0)
 
     # Cruise state
     #cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
@@ -287,7 +288,7 @@ def update(self, cp, cp_cam):
     else:
       ret.followDistanceS = 255
 
-    if (self.CP.carFingerprint != CAR.PREAP_MODELS):
+    if (self.CP.carFingerprint != CAR.PREAP_MODELS) and (not self.autopilot_disabled):
       acc_enabled = (cruise_state in ["ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"])
       self.autopilot_enabled = (autopilot_status in ["ACTIVE_1", "ACTIVE_2"]) #, "ACTIVE_NAVIGATE_ON_AUTOPILOT"])
       cruiseEnabled = acc_enabled and not self.autopilot_enabled and not summon_or_autopark_enabled
@@ -436,6 +437,7 @@ def update(self, cp, cp_cam):
       if gtw_esp1 is not None:
         self.gtw_esp1 = gtw_esp1
         self.gtw_esp1_last_sent_id = self.gtw_esp1["GTW_ESP1Counter"]
+    if self.CP.carFingerprint in [CAR.PREAP_MODELS] or self.autopilot_disabled:
       #Pedal Interceptor
       self.prev_pedal_interceptor_state = self.pedal_interceptor_state
       self.prev_pedal_idx = self.pedal_idx
@@ -451,7 +453,8 @@ def update(self, cp, cp_cam):
           self.pedal_interceptor_value2 = transform_pedal_to_di(cp_cam.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"])
           self.pedal_idx = cp_cam.vl["GAS_SENSOR"]["IDX"]
         ret.gas = self.pedal_interceptor_value/100.
-        ret.gasPressed = (self.pedal_interceptor_value > PEDAL_DI_PRESSED) 
+        ret.gasPressed = (self.pedal_interceptor_value > PEDAL_DI_PRESSED)
+      if self.enablePedal:
         if self.enableHAO and self.pcc_enabled:
           self.DAS_216_driverOverriding = 1 if ret.gasPressed else 0
           ret.gas = 0

selfdrive/car/tesla/carstate.py

@@ -9,7 +9,8 @@
 from selfdrive.car.tesla.values import CarControllerParams, CAN_CHASSIS, CAN_POWERTRAIN
 from selfdrive.car.modules.CFG_module import load_bool_param
 
-USE_AEB_EVENT = load_bool_param("TinklaUseAEBevent",False)
+AUTOPILOT_DISABLED = load_bool_param("TinklaAutopilotDisabled",False)
+ENABLE_AEB_EVENTS = False
 
 class TeslaCAN: 
   def __init__(self, dbc_name, packer, pt_packer):
@@ -137,16 +138,17 @@ def create_steering_control(self, angle, enabled, ldw, bus, counter):
   def create_ap1_long_control(self, in_drive, static_cruise, cruise_enabled, speed, accel_limits, jerk_limits, bus, counter):
     accState = 0
     aeb_event = 0
-    if USE_AEB_EVENT:
-      aeb_event = 1 #AEB ACTIVE
     if in_drive:
       accState = 4
       if static_cruise and cruise_enabled:
         accState = 3
+      if cruise_enabled and AUTOPILOT_DISABLED and ENABLE_AEB_EVENTS:
+        aeb_event = 1 #AEB ACTIVE
+        #accState = 0 #ACC DISABLED
     values = {
       "DAS_setSpeed" :  clip(speed,0,200), #kph
       "DAS_accState" :  accState, # 4-ACC ON, 3-HOLD, 0-CANCEL
-      "DAS_aebEvent" :  aeb_event, # 0 - AEB NOT ACTIVE
+      "DAS_aebEvent" :  aeb_event, # 1 - AEB ACTIVE, 0 - AEB NOT ACTIVE
       "DAS_jerkMin" :  clip(jerk_limits[0],-8.,8.), #m/s^3 -8.67,0
       "DAS_jerkMax" :  clip(jerk_limits[1],-8.,8.), #m/s^3 0,8.67
       "DAS_accelMin" : clip(accel_limits[0],-12.,3.44), #m/s^2 -15,5.44
@@ -305,6 +307,7 @@ def create_fake_DAS_msg(
     apply_angle,
     enable_steer_control,
     pedalEnabled,
+    autopilot_disabled,
     bus,
   ):
     units_included = 1
@@ -332,7 +335,7 @@ def create_fake_DAS_msg(
             acc_speed_limit + 0.5
         ),  # IC rounds current speed, so we need to round cruise speed the same way
         int(
-            (legal_speed_limit & 0x1F) + ((pedalEnabled << 5) & 0x20)
+            (legal_speed_limit & 0x1F) + ((pedalEnabled << 5) & 0x20) + ((autopilot_disabled << 7) & 0x80)
         ),  # positions 7 not used yet, 6 determines extended or not CC_state
         int(c_apply_steer & 0xFF),
         int((c_apply_steer >> 8) & 0xFF)