mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

UpstreamStructs

Browse Source
This commit is contained in:
firestar5683
2026-06-24 14:06:16 -05:00
parent 94aff6faed
commit 028ee7b3f5
18 changed files with 204 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+22 -2
View File
@@ -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 {
cereal/log.capnp
+3 -17
View File
@@ -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;
opendbc_repo/opendbc/car/__init__.py
+1 -1
View File
@@ -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:
opendbc_repo/opendbc/car/car.capnp
-8
View File
@@ -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 {
opendbc_repo/opendbc/car/gm/carstate.py
+5 -3
View File
@@ -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
opendbc_repo/opendbc/car/tesla/preap/carstate.py
+5 -5
View File
@@ -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
opendbc_repo/opendbc/car/tesla/preap/interface.py
+2 -1
View File
@@ -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
opendbc_repo/opendbc/safety/tests/test_tesla_preap.py
+2 -2
View File
@@ -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
selfdrive/car/car_specific.py
-13
View File
@@ -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)
selfdrive/car/card.py
+3 -2
View File
@@ -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)
selfdrive/car/cruise.py
+27 -18
View File
@@ -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):
selfdrive/car/tests/test_car_specific_tesla_preap.py
+11 -10
View File
@@ -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)
selfdrive/car/tests/test_cruise_speed.py
+10 -4
View File
@@ -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
selfdrive/controls/lib/longitudinal_planner.py
+7 -3
View File
@@ -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
selfdrive/modeld/dmonitoringmodeld.py
+4 -1
View File
@@ -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):
selfdrive/modeld/fill_model_msg.py
+8 -3
View File
@@ -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)
selfdrive/selfdrived/events.py
+55 -55
View File
@@ -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"),
},
selfdrive/selfdrived/selfdrived.py
+39 -19
View File
@@ -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()