diff --git a/cereal/log.capnp b/cereal/log.capnp index a0f51acf5..bdfa81f30 100644 --- a/cereal/log.capnp +++ b/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; } diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index 9baf6d785..121c3c0b3 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/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; diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index 1fdba014a..4af62a948 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index cfcd5d6c6..9d2e74da3 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/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) diff --git a/opendbc_repo/opendbc/car/tesla/preap/carcontroller.py b/opendbc_repo/opendbc/car/tesla/preap/carcontroller.py index 3ca1865cc..912b4102c 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/carcontroller.py +++ b/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 - diff --git a/opendbc_repo/opendbc/car/tesla/preap/carstate.py b/opendbc_repo/opendbc/car/tesla/preap/carstate.py index da48e3de1..e343d015a 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/carstate.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/preap/stock_cc_spoofer.py b/opendbc_repo/opendbc/car/tesla/preap/stock_cc_spoofer.py index dfa13fc44..b8b328dd6 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/stock_cc_spoofer.py +++ b/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) - diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap.h b/opendbc_repo/opendbc/safety/modes/tesla_preap.h index 081678a08..00219a22f 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla_preap.h +++ b/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) { diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 130235b89..9b0da628d 100644 --- a/selfdrive/car/car_specific.py +++ b/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) diff --git a/selfdrive/car/tests/test_car_specific_tesla_preap.py b/selfdrive/car/tests/test_car_specific_tesla_preap.py new file mode 100644 index 000000000..a894c789a --- /dev/null +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 556b4dc92..f146f5e5a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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, diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 6b66146fe..3a32f9e3e 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/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) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index be1b03b4f..a6355b3a2 100644 --- a/selfdrive/selfdrived/events.py +++ b/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), }, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index d1a99aad4..323744264 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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