teslurrrr

cereal/log.capnp

@@ -131,6 +131,13 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     excessiveActuation @96;
     audioFeedback @97;
     lateralManeuver @98;
+    pedalCruiseEnabled @99;
+    pedalCruiseDisabled @100;
+    pedalMaxRegen @101;
+    teslaCCEngaged @102;
+    teslaCCDisengaged @103;
+    teslaCCNotArmed @104;
+    pedalNotCalibrated @105;
 
     soundsUnavailableDEPRECATED @47;
   }

opendbc_repo/opendbc/car/car.capnp

@@ -205,6 +205,11 @@ struct CarState {
   vehicleSensorsInvalid @52 :Bool;  # invalid steering angle readings, etc.
   lowSpeedAlert @56 :Bool;  # lost steering control due to a dynamic min steering speed
   blockPcmEnable @60 :Bool;  # whether to allow PCM to enable this frame
+  pedalMaxRegen @61 :Bool;  # pedal at max regen, driver should use brake for more decel
+  pedalLongActive @62 :Bool;  # Pre-AP pedal longitudinal mode is active (enableLongControl)
+  teslaCCEngaged @63 :Bool;  # rising edge of stock Tesla CC engaging (no-pedal mode)
+  teslaCCDisengaged @64 :Bool;  # falling edge of stock Tesla CC
+  teslaCCNotArmed @65 :Bool;  # lateral engaged but DI_cruiseState != STANDBY/ENABLED
 
   # cruise state
   cruiseState @10 :CruiseState;

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -124,11 +124,14 @@ class CarController(CarControllerBase):
       can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_last, lat_active))
       can_sends.append(self.tesla_can.create_epas_control(cntr, 1))
 
+    CS.pccEvent = None
     if self.CP.openpilotLongitudinalControl and self.preap_long is not None:
       can_sends.extend(self.preap_long.update(CC, CS, self.frame, self.tesla_can, CANBUS.party))
 
     if self.stock_cc is not None:
       can_sends.extend(self.stock_cc.update(CS, self.frame, self.tesla_can, CANBUS.party))
+      if self.stock_cc.pcc_event:
+        CS.pccEvent = self.stock_cc.pcc_event
 
     new_actuators = actuators.as_builder()
     new_actuators.steeringAngleDeg = self.apply_angle_last

opendbc_repo/opendbc/car/tesla/carstate.py

@@ -37,6 +37,8 @@ class CarState(CarStateBase):
       self.enableLongControl = False
       self.enableJustCC = False
       self.pedal_speed_kph = 0.0
+      self.prev_stalk_follow = 0
+      self.pccEvent = None
       self.preap_cc_cancel_needed = False
       self.preap_cc_engage_needed = False
       self.di_cruise_state = "OFF"
@@ -53,6 +55,11 @@ class CarState(CarStateBase):
     self.autopark_prev = autopark_now
     self.cruise_enabled_prev = cruise_enabled
 
+  def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return False
+    return super().update_button_enable(buttonEvents)
+
   def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
     if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
       return update_preap(self, can_parsers)

opendbc_repo/opendbc/car/tesla/preap/carcontroller.py

@@ -69,6 +69,8 @@ class PreAPLongController:
               freeze_integrator=in_engage_grace, orientation_ned=list(CC.orientationNED),
             )
             can_sends.append(tesla_can.create_pedal_command(nap_conf.di_to_pedal(self.prev_pedal_di), enable=1))
+            if self.prev_pedal_di <= 0.95 * PEDAL_DI_MIN and not in_engage_grace:
+              CS.pccEvent = "pedalMaxRegen"
           else:
             zero_torque_di = get_zero_torque().get(CS.out.vEgo)
             self.prev_pedal_di = zero_torque_di
@@ -87,4 +89,3 @@ class PreAPLongController:
   @staticmethod
   def _pedal_ready() -> bool:
     return nap_conf.pedal_calibrated and abs(nap_conf.pedal_factor) > 1e-6
-

opendbc_repo/opendbc/car/tesla/preap/carstate.py

@@ -6,11 +6,18 @@ from cereal import custom
 from opendbc.can import CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.tesla.preap.nap_params import NAPParamKeys
 from opendbc.car.tesla.preap.engagement import PreAPEngagement
 from opendbc.car.tesla.preap.nap_conf import PEDAL_DI_PRESSED, nap_conf
 from opendbc.car.tesla.preap.pedal_feedback import PedalFeedback
 from opendbc.car.tesla.values import CANBUS, DBC, GEAR_MAP, STEER_THRESHOLD
 
+try:
+  from openpilot.common.params import Params as _NAPParams
+  _nap_params = _NAPParams()
+except ImportError:
+  _nap_params = None
+
 _DOORS = ("DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE")
 
 
@@ -83,6 +90,14 @@ def update_preap(cs, can_parsers):
   cs.cruise_buttons = int(cp_chassis.vl["STW_ACTN_RQ"]["SpdCtrlLvr_Stat"])
   cs.msg_stw_actn_req = copy.copy(cp_chassis.vl["STW_ACTN_RQ"])
 
+  if _nap_params is not None:
+    dtr_dist = int(cp_chassis.vl["STW_ACTN_RQ"]["DTR_Dist_Rq"])
+    if dtr_dist != 255:
+      stalk_follow = min((dtr_dist // 33) + 1, 7)
+      if stalk_follow != cs.prev_stalk_follow:
+        _nap_params.put(NAPParamKeys.FOLLOW_DISTANCE, str(stalk_follow))
+        cs.prev_stalk_follow = stalk_follow
+
   curr_time_ms = _current_time_millis()
   ret.buttonEvents = cs.engagement.process_buttons(
     cs.cruise_buttons, cs.prev_cruise_buttons, curr_time_ms, ret.vEgo, cs.speed_units,
@@ -109,7 +124,18 @@ def update_preap(cs, can_parsers):
     if use_pedal:
       ret.gasPressed = cs.pedal.gas_pressed
 
+  cs.das_control = None
   cs.cruise_enabled_prev = ret.cruiseState.enabled
+  ret.pedalMaxRegen = cs.pccEvent == "pedalMaxRegen"
+  ret.teslaCCEngaged = cs.pccEvent == "teslaCCEngaged"
+  ret.teslaCCDisengaged = cs.pccEvent == "teslaCCDisengaged"
+  ret.teslaCCNotArmed = (
+    not nap_conf.use_pedal and
+    cs.cruiseEnabled and
+    cs.enableLongControl and
+    cs.di_cruise_state not in ("STANDBY", "ENABLED")
+  )
+  ret.pedalLongActive = cs.enableLongControl and nap_conf.use_pedal
   return ret, fp_ret
 
 

opendbc_repo/opendbc/car/tesla/preap/stock_cc_spoofer.py

@@ -12,6 +12,8 @@ class StockCCSpoofer:
     self.cc_engage_start_frame = 0
     self.cancel_pending = False
     self.cancel_frame = -1_000_000
+    self.prev_di_cc_engaged = False
+    self.pcc_event = None
 
   def update(self, CS, frame: int, tesla_can, can_bus_party: int):
     can_sends = []
@@ -40,6 +42,15 @@ class StockCCSpoofer:
         if sent is not None:
           can_sends.append(sent)
 
+    di_cc_engaged = getattr(CS, "di_cruise_state", "OFF") == "ENABLED"
+    if di_cc_engaged and not self.prev_di_cc_engaged:
+      self.pcc_event = "teslaCCEngaged"
+    elif not di_cc_engaged and self.prev_di_cc_engaged:
+      self.pcc_event = "teslaCCDisengaged"
+    else:
+      self.pcc_event = None
+    self.prev_di_cc_engaged = di_cc_engaged
+
     return can_sends
 
   @staticmethod
@@ -49,4 +60,3 @@ class StockCCSpoofer:
       return None
     counter = (int(msg_stw.get("MC_STW_ACTN_RQ", 0)) + 1) % 16
     return tesla_can.create_action_request(button, can_bus_party, counter, msg_stw)
-

opendbc_repo/opendbc/safety/modes/tesla_preap.h

@@ -370,7 +370,7 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) {
 static bool tesla_preap_fwd_hook(int bus_num, int addr) {
   (void)bus_num;
   (void)addr;
-  return false;
+  return true;
 }
 
 static safety_config tesla_preap_init(uint16_t param) {

selfdrive/car/car_specific.py

@@ -173,6 +173,22 @@ class CarSpecificEvents:
       # if CC.eps_timer_soft_disable_alert:  # type: ignore[attr-defined]
       #   events.add(EventName.steerTimeLimit)
 
+    elif self.CP.brand == 'tesla':
+      events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears)
+
+      if self.CP.carFingerprint == "TESLA_MODEL_S_PREAP":
+        if self.CP.pcmCruise:
+          if getattr(CS, 'teslaCCEngaged', False):
+            events.add(EventName.teslaCCEngaged)
+          if getattr(CS, 'teslaCCDisengaged', False):
+            events.add(EventName.teslaCCDisengaged)
+          if getattr(CS, 'teslaCCNotArmed', False):
+            events.add(EventName.teslaCCNotArmed)
+        else:
+          from opendbc.car.tesla.preap.nap_conf import nap_conf
+          if not nap_conf.pedal_calibrated:
+            events.add(EventName.pedalNotCalibrated)
+
     elif self.CP.brand == 'hyundai':
       events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise, allow_button_cancel=False)
 

selfdrive/car/tests/test_car_specific_tesla_preap.py

@@ -0,0 +1,57 @@
+from unittest.mock import PropertyMock, patch  # noqa: TID251
+
+from cereal import car, log
+
+from openpilot.selfdrive.car.car_specific import CarSpecificEvents
+
+
+EventName = log.OnroadEvent.EventName
+NAP_CONF_PATH = "opendbc.car.tesla.preap.nap_conf.NAPConf.pedal_calibrated"
+
+
+def _make_cp(*, fingerprint="TESLA_MODEL_S_PREAP", brand="tesla", pcm_cruise=False, op_long=True):
+  cp = car.CarParams.new_message()
+  cp.carFingerprint = fingerprint
+  cp.brand = brand
+  cp.pcmCruise = pcm_cruise
+  cp.openpilotLongitudinalControl = op_long
+  return cp
+
+
+def _make_cs():
+  cs = car.CarState.new_message()
+  cs.cruiseState.available = True
+  cs.gearShifter = "drive"
+  return cs
+
+
+def _run(cp, calibrated):
+  with patch(NAP_CONF_PATH, new_callable=PropertyMock, return_value=calibrated):
+    ce = CarSpecificEvents(cp)
+    events = ce.update(_make_cs(), _make_cs(), car.CarControl.new_message())
+  return events.names
+
+
+def test_fires_in_preap_pedal_mode_when_uncalibrated():
+  cp = _make_cp(pcm_cruise=False, op_long=True)
+  assert EventName.pedalNotCalibrated in _run(cp, calibrated=False)
+
+
+def test_silent_in_preap_pedal_mode_when_calibrated():
+  cp = _make_cp(pcm_cruise=False, op_long=True)
+  assert EventName.pedalNotCalibrated not in _run(cp, calibrated=True)
+
+
+def test_silent_in_preap_nopedal_mode_even_when_uncalibrated():
+  cp = _make_cp(pcm_cruise=True, op_long=False)
+  assert EventName.pedalNotCalibrated not in _run(cp, calibrated=False)
+
+
+def test_silent_on_non_preap_tesla():
+  cp = _make_cp(fingerprint="TESLA_MODEL_S", pcm_cruise=False, op_long=True)
+  assert EventName.pedalNotCalibrated not in _run(cp, calibrated=False)
+
+
+def test_silent_on_non_tesla_brand():
+  cp = _make_cp(fingerprint="HONDA_CIVIC_2022", brand="honda", pcm_cruise=True, op_long=False)
+  assert EventName.pedalNotCalibrated not in _run(cp, calibrated=False)

selfdrive/controls/lib/latcontrol_torque.py

@@ -49,9 +49,9 @@ MIN_LATERAL_CONTROL_SPEED = 0.3
 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
 CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
-CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.26
+CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.32
 CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0
-CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.10
+CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08
 CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18
 CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07
 CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.00
@@ -76,16 +76,16 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
-CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.24
-CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.34
+CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.26
+CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.38
 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.06
 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.06
-CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 1.18
-CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 1.48
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 1.55
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 1.95
 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.03
 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
-CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.88
-CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 1.20
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 1.05
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 1.40
 
 BOLT_2022_2023_CARS = (
   GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,

selfdrive/controls/tests/test_latcontrol.py

@@ -441,7 +441,7 @@ class TestLatControl:
     monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)
     variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
 
-    assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.26)
+    assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.32)
 
   def test_modified_civic_b_torque_ff_scale_curve(self):
     steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0)

selfdrive/selfdrived/events.py

@@ -920,6 +920,11 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
   },
 
+  EventName.pedalNotCalibrated: {
+    ET.PERMANENT: NormalPermanentAlert("Pedal Not Calibrated", "Check Calibration"),
+    ET.NO_ENTRY: NoEntryAlert("Pedal Not Calibrated: Check Calibration"),
+  },
+
   EventName.calibrationRecalibrating: {
     ET.PERMANENT: calibration_incomplete_alert,
     ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"),
@@ -1120,6 +1125,50 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: personality_changed_alert,
   },
 
+  EventName.pedalCruiseEnabled: {
+    ET.WARNING: Alert(
+      "Pedal Cruise Engaged",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.engage, 0.8),
+  },
+
+  EventName.pedalCruiseDisabled: {
+    ET.WARNING: Alert(
+      "Pedal Cruise Disengaged",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 0.8),
+  },
+
+  EventName.pedalMaxRegen: {
+    ET.WARNING: Alert(
+      "Max Regen Being Used",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.prompt, 2.),
+  },
+
+  EventName.teslaCCEngaged: {
+    ET.WARNING: Alert(
+      "Tesla Cruise Engaged",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.engage, 0.8),
+  },
+
+  EventName.teslaCCDisengaged: {
+    ET.WARNING: Alert(
+      "Tesla Cruise Disengaged",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 0.8),
+  },
+
+  EventName.teslaCCNotArmed: {
+    ET.PERMANENT: NormalPermanentAlert("Arm Stock Cruise to Enable Speed Control"),
+  },
+
   EventName.userBookmark: {
     ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5),
   },

selfdrive/selfdrived/selfdrived.py

@@ -133,6 +133,7 @@ class SelfdriveD:
     self.recalibrating_seen = False
     self.state_machine = StateMachine()
     self.rk = Ratekeeper(100, print_delay_threshold=None)
+    self.prev_pedal_long_active = False
 
     # Determine startup event
     self.startup_event = StarPilotEventName.customStartupAlert
@@ -241,6 +242,22 @@ class SelfdriveD:
           self.last_below_steer_speed_alert_time = now
       self.events.add_from_msg(car_events)
 
+      if (self.CP.brand == "tesla"
+          and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP"
+          and self.CP.openpilotLongitudinalControl
+          and not self.CP.pcmCruise):
+        pedal_long_active = bool(CS.cruiseState.enabled and getattr(CS, 'pedalLongActive', False))
+        if pedal_long_active and not self.prev_pedal_long_active:
+          self.events.add(EventName.pedalCruiseEnabled)
+        elif self.prev_pedal_long_active and not pedal_long_active:
+          self.events.add(EventName.pedalCruiseDisabled)
+        self.prev_pedal_long_active = pedal_long_active
+
+        if getattr(CS, 'pedalMaxRegen', False):
+          self.events.add(EventName.pedalMaxRegen)
+      else:
+        self.prev_pedal_long_active = False
+
       if (getattr(self.starpilot_toggles, "nostalgia_mode", False) and
           self.CP.openpilotLongitudinalControl and
           self.enabled and