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

There's Pou in this Tine

Browse Source
This commit is contained in:
firestar5683
2026-06-18 10:07:26 -05:00
parent 7dab3a0ceb
commit bb4f3b74d9
10 changed files with 554 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
+1
View File
@@ -632,6 +632,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"VeryLongStarButtonControl", {PERSISTENT, INT, "0", "0", 2}},
{"VoltSNG", {PERSISTENT, BOOL, "0", "0", 2}},
{"GMAutoHold", {PERSISTENT, BOOL, "0", "0", 2}},
{"VoltOnePedalMode", {PERSISTENT, BOOL, "0", "0", 2}},
{"ToyotaAutoHold", {PERSISTENT, BOOL, "0", "0", 2}},
{"WarningImmediateVolume", {PERSISTENT, INT, "101", "101", 2}},
{"WarningSoftVolume", {PERSISTENT, INT, "101", "101", 2}},
opendbc_repo/opendbc/car/gm/carcontroller.py
+172 -4
View File
@@ -10,11 +10,13 @@ from opendbc.car.gm.values import (
CruiseButtons, GMFlags, GMSafetyFlags,
)
from opendbc.car.interfaces import CarControllerBase
from openpilot.common.pid import PIDController
from openpilot.common.params import Params, UnknownKeyName
from openpilot.starpilot.common.testing_grounds import testing_ground
VisualAlert = structs.CarControl.HUDControl.VisualAlert
NetworkLocation = structs.CarParams.NetworkLocation
TransmissionType = structs.CarParams.TransmissionType
LongCtrlState = structs.CarControl.Actuators.LongControlState
GearShifter = structs.CarState.GearShifter
@@ -36,6 +38,20 @@ AUTO_HOLD_DRIVE_GEARS = (
AUTO_HOLD_MIN_BRAKE = 80
AUTO_HOLD_MAX_BRAKE = 240
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
VOLT_ONE_PEDAL_DECEL_BP = [0.5 * CV.MPH_TO_MS, 6.0 * CV.MPH_TO_MS]
VOLT_ONE_PEDAL_DECEL_V = [-1.0, -1.1]
VOLT_ONE_PEDAL_MAX_DECEL = -1.6
VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_BP = [1.5, 20.0]
VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_V = [0.4, 0.2]
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_BP = [0.0, 10.0 * CV.MPH_TO_MS]
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_V = [0.25, 1.0]
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_BP = [20.0, 120.0]
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_V = [1.0, 0.25]
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_UP = 0.8 * DT_CTRL * 4
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_DOWN = 0.8 * DT_CTRL * 4
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_BP = [4.0, 8.0]
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_V = [0.4, 1.0]
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_INCLINE_V = [0.2, 1.0]
def get_stock_cc_active_for_cancel(CP, CS):
@@ -131,6 +147,19 @@ def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int:
return int(round(np.interp(v_ego, [0.0, 6.0, 15.0, 30.0], [40.0, 85.0, 130.0, 170.0])))
def get_lka_steering_cmd_counter(next_counter: int, CS) -> int:
if getattr(CS, "loopback_lka_steering_cmd_updated", False):
return (getattr(CS, "loopback_lka_steering_cmd_counter", next_counter) + 1) % 4
if next_counter < 0 and getattr(CS, "loopback_lka_steering_cmd_ts_nanos", 0) == 0:
return (getattr(CS, "pt_lka_steering_cmd_counter", next_counter) + 1) % 4
return next_counter
def should_send_stock_long_cancel(cancel_counter: int, CS) -> bool:
cs_out = getattr(CS, "out", None)
return cancel_counter > CAMERA_CANCEL_DELAY_FRAMES and not bool(getattr(cs_out, "accFaulted", False))
def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
safety_cfg = getattr(CP, "safetyConfigs", ())
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
@@ -142,12 +171,40 @@ def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
)
def supports_volt_one_pedal(CP, one_pedal_enabled: bool):
safety_cfg = getattr(CP, "safetyConfigs", ())
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
stock_hold_safety_ready = bool(safety_param & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value)
return (
one_pedal_enabled and
stock_hold_safety_ready and
getattr(CP, "transmissionType", None) == TransmissionType.direct and
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
)
def estimate_auto_hold_brake(driver_brake: float, op_brake: float) -> int:
driver_hold = np.interp(float(driver_brake), [8.0, 20.0, 40.0, 80.0], [80.0, 110.0, 150.0, 220.0])
hold_brake = max(float(op_brake), float(driver_hold))
return int(round(np.clip(hold_brake, AUTO_HOLD_MIN_BRAKE, AUTO_HOLD_MAX_BRAKE)))
def should_activate_volt_one_pedal(one_pedal_ready: bool, cruise_main: bool, long_active: bool,
gas_pressed: bool, brake_pressed: bool, regen_braking: bool,
single_pedal_mode: bool, gear_shifter, moving_backward: bool) -> bool:
return (
one_pedal_ready and
cruise_main and
single_pedal_mode and
gear_shifter in AUTO_HOLD_DRIVE_GEARS and
not long_active and
not gas_pressed and
not brake_pressed and
not regen_braking and
not moving_backward
)
def should_activate_auto_hold(hold_ready: bool, auto_hold_armed: bool, auto_hold_engaged: bool,
brake_pressed: bool, gas_pressed: bool, standstill: bool, long_active: bool,
regen_braking: bool, v_ego: float) -> bool:
@@ -232,11 +289,56 @@ class CarController(CarControllerBase):
self.malibu_button_phase = 0
self.malibu_last_button_ts_nanos = 0
self.auto_hold_brake = 0
self.volt_one_pedal_pid = PIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=1 / (DT_CTRL * 4),
pos_limit=0.0,
neg_limit=VOLT_ONE_PEDAL_MAX_DECEL,
)
self.volt_one_pedal_decel = 0.0
self.volt_one_pedal_brake = 0
try:
self.gm_auto_hold_enabled = self.params_.get_bool("GMAutoHold")
except UnknownKeyName:
self.gm_auto_hold_enabled = False
def _reset_volt_one_pedal(self):
self.volt_one_pedal_pid.reset()
self.volt_one_pedal_decel = min(0.0, float(self.aego))
self.volt_one_pedal_brake = 0
def _update_volt_one_pedal_brake(self, CC, CS):
if CS.out.vEgo > VOLT_ONE_PEDAL_DECEL_BP[-1]:
self._reset_volt_one_pedal()
return
pitch_accel = 0.0
if len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
pitch_accel = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
pitch_factor_values = VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_V if pitch_accel <= 0.0 else VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_INCLINE_V
pitch_accel *= float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_BP, pitch_factor_values))
target_decel = float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_DECEL_BP, VOLT_ONE_PEDAL_DECEL_V))
measured_decel = min(0.0, CS.out.aEgo + pitch_accel)
error_factor = float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_BP, VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_V))
error = (target_decel - measured_decel) * error_factor
raw_decel = float(self.volt_one_pedal_pid.update(error, speed=CS.out.vEgo, feedforward=target_decel))
rate_limit_factor = min(
float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_BP, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_V)),
float(np.interp(abs(CS.out.steeringAngleDeg), VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_BP, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_V)),
)
lower = min(self.volt_one_pedal_decel, measured_decel) - VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_UP * rate_limit_factor
upper = max(self.volt_one_pedal_decel, measured_decel) + VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_DOWN * rate_limit_factor
self.volt_one_pedal_decel = float(np.clip(raw_decel, lower, upper))
self.volt_one_pedal_decel = max(self.volt_one_pedal_decel, VOLT_ONE_PEDAL_MAX_DECEL)
self.volt_one_pedal_brake = int(round(np.clip(
np.interp(self.volt_one_pedal_decel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V),
0,
self.params.MAX_BRAKE,
)))
def calc_pedal_command(self, accel: float, long_active: bool, v_ego: float):
if not long_active:
self.planner_regen_hold = False
@@ -389,7 +491,31 @@ class CarController(CarControllerBase):
accel = actuators.accel
press_regen_paddle = False
auto_hold_enabled = supports_volt_auto_hold(self.CP, self.gm_auto_hold_enabled)
stock_hold_apply_brake = self.apply_brake if self.CP.openpilotLongitudinalControl else 0
volt_one_pedal_supported = supports_volt_one_pedal(
self.CP, bool(getattr(starpilot_toggles, "volt_one_pedal_mode", False))
)
volt_one_pedal_active = should_activate_volt_one_pedal(
volt_one_pedal_supported,
CS.out.cruiseState.available,
CC.longActive,
CS.out.gasPressed,
CS.out.brakePressed,
CS.out.regenBraking,
bool(getattr(CS, "single_pedal_mode", False)),
CS.out.gearShifter,
bool(getattr(CS, "moving_backward", False)),
)
if self.frame % 4 == 0:
if volt_one_pedal_active:
self._update_volt_one_pedal_brake(CC, CS)
else:
self._reset_volt_one_pedal()
if not self.CP.openpilotLongitudinalControl:
self.apply_gas = 0
self.apply_brake = self.volt_one_pedal_brake if volt_one_pedal_active else 0
stock_hold_apply_brake = max(self.apply_brake if self.CP.openpilotLongitudinalControl else 0, self.volt_one_pedal_brake)
hold_ready = (
auto_hold_enabled and
@@ -506,6 +632,13 @@ class CarController(CarControllerBase):
CS.out.regenBraking,
CS.out.vEgo,
)
volt_one_pedal_braking = volt_one_pedal_active and self.volt_one_pedal_brake > 0
volt_one_pedal_hold_active = (
volt_one_pedal_braking and
not auto_hold_active and
CS.auto_hold_drive_time >= AUTO_HOLD_MIN_DRIVE_TIME_S and
(CS.out.standstill or CS.out.vEgo < 0.02)
)
# Steering (Active: 50Hz, inactive: 10Hz)
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
@@ -631,6 +764,10 @@ class CarController(CarControllerBase):
# gas interceptor only used for full long control on cars without ACC
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
if volt_one_pedal_braking:
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = max(self.apply_brake, self.volt_one_pedal_brake)
maneuver_sng_launch = self.longitudinal_maneuver_mode and self.is_volt
if (
self.CP.enableGasInterceptorDEPRECATED and
@@ -690,7 +827,16 @@ class CarController(CarControllerBase):
acc_engaged = CC.enabled
if auto_hold_active:
hold_brake = self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, self.apply_brake)
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, self.apply_brake))
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
can_sends.append(gmcan.create_friction_brake_command(
self.packer_ch, friction_brake_bus, hold_brake, idx, False, hold_near_stop, hold_standstill,
self.CP, allow_near_stop_mode=True))
CS.auto_hold_engaged = True
CS.auto_hold_fault_suppression_timer = 1.0
elif volt_one_pedal_hold_active:
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(0.0, self.volt_one_pedal_brake))
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
can_sends.append(gmcan.create_friction_brake_command(
@@ -699,12 +845,16 @@ class CarController(CarControllerBase):
CS.auto_hold_engaged = True
CS.auto_hold_fault_suppression_timer = 1.0
else:
if volt_one_pedal_braking:
at_full_stop = at_full_stop or CS.pcm_acc_status == AccState.STANDSTILL
near_stop = near_stop or (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
can_sends.append(gmcan.create_gas_regen_command(
self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop,
include_always_one3=self.CP.carFingerprint in kaofui_cars, use_volt_layout=self.is_volt))
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
idx, CC.enabled, near_stop, at_full_stop, self.CP))
idx, CC.enabled, near_stop, at_full_stop, self.CP,
allow_near_stop_mode=volt_one_pedal_braking))
CS.auto_hold_engaged = False
if should_send_acc_dashboard_status(self.CP, dash_speed_spoof_active):
@@ -766,7 +916,7 @@ class CarController(CarControllerBase):
else:
if self.frame % 4 == 0 and auto_hold_active:
idx = (self.frame // 4) % 4
hold_brake = self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake)
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake))
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
can_sends.append(gmcan.create_friction_brake_command(
@@ -774,7 +924,25 @@ class CarController(CarControllerBase):
self.CP, allow_near_stop_mode=True))
CS.auto_hold_engaged = True
CS.auto_hold_fault_suppression_timer = 1.0
elif self.frame % 4 == 0 and volt_one_pedal_hold_active:
idx = (self.frame // 4) % 4
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(0.0, self.volt_one_pedal_brake))
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
can_sends.append(gmcan.create_friction_brake_command(
self.packer_ch, get_friction_brake_bus(self.CP), hold_brake, idx, False, hold_near_stop, hold_standstill,
self.CP, allow_near_stop_mode=True))
CS.auto_hold_engaged = True
CS.auto_hold_fault_suppression_timer = 1.0
elif self.frame % 4 == 0 and volt_one_pedal_braking:
idx = (self.frame // 4) % 4
near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
can_sends.append(gmcan.create_friction_brake_command(
self.packer_ch, get_friction_brake_bus(self.CP), self.volt_one_pedal_brake, idx, False, near_stop, False,
self.CP, allow_near_stop_mode=True))
CS.auto_hold_engaged = False
elif self.frame % 4 == 0:
self.apply_brake = 0
CS.auto_hold_engaged = False
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py
+117
View File
@@ -43,10 +43,12 @@ from opendbc.car.gm.carcontroller import (
get_testing_ground_1_brake_switch_bias,
get_stock_cc_active_for_cancel,
should_activate_auto_hold,
should_activate_volt_one_pedal,
should_send_stock_long_cancel,
should_spoof_dash_speed,
should_spoof_ecm_cruise_status,
supports_volt_auto_hold,
supports_volt_one_pedal,
use_interceptor_sng_launch,
)
from opendbc.car.gm.gmcan import get_friction_brake_mode
@@ -223,6 +225,52 @@ def test_auto_hold_brake_estimate_uses_driver_or_op_brake_and_clamps():
assert estimate_auto_hold_brake(100.0, 400.0) == 240
def test_volt_one_pedal_requires_toggle_supported_volt_stock_safety_and_ev_transmission():
stock_safety = [SimpleNamespace(safetyParam=0x8000)]
no_safety = [SimpleNamespace(safetyParam=0)]
assert supports_volt_one_pedal(
SimpleNamespace(
carFingerprint=CAR.CHEVROLET_VOLT_CAMERA,
safetyConfigs=stock_safety,
transmissionType=structs.CarParams.TransmissionType.direct,
),
True,
)
assert not supports_volt_one_pedal(
SimpleNamespace(
carFingerprint=CAR.CHEVROLET_VOLT_CAMERA,
safetyConfigs=no_safety,
transmissionType=structs.CarParams.TransmissionType.direct,
),
True,
)
assert not supports_volt_one_pedal(
SimpleNamespace(
carFingerprint=CAR.CHEVROLET_VOLT_CC,
safetyConfigs=stock_safety,
transmissionType=structs.CarParams.TransmissionType.direct,
),
True,
)
assert not supports_volt_one_pedal(
SimpleNamespace(
carFingerprint=CAR.CHEVROLET_VOLT_CAMERA,
safetyConfigs=stock_safety,
transmissionType=structs.CarParams.TransmissionType.automatic,
),
True,
)
assert not supports_volt_one_pedal(
SimpleNamespace(
carFingerprint=CAR.CHEVROLET_VOLT_CAMERA,
safetyConfigs=stock_safety,
transmissionType=structs.CarParams.TransmissionType.direct,
),
False,
)
def test_auto_hold_drive_gears_accept_capnp_dynamic_enum_membership():
msg = structs.CarState.new_message()
msg.gearShifter = structs.CarState.GearShifter.drive
@@ -310,6 +358,75 @@ def test_auto_hold_activation_releases_immediately_on_gas_press():
)
def test_volt_one_pedal_activation_requires_main_l_mode_and_no_driver_input():
assert should_activate_volt_one_pedal(
True,
True,
False,
False,
False,
False,
True,
structs.CarState.GearShifter.low,
False,
)
assert not should_activate_volt_one_pedal(
True,
False,
False,
False,
False,
False,
True,
structs.CarState.GearShifter.low,
False,
)
assert not should_activate_volt_one_pedal(
True,
True,
True,
False,
False,
False,
True,
structs.CarState.GearShifter.low,
False,
)
assert not should_activate_volt_one_pedal(
True,
True,
False,
True,
False,
False,
True,
structs.CarState.GearShifter.low,
False,
)
assert not should_activate_volt_one_pedal(
True,
True,
False,
False,
False,
True,
True,
structs.CarState.GearShifter.low,
False,
)
assert not should_activate_volt_one_pedal(
True,
True,
False,
False,
False,
False,
False,
structs.CarState.GearShifter.drive,
False,
)
def test_friction_brake_mode_keeps_near_stop_disabled_for_regular_long_braking():
CP = SimpleNamespace(carFingerprint=CAR.CHEVROLET_VOLT_ASCM)
selfdrive/controls/lib/longitudinal_planner.py
+84
View File
@@ -357,6 +357,18 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35
NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22
NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32
NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED = 12.0
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB = 0.95
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL = 0.35
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED = 0.5
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE = 0.8
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.85
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A = 0.35
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF = 1.5
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF = 0.35
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP = 0.12
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP = 0.28
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP = 0.22
TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80
@@ -1898,6 +1910,62 @@ class LongitudinalPlanner:
return None
def get_duplicate_slow_lead_brake_hold_target(self, lead, v_ego, base_t_follow,
prev_output_a_target, output_a_target,
current_source, tracking_lead_active):
if lead is None or not lead.status:
return None
if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active:
return None
if not (self.lead_one.status and self.lead_two.status):
return None
if (
abs(float(self.lead_one.dRel) - float(self.lead_two.dRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF or
abs(float(self.lead_one.vRel) - float(self.lead_two.vRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF
):
return None
if float(v_ego) < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED:
return None
lead_prob = float(getattr(lead, "modelProb", 0.0))
if bool(getattr(lead, "radar", False)) or lead_prob < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB:
return None
prev_brake = max(0.0, -float(prev_output_a_target))
if prev_brake < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL:
return None
target_delta = float(output_a_target) - float(prev_output_a_target)
if target_delta < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A:
return None
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
if lead_brake > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE:
return None
closing_speed = max(0.0, float(v_ego) - float(lead.vLead))
if closing_speed < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED:
return None
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
if actual_headway > float(base_t_follow) + DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET:
return None
positive_step = float(np.interp(
closing_speed,
[DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED, 1.5, 4.0, 8.0],
[DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP,
0.16,
0.22,
DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP],
))
if float(prev_output_a_target) * float(output_a_target) < 0.0:
positive_step = min(positive_step, DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP)
upper = float(prev_output_a_target) + positive_step
smoothed_target = float(min(float(output_a_target), upper))
return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None
def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired):
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
return None
@@ -2736,6 +2804,22 @@ class LongitudinalPlanner:
self.a_desired = max(self.a_desired, near_duplicate_transition_target)
output_a_target = near_duplicate_transition_target
duplicate_slow_lead_brake_hold_target = self.get_duplicate_slow_lead_brake_hold_target(
comfort_lead,
scene_v_ego,
effective_t_follow,
prev_output_a_target,
output_a_target,
self.mpc.source,
bool(getattr(sm["starpilotPlan"], "trackingLead", False)),
)
if duplicate_slow_lead_brake_hold_target is not None:
if duplicate_slow_lead_brake_hold_target < output_a_target:
self.a_desired = min(self.a_desired, duplicate_slow_lead_brake_hold_target)
else:
self.a_desired = max(self.a_desired, duplicate_slow_lead_brake_hold_target)
output_a_target = duplicate_slow_lead_brake_hold_target
if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap(
follow_control_lead,
selfdrive/controls/tests/test_longitudinal_planner.py
+49
View File
@@ -2730,6 +2730,55 @@ def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip()
assert smoothed == pytest.approx(-0.92, abs=1e-6)
def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads():
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998)
lead_two = make_lead(status=True, d_rel=19.25, v_lead=20.02, a_lead=-0.41, radar=False, model_prob=0.996)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
smoothed = planner.get_duplicate_slow_lead_brake_hold_target(
lead_one,
v_ego,
1.0,
prev_output_a_target=-3.50,
output_a_target=0.0,
current_source="lead0",
tracking_lead_active=True,
)
assert smoothed is not None
assert smoothed == pytest.approx(-3.28, abs=1e-6)
def test_duplicate_slow_lead_brake_hold_skips_distinct_leads():
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998)
lead_two = make_lead(status=True, d_rel=24.0, v_lead=21.5, a_lead=-0.10, radar=False, model_prob=0.996)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
smoothed = planner.get_duplicate_slow_lead_brake_hold_target(
lead_one,
v_ego,
1.0,
prev_output_a_target=-3.50,
output_a_target=0.0,
current_source="lead0",
tracking_lead_active=True,
)
assert smoothed is None
def test_near_duplicate_lead_transition_target_skips_plain_cruise_without_tracking():
v_ego = 25.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
selfdrive/controls/tests/test_starpilot_vcruise.py
+65
View File
@@ -141,6 +141,71 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
assert vcruise.forcing_stop
def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold():
_, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
result = vcruise.update(
controls_enabled=True,
now=0.0,
time_validated=True,
v_cruise=20.0,
v_ego=0.0,
sm=make_sm(standstill=True),
starpilot_toggles=make_toggles(),
)
assert result == pytest.approx(0.0)
assert vcruise.standstill_force_stop_hold
assert vcruise.force_stop_timer >= 0.5
assert vcruise.forcing_stop
assert vcruise.tracked_model_length == pytest.approx(0.0)
def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release():
planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
sm = make_sm(standstill=True)
toggles = make_toggles()
first = vcruise.update(
controls_enabled=True,
now=0.0,
time_validated=True,
v_cruise=20.0,
v_ego=0.0,
sm=sm,
starpilot_toggles=toggles,
)
assert first == pytest.approx(0.0)
assert vcruise.standstill_force_stop_hold
planner.starpilot_cem.stop_light_detected = False
second = vcruise.update(
controls_enabled=True,
now=0.4,
time_validated=True,
v_cruise=20.0,
v_ego=0.0,
sm=sm,
starpilot_toggles=toggles,
)
assert second == pytest.approx(0.0)
assert vcruise.standstill_force_stop_hold
assert vcruise.forcing_stop
released = vcruise.update(
controls_enabled=True,
now=1.2,
time_validated=True,
v_cruise=20.0,
v_ego=0.0,
sm=sm,
starpilot_toggles=toggles,
)
assert released == pytest.approx(20.0)
assert not vcruise.standstill_force_stop_hold
assert not vcruise.forcing_stop
def test_nav_turn_speed_control_default_off():
_, vcruise = make_vcruise(nav_state={
"valid": True,
starpilot/common/safe_mode.py
+1
View File
@@ -186,6 +186,7 @@ SAFE_MODE_MANAGED_KEYS = (
"SubaruSNGManualParkingBrake",
"VoltSNG",
"GMAutoHold",
"VoltOnePedalMode",
"GMPedalLongitudinal",
"GMDashSpoofOffsets",
"LongPitch",
starpilot/common/starpilot_variables.py
+1
View File
@@ -1367,6 +1367,7 @@ class StarPilotVariables:
gm_auto_hold_supported = toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS
toggle.gm_auto_hold = self.get_value("GMAutoHold", condition=gm_auto_hold_supported)
toggle.volt_one_pedal_mode = self.get_value("VoltOnePedalMode", condition=gm_auto_hold_supported)
toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS)
starpilot/controls/lib/starpilot_vcruise.py
+57 -17
View File
@@ -11,6 +11,7 @@ from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitCo
CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
OVERRIDE_FORCE_STOP_TIMER = 10
STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75
NAV_TURN_COMFORT_DECEL = 1.25
NAV_TURN_DISTANCE_BUFFER = 8.0
NAV_TURN_MIN_TARGET_DELTA = 0.25
@@ -64,6 +65,8 @@ class StarPilotVCruise:
self.override_force_stop_timer = 0
self.force_stop_timer = 0.0
self.standstill_force_stop_hold = False
self.standstill_force_stop_clear_since = 0.0
# Kinematic distance estimator. Same attribute also published as
# starpilotPlan.forcingStopLength, so the existing reader keeps working.
self.tracked_model_length = 0.0
@@ -213,11 +216,41 @@ class StarPilotVCruise:
self.stop_sign_confirmed = True
raw_model_stopped = bool(getattr(self.starpilot_planner, "raw_model_stopped", False))
standstill_force_stop_scene_active = bool(force_stop_active or raw_model_stopped)
# If the driver engages while already stopped at a red light / stop sign, seed
# the same stop-hold path openpilot would have had if it made the stop itself.
# Without this, a brief model-clear dropout can release the stop immediately.
if (
controls_enabled and
sm["carState"].standstill and
standstill_force_stop_scene_active and
not self.forcing_stop and
self.force_stop_timer < 0.5
):
self.standstill_force_stop_hold = True
self.standstill_force_stop_clear_since = 0.0
self.tracked_model_length = 0.0
if self.standstill_force_stop_hold:
pedal_override = bool(sm["carState"].gasPressed or sm["starpilotCarState"].accelPressed)
if (not controls_enabled) or (not sm["carState"].standstill) or lead_present or pedal_override:
self.standstill_force_stop_hold = False
self.standstill_force_stop_clear_since = 0.0
elif standstill_force_stop_scene_active:
self.standstill_force_stop_clear_since = 0.0
elif self.standstill_force_stop_clear_since == 0.0:
self.standstill_force_stop_clear_since = now
elif (now - self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME:
self.standstill_force_stop_hold = False
self.standstill_force_stop_clear_since = 0.0
# Timer ramp. Faster commitment when the dashboard confirms.
if force_stop_active and not sm["carState"].standstill:
rate = DT_MDL * 2 if dash_active else DT_MDL
self.force_stop_timer = min(self.force_stop_timer + rate, 2.0)
elif self.standstill_force_stop_hold:
self.force_stop_timer = max(self.force_stop_timer, 0.5)
elif (self.forcing_stop and sm["carState"].standstill and not dash_active and
not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped):
self.force_stop_timer = 0.0
@@ -227,6 +260,7 @@ class StarPilotVCruise:
force_stop_enabled = self.force_stop_timer >= 0.5
# Stay committed across model dropouts until standstill
force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill
force_stop_enabled |= self.standstill_force_stop_hold
# Override: gas/accel pedal during an active force stop
self.override_force_stop |= sm["carState"].gasPressed
@@ -298,29 +332,35 @@ class StarPilotVCruise:
v_cruise = 0.0
elif force_stop_enabled and not self.override_force_stop:
self.forcing_stop |= not sm["carState"].standstill
self.forcing_stop |= not sm["carState"].standstill or self.standstill_force_stop_hold
# Kinematic distance estimator (also published as forcingStopLength).
# Decay one-to-one with motion, clamp by current model_length so we adopt
# the model's view when it regains sight, and snap closer to DASH_SEED_M
# whenever the dashboard signal is active.
self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length)
if dash_active:
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
# Kinematic profile with user offset. Positive offset shifts the perceived
# line further down the road -> car rolls further before commanding 0.
effective_d = self.tracked_model_length + offset_m
if effective_d <= MPC_HANDOFF_M:
v_target = 0.0
if self.standstill_force_stop_hold:
self.tracked_model_length = 0.0
v_cruise = 0.0
else:
v_target = math.sqrt(2.0 * COMFORT_DECEL * (effective_d - MPC_HANDOFF_M))
# Kinematic distance estimator (also published as forcingStopLength).
# Decay one-to-one with motion, clamp by current model_length so we adopt
# the model's view when it regains sight, and snap closer to DASH_SEED_M
# whenever the dashboard signal is active.
self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length)
if dash_active:
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
v_cruise = min(v_target, v_cruise)
# Kinematic profile with user offset. Positive offset shifts the perceived
# line further down the road -> car rolls further before commanding 0.
effective_d = self.tracked_model_length + offset_m
if effective_d <= MPC_HANDOFF_M:
v_target = 0.0
else:
v_target = math.sqrt(2.0 * COMFORT_DECEL * (effective_d - MPC_HANDOFF_M))
v_cruise = min(v_target, v_cruise)
else:
self.forcing_stop = False
self.standstill_force_stop_hold = False
self.standstill_force_stop_clear_since = 0.0
# Latch is only meaningful during an active force-stop cycle
self.stop_sign_confirmed = False
starpilot/system/the_pond/assets/components/tools/device_settings_layout.json
+7
View File
@@ -2508,6 +2508,13 @@
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "VoltOnePedalMode",
"label": "Volt One Pedal Mode",
"description": "On supported Chevy Volts in L / single-pedal mode, blend light friction braking at low speed so the car can come to a stop and hold without using the brake pedal.",
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "SubaruSNG",
"label": "Stop and Go",