From bb4f3b74d9b8900e1899a483b47936082ca8985c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 18 Jun 2026 10:07:26 -0500 Subject: [PATCH] There's Pou in this Tine --- common/params_keys.h | 1 + opendbc_repo/opendbc/car/gm/carcontroller.py | 176 +++++++++++++++++- .../car/gm/tests/test_carcontroller.py | 117 ++++++++++++ .../controls/lib/longitudinal_planner.py | 84 +++++++++ .../tests/test_longitudinal_planner.py | 49 +++++ .../controls/tests/test_starpilot_vcruise.py | 65 +++++++ starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 1 + starpilot/controls/lib/starpilot_vcruise.py | 74 ++++++-- .../tools/device_settings_layout.json | 7 + 10 files changed, 554 insertions(+), 21 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 80805471b..cac713eb0 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -632,6 +632,7 @@ inline static std::unordered_map 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}}, diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 7342a01dc..c7721b835 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -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. diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 383617933..284c0b623 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f97353dfb..a291292ba 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 778da73ee..c91e81cdb 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index d97f73d73..3e6ec2f97 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -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, diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 5aa0f8bfb..caeaf40ba 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -186,6 +186,7 @@ SAFE_MODE_MANAGED_KEYS = ( "SubaruSNGManualParkingBrake", "VoltSNG", "GMAutoHold", + "VoltOnePedalMode", "GMPedalLongitudinal", "GMDashSpoofOffsets", "LongPitch", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index b5bdd133a..321eb2dc0 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -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) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 95c0faf79..98f0fca9f 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -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 diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index e2650a50d..9ca5426fd 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -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",