diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 54b825e59..71e0d3145 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -91,6 +91,13 @@ struct StarPilotCarState @0xf35cc4560bbf6ec2 { cancelPressed @20 :Bool; cancelLongPressed @21 :Bool; cancelVeryLongPressed @22 :Bool; + pedalMaxRegen @23 :Bool; # pedal at max regen, driver should use brake for more decel + pedalLongActive @24 :Bool; # Pre-AP pedal longitudinal mode is active (enableLongControl) + teslaCCEngaged @25 :Bool; # rising edge of stock Tesla CC engaging (no-pedal mode) + teslaCCDisengaged @26 :Bool; # falling edge of stock Tesla CC + teslaCCNotArmed @27 :Bool; # lateral engaged but DI_cruiseState != STANDBY/ENABLED + accelHardCruise @28 :Bool; # current/releasing accel cruise button came from GM hard-press signal + decelHardCruise @29 :Bool; # current/releasing decel cruise button came from GM hard-press signal } struct StarPilotDeviceState @0xda96579883444c35 { @@ -108,7 +115,11 @@ struct StarPilotModelDataV2 @0x80ae746ee2596b11 { } } -struct StarPilotOnroadEvent @0xa5cd762cd951a455 { +struct StarPilotOnroadEvents @0xa5cd762cd951a455 { + events @0 :List(StarPilotOnroadEvent); +} + +struct StarPilotOnroadEvent @0xe344718567f9ce71 { name @0 :EventName; enable @1 :Bool; @@ -158,6 +169,14 @@ struct StarPilotOnroadEvent @0xa5cd762cd951a455 { switchbackModeInactive @30; lkasEnable @31; lkasDisable @32; + lateralManeuver @33; + pedalCruiseEnabled @34; + pedalCruiseDisabled @35; + pedalMaxRegen @36; + teslaCCEngaged @37; + teslaCCDisengaged @38; + teslaCCNotArmed @39; + pedalNotCalibrated @40; } } @@ -259,7 +278,8 @@ struct CustomReserved9 @0xa1680744031fdb2d { wallTimeNanos @5 :UInt64; } -struct CustomReserved10 @0xcb9fd56c7057593a { +struct LateralManeuverPlan @0xcb9fd56c7057593a { + desiredCurvature @0 :Float32; # 1/m } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 48e3a6784..8414d6284 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -130,14 +130,6 @@ struct OnroadEvent @0xc4fa6047f024e718 { userBookmark @95; excessiveActuation @96; audioFeedback @97; - lateralManeuver @98; - pedalCruiseEnabled @99; - pedalCruiseDisabled @100; - pedalMaxRegen @101; - teslaCCEngaged @102; - teslaCCDisengaged @103; - teslaCCNotArmed @104; - pedalNotCalibrated @105; soundsUnavailableDEPRECATED @47; } @@ -1095,7 +1087,7 @@ struct ModelDataV2 { confidence @23: ConfidenceClass; # Model perceived motion - temporalPose @21 :Pose; + temporalPoseDEPRECATED @21 :Pose; # e2e lateral planner action @26: Action; @@ -1246,10 +1238,6 @@ struct DriverAssistance { # FCW, AEB, etc. will go here } -struct LateralManeuverPlan { - desiredCurvature @0 :Float32; # 1/m -} - struct LongitudinalPlan @0xe00b5b3eba12876c { modelMonoTime @9 :UInt64; hasLead @7 :Bool; @@ -2179,7 +2167,6 @@ struct DriverStateV2 { rightBlinkProb @8 :Float32; sunglassesProb @9 :Float32; phoneProb @13 :Float32; - sleepProb @14 :Float32; notReadyProbDEPRECATED @12 :List(Float32); occludedProbDEPRECATED @10 :Float32; readyProbDEPRECATED @11 :List(Float32); @@ -2614,7 +2601,6 @@ struct Event { userBookmark @93 :UserBookmark; bookmarkButton @148 :UserBookmark; audioFeedback @149 :AudioFeedback; - lateralManeuverPlan @150 :LateralManeuverPlan; # *********** debug *********** testJoystick @52 :Joystick; @@ -2644,12 +2630,12 @@ struct Event { starpilotCarState @109 :Custom.StarPilotCarState; starpilotDeviceState @110 :Custom.StarPilotDeviceState; starpilotModelV2 @111 :Custom.StarPilotModelDataV2; - starpilotOnroadEvents @112 :List(Custom.StarPilotOnroadEvent); + starpilotOnroadEvents @112 :Custom.StarPilotOnroadEvents; starpilotPlan @113 :Custom.StarPilotPlan; starpilotRadarState @114 :Custom.StarPilotRadarState; starpilotSelfdriveState @115 :Custom.StarPilotSelfdriveState; customReserved9 @116 :Custom.CustomReserved9; - customReserved10 @136 :Custom.CustomReserved10; + lateralManeuverPlan @136 :Custom.LateralManeuverPlan; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/opendbc_repo/opendbc/car/__init__.py b/opendbc_repo/opendbc/car/__init__.py index 6c5ec477d..ce9bd1a22 100644 --- a/opendbc_repo/opendbc/car/__init__.py +++ b/opendbc_repo/opendbc/car/__init__.py @@ -106,7 +106,7 @@ def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): return CanData(addr, bytes(dat), bus) -def get_safety_config(safety_model: structs.CarParams.SafetyModel, safety_param: int = None) -> structs.CarParams.SafetyConfig: +def get_safety_config(safety_model, safety_param: int = None) -> structs.CarParams.SafetyConfig: ret = structs.CarParams.SafetyConfig() ret.safetyModel = safety_model if safety_param is not None: diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index e859aa006..d2b8d758a 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -205,11 +205,6 @@ 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; @@ -285,8 +280,6 @@ struct CarState { setCruise @9; resumeCruise @10; gapAdjustCruise @11; - accelHardCruise @12; - decelHardCruise @13; } } @@ -642,7 +635,6 @@ struct CarParams { fcaGiorgio @32; rivian @33; volkswagenMeb @34; - teslaPreap @35; } enum SteerControlType { diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 843b7746b..e53013af7 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -33,7 +33,7 @@ AUTO_HOLD_REGEN_RELEASE_COOLDOWN_S = 1.0 BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel} -HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelHardCruise, CruiseButtons.DECEL_SET: ButtonType.decelHardCruise} +HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise} NORMAL_CRUISE_BUTTONS = (CruiseButtons.RES_ACCEL, CruiseButtons.DECEL_SET) @@ -96,8 +96,8 @@ class CarState(CarStateBase): if not self.CP.pcmCruise: for b in buttonEvents: # The ECM allows enabling on falling edge of set, but only rising edge of resume - if (b.type in (ButtonType.accelCruise, ButtonType.accelHardCruise) and b.pressed) or \ - (b.type in (ButtonType.decelCruise, ButtonType.decelHardCruise) and not b.pressed): + if (b.type == ButtonType.accelCruise and b.pressed) or \ + (b.type == ButtonType.decelCruise and not b.pressed): return True return False @@ -410,6 +410,8 @@ class CarState(CarStateBase): ret.lowSpeedAlert = True fp_ret = custom.StarPilotCarState.new_message() + fp_ret.accelHardCruise = self.hard_cruise_buttons == CruiseButtons.RES_ACCEL or prev_hard_cruise_buttons == CruiseButtons.RES_ACCEL + fp_ret.decelHardCruise = self.hard_cruise_buttons == CruiseButtons.DECEL_SET or prev_hard_cruise_buttons == CruiseButtons.DECEL_SET if bolt_cancel_button and self.cruise_buttons == CruiseButtons.CANCEL: fp_ret.cancelPressed = True fp_ret.sportGear = pt_cp.vl["SportMode"]["SportMode"] == 1 diff --git a/opendbc_repo/opendbc/car/tesla/preap/carstate.py b/opendbc_repo/opendbc/car/tesla/preap/carstate.py index e343d015a..25df8296c 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/preap/carstate.py @@ -126,16 +126,16 @@ def update_preap(cs, can_parsers): 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 = ( + fp_ret.pedalMaxRegen = cs.pccEvent == "pedalMaxRegen" + fp_ret.teslaCCEngaged = cs.pccEvent == "teslaCCEngaged" + fp_ret.teslaCCDisengaged = cs.pccEvent == "teslaCCDisengaged" + fp_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 + fp_ret.pedalLongActive = cs.enableLongControl and nap_conf.use_pedal return ret, fp_ret diff --git a/opendbc_repo/opendbc/car/tesla/preap/interface.py b/opendbc_repo/opendbc/car/tesla/preap/interface.py index 39d21fab3..f3210838f 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/interface.py +++ b/opendbc_repo/opendbc/car/tesla/preap/interface.py @@ -7,6 +7,7 @@ from opendbc.car.tesla.preap.nap_conf import nap_conf PREAP_FLAG_ENABLE_PEDAL = 1 PREAP_FLAG_RADAR_EMULATION = 2 PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 +SAFETY_TESLA_PREAP = 35 def get_preap_accel_limits(current_speed: float) -> tuple[float, float]: @@ -29,7 +30,7 @@ def get_preap_params(ret: structs.CarParams) -> structs.CarParams: safety_flags |= PREAP_FLAG_RADAR_BEHIND_NOSECONE use_pedal = nap_conf.use_pedal and nap_conf.pedal_calibrated - ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.teslaPreap, safety_flags)] + ret.safetyConfigs = [get_safety_config(SAFETY_TESLA_PREAP, safety_flags)] ret.radarUnavailable = not nap_conf.radar_enabled ret.steerControlType = structs.CarParams.SteerControlType.angle ret.openpilotLongitudinalControl = use_pedal diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py index 18fb40fb2..217083728 100644 --- a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import unittest -from opendbc.car.structs import CarParams +from opendbc.car.tesla.preap.interface import SAFETY_TESLA_PREAP from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common @@ -14,7 +14,7 @@ class TestTeslaPreAPSafety(common.SafetyTestBase): self._set_mode(0) def _set_mode(self, param: int) -> None: - self.safety.set_safety_hooks(CarParams.SafetyModel.teslaPreap, param) + self.safety.set_safety_hooks(SAFETY_TESLA_PREAP, param) self.safety.init_tests() @staticmethod diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 2f5d590df..a5b40040d 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -177,19 +177,6 @@ class CarSpecificEvents: 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/card.py b/selfdrive/car/card.py index 7c258688e..3a4213f9a 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -236,6 +236,7 @@ class Car: self.is_metric, self.sm['starpilotPlan'].speedLimitChanged, self.starpilot_toggles, + FPCS, ) else: preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH) @@ -267,9 +268,9 @@ class Car: CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) - if any(be.type in (ButtonType.accelCruise, ButtonType.accelHardCruise, ButtonType.resumeCruise) for be in CS.buttonEvents): + if any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents): self.resume_prev_button = True - elif any(be.type in (ButtonType.decelCruise, ButtonType.decelHardCruise, ButtonType.setCruise) for be in CS.buttonEvents): + elif any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents): self.resume_prev_button = False FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 7e81302e3..ed036b107 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -24,17 +24,12 @@ CRUISE_LONG_PRESS = 50 CRUISE_NEAREST_FUNC = { ButtonType.accelCruise: math.ceil, ButtonType.decelCruise: math.floor, - ButtonType.accelHardCruise: math.ceil, - ButtonType.decelHardCruise: math.floor, } CRUISE_INTERVAL_SIGN = { ButtonType.accelCruise: +1, ButtonType.decelCruise: -1, - ButtonType.accelHardCruise: +1, - ButtonType.decelHardCruise: -1, } -HARD_CRUISE_BUTTONS = (ButtonType.accelHardCruise, ButtonType.decelHardCruise) -ACCEL_CRUISE_BUTTONS = (ButtonType.accelCruise, ButtonType.accelHardCruise) +ACCEL_CRUISE_BUTTONS = (ButtonType.accelCruise,) class VCruiseHelper: @@ -46,9 +41,8 @@ class VCruiseHelper: self.button_timers = { ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, - ButtonType.decelHardCruise: 0, - ButtonType.accelHardCruise: 0, } + self.button_hard_states = dict.fromkeys(self.button_timers, False) self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} self.gm_cc_only = self.CP.carFingerprint in CC_ONLY_CAR and self.CP.flags & GMFlags.CC_LONG.value @@ -77,15 +71,25 @@ class VCruiseHelper: def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles): + @staticmethod + def _is_hard_cruise_button(button_type, starpilot_car_state) -> bool: + if starpilot_car_state is None: + return False + if button_type == ButtonType.accelCruise: + return bool(getattr(starpilot_car_state, "accelHardCruise", False)) + if button_type == ButtonType.decelCruise: + return bool(getattr(starpilot_car_state, "decelHardCruise", False)) + return False + + def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None): self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state) self.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS, enabled) + self.update_button_timers(CS, enabled, starpilot_car_state) else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH @@ -99,7 +103,7 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition if not enabled: @@ -107,6 +111,7 @@ class VCruiseHelper: long_press = False button_type = None + button_is_hard = False v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT @@ -115,12 +120,14 @@ class VCruiseHelper: if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: return # end long press button_type = b.type.raw + button_is_hard = self.button_hard_states.get(button_type, False) or self._is_hard_cruise_button(button_type, starpilot_car_state) break else: for k, timer in self.button_timers.items(): if timer and timer % CRUISE_LONG_PRESS == 0: button_type = k long_press = True + button_is_hard = self.button_hard_states.get(button_type, False) break if button_type is None: @@ -140,7 +147,7 @@ class VCruiseHelper: return short_interval, long_interval = self._get_cruise_delta_intervals(starpilot_toggles) - v_cruise_delta_interval = long_interval if long_press or button_type in HARD_CRUISE_BUTTONS else short_interval + v_cruise_delta_interval = long_interval if long_press or button_is_hard else short_interval v_cruise_delta = v_cruise_delta * v_cruise_delta_interval if v_cruise_delta_interval % 5 == 0 and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta @@ -148,12 +155,12 @@ class VCruiseHelper: self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] # If set is pressed while overriding, clip cruise speed to minimum of vEgo - if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.decelHardCruise, ButtonType.setCruise): + if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) - def update_button_timers(self, CS, enabled): + def update_button_timers(self, CS, enabled, starpilot_car_state=None): # increment timer for buttons still pressed for k in self.button_timers: if self.button_timers[k] > 0: @@ -162,8 +169,10 @@ class VCruiseHelper: for b in CS.buttonEvents: if b.type.raw in self.button_timers: # Start/end timer and store current state on change of button pressed - self.button_timers[b.type.raw] = 1 if b.pressed else 0 - self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} + button_type = b.type.raw + self.button_timers[button_type] = 1 if b.pressed else 0 + self.button_change_states[button_type] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} + self.button_hard_states[button_type] = self._is_hard_cruise_button(button_type, starpilot_car_state) if b.pressed else False def initialize_v_cruise(self, CS, experimental_mode: bool, resume_prev_button: bool, starpilot_toggles: SimpleNamespace, desired_speed_limit: float = 0.0) -> None: @@ -172,7 +181,7 @@ class VCruiseHelper: return engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH) - resume_pressed = any(b.type in (ButtonType.accelCruise, ButtonType.accelHardCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) + resume_pressed = any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) remembered_resume = resume_prev_button and (self.gm_cc_only or self.redneck_non_pcm) if self.v_cruise_initialized and (resume_pressed or remembered_resume): diff --git a/selfdrive/car/tests/test_car_specific_tesla_preap.py b/selfdrive/car/tests/test_car_specific_tesla_preap.py index a894c789a..fe795f68a 100644 --- a/selfdrive/car/tests/test_car_specific_tesla_preap.py +++ b/selfdrive/car/tests/test_car_specific_tesla_preap.py @@ -1,11 +1,12 @@ from unittest.mock import PropertyMock, patch # noqa: TID251 -from cereal import car, log +from cereal import car, custom -from openpilot.selfdrive.car.car_specific import CarSpecificEvents +from openpilot.selfdrive.selfdrived.events import Events +from openpilot.selfdrive.selfdrived.selfdrived import add_tesla_preap_starpilot_events -EventName = log.OnroadEvent.EventName +StarPilotEventName = custom.StarPilotOnroadEvent.EventName NAP_CONF_PATH = "opendbc.car.tesla.preap.nap_conf.NAPConf.pedal_calibrated" @@ -27,31 +28,31 @@ def _make_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()) + events = Events(starpilot=True) + add_tesla_preap_starpilot_events(cp, _make_cs(), custom.StarPilotCarState.new_message(), events, False) 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) + assert StarPilotEventName.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) + assert StarPilotEventName.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) + assert StarPilotEventName.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) + assert StarPilotEventName.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) + assert StarPilotEventName.pedalNotCalibrated not in _run(cp, calibrated=False) diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 0b5a5e05a..10b385fac 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -107,23 +107,26 @@ class TestVCruiseHelper: initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph pressed_cs = car.CarState(cruiseState={"available": True}) - pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelHardCruise, pressed=True)] + pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)] + starpilot_car_state = SimpleNamespace(accelHardCruise=True, decelHardCruise=False) self.v_cruise_helper.update_v_cruise( pressed_cs, enabled=True, is_metric=False, speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles, + starpilot_car_state=starpilot_car_state, ) released_cs = car.CarState(cruiseState={"available": True}) - released_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelHardCruise, pressed=False)] + released_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)] self.v_cruise_helper.update_v_cruise( released_cs, enabled=True, is_metric=False, speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles, + starpilot_car_state=starpilot_car_state, ) hard_interval = self.starpilot_toggles.cruise_increase_long * IMPERIAL_INCREMENT @@ -135,23 +138,26 @@ class TestVCruiseHelper: initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph pressed_cs = car.CarState(cruiseState={"available": True}) - pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.decelHardCruise, pressed=True)] + pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=True)] + starpilot_car_state = SimpleNamespace(accelHardCruise=False, decelHardCruise=True) self.v_cruise_helper.update_v_cruise( pressed_cs, enabled=True, is_metric=False, speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles, + starpilot_car_state=starpilot_car_state, ) released_cs = car.CarState(cruiseState={"available": True}) - released_cs.buttonEvents = [ButtonEvent(type=ButtonType.decelHardCruise, pressed=False)] + released_cs.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] self.v_cruise_helper.update_v_cruise( released_cs, enabled=True, is_metric=False, speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles, + starpilot_car_state=starpilot_car_state, ) hard_interval = self.starpilot_toggles.cruise_increase_long * IMPERIAL_INCREMENT diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5c22b3fad..b7e6f6cbd 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -626,10 +626,14 @@ class LongitudinalPlanner: @staticmethod def get_model_speed_error(model_msg, v_ego): try: - if len(model_msg.temporalPose.trans): - return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0)) + temporal_pose = model_msg.temporalPoseDEPRECATED except AttributeError: - pass + try: + temporal_pose = model_msg.temporalPose + except AttributeError: + return 0.0 + if len(temporal_pose.trans): + return float(np.clip(temporal_pose.trans[0] - v_ego, -5.0, 5.0)) return 0.0 @staticmethod diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index aadee1528..413c3b6f4 100644 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -114,7 +114,10 @@ def fill_driver_data(msg, model_output, suffix): msg.rightBlinkProb = model_output[f"right_blink_prob_{suffix}"][0, 0].item() msg.sunglassesProb = model_output[f"sunglasses_prob_{suffix}"][0, 0].item() msg.phoneProb = model_output[f"using_phone_prob_{suffix}"][0, 0].item() - msg.sleepProb = model_output[f"sleep_prob_{suffix}"][0, 0].item() + try: + msg.sleepProb = model_output[f"sleep_prob_{suffix}"][0, 0].item() + except AttributeError: + pass def get_driverstate_packet(model_output, frame_id: int, exec_time: float, gpu_exec_time: float): diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index b2700a4c4..529efb068 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -109,7 +109,14 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # temporal pose try: - temporal_pose = modelV2.temporalPose + temporal_pose = modelV2.temporalPoseDEPRECATED + except AttributeError: + try: + temporal_pose = modelV2.temporalPose + except AttributeError: + temporal_pose = None + + if temporal_pose is not None: if 'sim_pose' in net_output_data: temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() @@ -120,8 +127,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D temporal_pose.transStd = plan_stds_arr[0,Plan.VELOCITY].tolist() temporal_pose.rot = plan_arr[0,Plan.ORIENTATION_RATE].tolist() temporal_pose.rotStd = plan_stds_arr[0,Plan.ORIENTATION_RATE].tolist() - except AttributeError: - pass # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *plan_arr[:,Plan.POSITION].T) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 06264c791..201dc8439 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -516,12 +516,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { "Ensure road ahead is clear"), }, - EventName.lateralManeuver: { - ET.WARNING: longitudinal_maneuver_alert, - ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode", - "Ensure road ahead is clear"), - }, - EventName.selfdriveInitializing: { ET.NO_ENTRY: NoEntryAlert("System Initializing"), }, @@ -920,11 +914,6 @@ 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"), @@ -1131,50 +1120,6 @@ 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), }, @@ -1197,6 +1142,12 @@ STARPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: custom_startup_alert, }, + StarPilotEventName.lateralManeuver: { + ET.WARNING: longitudinal_maneuver_alert, + ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode", + "Ensure road ahead is clear"), + }, + StarPilotEventName.forcingStop: { ET.WARNING: forcing_stop_alert, }, @@ -1303,6 +1254,55 @@ STARPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: EngagementAlert(AudibleAlert.disengage), }, + StarPilotEventName.pedalNotCalibrated: { + ET.PERMANENT: NormalPermanentAlert("Pedal Not Calibrated", "Check Calibration"), + ET.NO_ENTRY: NoEntryAlert("Pedal Not Calibrated: Check Calibration"), + }, + + StarPilotEventName.pedalCruiseEnabled: { + ET.WARNING: Alert( + "Pedal Cruise Engaged", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.engage, 0.8), + }, + + StarPilotEventName.pedalCruiseDisabled: { + ET.WARNING: Alert( + "Pedal Cruise Disengaged", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 0.8), + }, + + StarPilotEventName.pedalMaxRegen: { + ET.WARNING: Alert( + "Max Regen Being Used", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.prompt, 2.), + }, + + StarPilotEventName.teslaCCEngaged: { + ET.WARNING: Alert( + "Tesla Cruise Engaged", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.engage, 0.8), + }, + + StarPilotEventName.teslaCCDisengaged: { + ET.WARNING: Alert( + "Tesla Cruise Disengaged", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 0.8), + }, + + StarPilotEventName.teslaCCNotArmed: { + ET.PERMANENT: NormalPermanentAlert("Arm Stock Cruise to Enable Speed Control"), + }, + StarPilotEventName.turningLeft: { ET.WARNING: turning_alert("Left"), }, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index e9db78ce4..4365969b9 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -94,6 +94,38 @@ def get_starpilot_alert_filters(current_alert_types: list[str], clear_event_type return starpilot_alert_types, starpilot_clear_event_types +def add_tesla_preap_starpilot_events(CP, CS, FPCS, starpilot_events: Events, prev_pedal_long_active: bool) -> bool: + if CP.brand != "tesla" or CP.carFingerprint != "TESLA_MODEL_S_PREAP": + return False + + if CP.pcmCruise: + if getattr(FPCS, 'teslaCCEngaged', False): + starpilot_events.add(StarPilotEventName.teslaCCEngaged) + if getattr(FPCS, 'teslaCCDisengaged', False): + starpilot_events.add(StarPilotEventName.teslaCCDisengaged) + if getattr(FPCS, 'teslaCCNotArmed', False): + starpilot_events.add(StarPilotEventName.teslaCCNotArmed) + return False + + from opendbc.car.tesla.preap.nap_conf import nap_conf + if not nap_conf.pedal_calibrated: + starpilot_events.add(StarPilotEventName.pedalNotCalibrated) + + if not CP.openpilotLongitudinalControl: + return False + + pedal_long_active = bool(CS.cruiseState.enabled and getattr(FPCS, 'pedalLongActive', False)) + if pedal_long_active and not prev_pedal_long_active: + starpilot_events.add(StarPilotEventName.pedalCruiseEnabled) + elif prev_pedal_long_active and not pedal_long_active: + starpilot_events.add(StarPilotEventName.pedalCruiseDisabled) + + if getattr(FPCS, 'pedalMaxRegen', False): + starpilot_events.add(StarPilotEventName.pedalMaxRegen) + + return pedal_long_active + + class SelfdriveD: def __init__(self, CP=None): self.params = Params() @@ -263,7 +295,7 @@ class SelfdriveD: self.startup_event = None if self.sm.recv_frame['lateralManeuverPlan'] > 0: - self.events.add(EventName.lateralManeuver) + self.starpilot_events.add(StarPilotEventName.lateralManeuver) self.startup_event = None elif self.sm.recv_frame['alertDebug'] > 0: self.events.add(EventName.longitudinalManeuver) @@ -294,7 +326,7 @@ class SelfdriveD: return # Block resume if cruise never previously enabled - resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.accelHardCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: self.events.add(EventName.resumeBlocked) @@ -314,21 +346,9 @@ 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 + self.prev_pedal_long_active = add_tesla_preap_starpilot_events( + self.CP, CS, self.sm['starpilotCarState'], self.starpilot_events, self.prev_pedal_long_active + ) if (getattr(self.starpilot_toggles, "nostalgia_mode", False) and self.CP.openpilotLongitudinalControl and @@ -765,9 +785,9 @@ class SelfdriveD: self.pm.send('starpilotSelfdriveState', fpss_msg) if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.starpilot_events.names != self.starpilot_events_prev): - fpce_send = messaging.new_message('starpilotOnroadEvents', len(self.starpilot_events)) + fpce_send = messaging.new_message('starpilotOnroadEvents') fpce_send.valid = True - fpce_send.starpilotOnroadEvents = self.starpilot_events.to_msg() + fpce_send.starpilotOnroadEvents.events = self.starpilot_events.to_msg() self.pm.send('starpilotOnroadEvents', fpce_send) self.starpilot_events_prev = self.starpilot_events.names.copy()