diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 0495aabeb..5265b1dd5 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -25,7 +25,7 @@ ACCEL_WINDUP_LIMIT = 4.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_PID_UNWIND = 0.03 * DT_CTRL * 3 # m/s^2 / frame PRIUS_INTEGRAL_MISMATCH_UNWIND = 8.0 -PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.5 +PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.7 MAX_PITCH_COMPENSATION = 1.5 # m/s^2 TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s @@ -142,6 +142,25 @@ def limit_interceptor_stopping_accel(pcm_accel_cmd: float, target_accel: float, return max(pcm_accel_cmd, max(stop_floor, planner_floor)) +def limit_prius_stopping_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, lead_visible: bool) -> float: + if not stopping or pcm_accel_cmd >= 0.0 or v_ego >= 1.5: + return pcm_accel_cmd + + # Prius can hold onto a stale full negative stop command at standstill even after the + # planner has already softened. Keep enough brake to hold the stop, but let the command + # unwind toward the live planner target so launches are not delayed and stop transitions + # are less abrupt. + if target_accel <= -1.8: + return pcm_accel_cmd + + stop_floor = float(np.interp(v_ego, + [0.0, 0.2, 0.5, 0.9, 1.5], + [-0.96, -1.00, -1.08, -1.18, -1.35] if lead_visible else [-0.84, -0.88, -0.96, -1.08, -1.24])) + target_buffer = float(np.interp(v_ego, [0.0, 0.5, 1.5], [0.06, 0.10, 0.16])) + planner_floor = float(target_accel) - target_buffer + return max(pcm_accel_cmd, max(stop_floor, planner_floor)) + + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -410,6 +429,8 @@ class CarController(CarControllerBase): if self.CP.enableGasInterceptorDEPRECATED: pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo) pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, bool(hud_control.leadVisible)) + elif self.CP.carFingerprint == CAR.TOYOTA_PRIUS: + pcm_accel_cmd = limit_prius_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index dbc0bcc6b..310b5c03d 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -7,7 +7,8 @@ from opendbc.can import CANPacker, CANParser from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict from opendbc.car.toyota import toyotacan -from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, update_permit_braking +from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, \ + limit_prius_stopping_accel, update_permit_braking from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.interface import CarInterface @@ -252,6 +253,14 @@ class TestToyotaCarController: assert update_permit_braking(False, 0.10, True, True, 25.0, False) is True assert update_permit_braking(False, 0.10, False, False, 25.0, False) is True + def test_prius_stopping_accel_unwinds_stale_stop_hold(self): + limited = limit_prius_stopping_accel(-3.28, -0.05, True, 0.0, True) + assert -1.5 < limited < 0.0 + + def test_prius_stopping_accel_keeps_hard_stop_commands(self): + limited = limit_prius_stopping_accel(-3.28, -2.0, True, 0.0, True) + assert limited == -3.28 + def test_sng_hack_clears_existing_standstill_latch(self): controller = self._make_controller(standstill_req=True, last_standstill=True) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index edf2804d4..4c1412ef7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -6,6 +6,7 @@ from cereal import log from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR +from opendbc.car.toyota.values import CAR as TOYOTA_CAR from opendbc.car.lateral import get_friction from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV from openpilot.common.filter_simple import FirstOrderFilter @@ -143,6 +144,9 @@ KIA_EV6_CARS = ( KIA_FORTE_CARS = ( HYUNDAI_CAR.KIA_FORTE, ) +PRIUS_CARS = ( + TOYOTA_CAR.TOYOTA_PRIUS, +) BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 @@ -551,6 +555,28 @@ VOLT_PLEXY_TURN_IN_FRICTION_BOOST_LEFT = 0.08 VOLT_PLEXY_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_LEFT = 0.16 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 +PRIUS_TRANSITION_SPEED = 10.0 +PRIUS_PHASE_SCALE = 0.09 +PRIUS_FF_GAIN_LEFT = 0.10 +PRIUS_FF_GAIN_RIGHT = 0.14 +PRIUS_FF_ONSET = 0.16 +PRIUS_FF_ONSET_WIDTH = 0.08 +PRIUS_FF_CUTOFF = 1.25 +PRIUS_FF_CUTOFF_WIDTH = 0.30 +PRIUS_FRICTION_LAT_RISE = 0.18 +PRIUS_FRICTION_JERK_RISE = 0.22 +PRIUS_TURN_IN_BOOST_LEFT = 0.48 +PRIUS_TURN_IN_BOOST_RIGHT = 0.62 +PRIUS_UNWIND_TAPER_LEFT = 0.44 +PRIUS_UNWIND_TAPER_RIGHT = 0.72 +PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18 +PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.24 +PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 +PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.44 +PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.08 +PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.12 +PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.14 +PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.24 def _sigmoid(x: float) -> float: @@ -567,6 +593,74 @@ def get_friction_threshold(v_ego: float) -> float: return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) +def _prius_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _prius_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / PRIUS_TRANSITION_SPEED) ** 2) + + +def _prius_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PRIUS_PHASE_SCALE) + + +def _prius_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _prius_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PRIUS_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PRIUS_FRICTION_JERK_RISE) + return _prius_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _prius_side_value(desired_lateral_accel, PRIUS_FF_GAIN_LEFT, PRIUS_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _prius_sigmoid((abs_lateral_accel - PRIUS_FF_ONSET) / PRIUS_FF_ONSET_WIDTH) + cutoff = _prius_sigmoid((PRIUS_FF_CUTOFF - abs_lateral_accel) / PRIUS_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _prius_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.86), 1.16) + + +def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = 1.0 + friction_scale += (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_FRICTION_BOOST_LEFT, PRIUS_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_FRICTION_REDUCTION_LEFT, PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.90), 1.14) + + def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") @@ -1748,6 +1842,7 @@ class LatControlTorque(LatControl): self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS self.is_palisade = CP.carFingerprint in PALISADE_CARS + self.is_prius = CP.carFingerprint in PRIUS_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS @@ -1880,6 +1975,7 @@ class LatControlTorque(LatControl): volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active() palisade_active = self.is_palisade + prius_active = self.is_prius ioniq_5_active = self.is_ioniq_5 ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 @@ -1922,6 +2018,10 @@ class LatControlTorque(LatControl): ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif prius_active: + ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = get_prius_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif ioniq_5_active: ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f65442632..71c146775 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -237,6 +237,21 @@ MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18 MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08 MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16 MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 10.0 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED = MATCHED_FOLLOW_TRANSITION_MIN_SPEED +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.45 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 1.00 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.98 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.08 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.25 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC = 18.0 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.06 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.10 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.05 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.08 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.06 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET = -0.12 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A = 0.12 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95 NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35 @@ -1215,18 +1230,26 @@ class LongitudinalPlanner: )) return -max(0.0, cap_decel - relax_decel) - def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target): + def get_matched_follow_transition_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 float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED: + low_speed_extension_active = ( + bool(tracking_lead_active) and + current_source == "cruise" and + LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED + ) + if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED and not low_speed_extension_active: return None lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB: + min_model_prob = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB + if lead_prob < min_model_prob: return None lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE: + max_lead_brake = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE + if lead_brake > max_lead_brake: return None relative_speed = float(v_ego) - float(lead.vLead) @@ -1234,49 +1257,65 @@ class LongitudinalPlanner: return None closing_speed = max(0.0, relative_speed) - if closing_speed > MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED: + max_closing_speed = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED + if closing_speed > max_closing_speed: return None ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - if ttc < MATCHED_FOLLOW_TRANSITION_MIN_TTC: + min_ttc = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_TTC + if ttc < min_ttc: return None actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) headway_margin = actual_headway - float(base_t_follow) - if headway_margin < MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN: + min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN + full_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN + if headway_margin < min_headway_margin: return None if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: return None target_delta = float(output_a_target) - float(prev_output_a_target) - if abs(target_delta) < 1e-3: + if low_speed_extension_active: + if float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: + return None + if float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: + return None + if abs(target_delta) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A: + return None + elif abs(target_delta) < 1e-3: return None headway_factor = float(np.clip( - (headway_margin - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN) / - max(MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN, 1e-3), + (headway_margin - min_headway_margin) / + max(full_headway_margin - min_headway_margin, 1e-3), 0.0, 1.0, )) + min_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP + max_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP positive_step = float(np.interp( max(float(lead.vLead) - float(v_ego), 0.0), [0.0, 1.0], - [MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP], + [min_positive_step, max_positive_step], )) + min_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP + max_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP negative_step = float(np.interp( closing_speed, - [0.0, MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED], - [MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP], + [0.0, max_closing_speed], + [min_negative_step, max_negative_step], )) # The more space we still have, the less abrupt the comfort path should be. - positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP])) - negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP])) + positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, min_positive_step])) + negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, min_negative_step])) if float(prev_output_a_target) * float(output_a_target) < 0.0: - positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP) - negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP) + sign_cross_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP + positive_step = min(positive_step, sign_cross_step) + negative_step = min(negative_step, sign_cross_step) lower = float(prev_output_a_target) - negative_step upper = float(prev_output_a_target) + positive_step @@ -2042,6 +2081,8 @@ class LongitudinalPlanner: effective_t_follow, prev_output_a_target, output_a_target, + self.mpc.source, + bool(getattr(sm["starpilotPlan"], "trackingLead", False)), ) if matched_follow_transition_target is not None: if matched_follow_transition_target < output_a_target: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 23d9fd500..0ce61cfe2 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -43,6 +43,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_palisade_ff_scale, get_palisade_friction_scale, get_palisade_friction_threshold, + get_prius_ff_scale, + get_prius_friction_scale, + get_prius_friction_threshold, get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, @@ -363,6 +366,41 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_prius_ff_scale_curve(self): + assert get_prius_ff_scale(0.0, 0.0, 20.0) == 1.0 + steady_left = get_prius_ff_scale(0.7, 0.0, 8.0) + steady_right = get_prius_ff_scale(-0.7, 0.0, 8.0) + turn_in_left = get_prius_ff_scale(0.7, 0.8, 8.0) + turn_in_right = get_prius_ff_scale(-0.7, -0.8, 8.0) + unwind_left = get_prius_ff_scale(0.7, -0.8, 8.0) + unwind_right = get_prius_ff_scale(-0.7, 0.8, 8.0) + assert steady_left > 1.0 + assert steady_right > steady_left + assert turn_in_left > steady_left + assert turn_in_right > steady_right + assert unwind_left < steady_left + assert unwind_right < steady_right + assert unwind_right < unwind_left + + def test_prius_friction_curves(self): + base_threshold = get_friction_threshold(12.0) + left_turn_in_threshold = get_prius_friction_threshold(6.0, 0.7, 0.8) + right_turn_in_threshold = get_prius_friction_threshold(6.0, -0.7, -0.8) + left_unwind_threshold = get_prius_friction_threshold(6.0, 0.7, -0.8) + right_unwind_threshold = get_prius_friction_threshold(6.0, -0.7, 0.8) + assert left_turn_in_threshold < base_threshold + assert right_turn_in_threshold < left_turn_in_threshold + assert left_unwind_threshold > base_threshold + assert right_unwind_threshold >= left_unwind_threshold + + base_scale = get_prius_friction_scale(25.0, 0.7, 0.8) + left_turn_in_scale = get_prius_friction_scale(6.0, 0.7, 0.8) + right_turn_in_scale = get_prius_friction_scale(6.0, -0.7, -0.8) + left_unwind_scale = get_prius_friction_scale(6.0, 0.7, -0.8) + right_unwind_scale = get_prius_friction_scale(6.0, -0.7, 0.8) + assert right_turn_in_scale > left_turn_in_scale > base_scale + assert base_scale > left_unwind_scale > right_unwind_scale + def test_ioniq_5_ff_scale_curve(self): assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 50072b9ac..c0ecd4438 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1778,6 +1778,8 @@ def test_matched_follow_transition_target_damps_large_comfort_sign_flip(): 1.45, prev_output_a_target=0.12, output_a_target=-0.40, + current_source="cruise", + tracking_lead_active=True, ) assert smoothed is not None @@ -1797,6 +1799,66 @@ def test_matched_follow_transition_target_skips_urgent_closure(): 1.45, prev_output_a_target=0.10, output_a_target=-0.60, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is None + + +def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_throttle_jitter(): + v_ego = 14.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.45, + prev_output_a_target=0.08, + output_a_target=0.46, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is not None + assert smoothed == pytest.approx(0.14, abs=1e-6) + + +def test_matched_follow_transition_target_skips_low_speed_without_tracking(): + v_ego = 14.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.45, + prev_output_a_target=0.08, + output_a_target=0.46, + current_source="cruise", + tracking_lead_active=False, + ) + + assert smoothed is None + + +def test_matched_follow_transition_target_skips_low_speed_real_braking(): + v_ego = 14.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=29.0, v_lead=13.6, a_lead=0.0, radar=False, model_prob=0.999) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.45, + prev_output_a_target=0.08, + output_a_target=-0.30, + current_source="cruise", + tracking_lead_active=True, ) assert smoothed is None diff --git a/starpilot/system/the_pond/factory_reset.py b/starpilot/system/the_pond/factory_reset.py new file mode 100644 index 000000000..d807d6e8c --- /dev/null +++ b/starpilot/system/the_pond/factory_reset.py @@ -0,0 +1,29 @@ +import subprocess +import time + + +_DELETE_TIMEOUT_S = 1800 +_DELETE_RETRY_ATTEMPTS = 20 +_DELETE_RETRY_DELAY_S = 0.25 +_DIRECTORY_NOT_EMPTY_ERROR = "directory not empty" + + +def remove_path(path): + # Managed processes can recreate entries under /data/params while rm is finishing. + for attempt in range(_DELETE_RETRY_ATTEMPTS): + result = subprocess.run( + ["sudo", "rm", "-rf", "--", path], + capture_output=True, + text=True, + timeout=_DELETE_TIMEOUT_S, + check=False, + ) + if result.returncode == 0: + return + + error_text = (result.stderr or result.stdout or "sudo rm -rf failed").strip() + can_retry = _DIRECTORY_NOT_EMPTY_ERROR in error_text.lower() + if not can_retry or attempt == _DELETE_RETRY_ATTEMPTS - 1: + raise RuntimeError(f"Failed to remove {path}: {error_text}") + + time.sleep(_DELETE_RETRY_DELAY_S) diff --git a/starpilot/system/the_pond/tests/test_factory_reset.py b/starpilot/system/the_pond/tests/test_factory_reset.py new file mode 100644 index 000000000..3bdc04775 --- /dev/null +++ b/starpilot/system/the_pond/tests/test_factory_reset.py @@ -0,0 +1,44 @@ +import subprocess + +import pytest + +from openpilot.starpilot.system.the_pond import factory_reset + + +def test_remove_path_retries_directory_not_empty(monkeypatch): + results = iter( + [ + subprocess.CompletedProcess([], 1, stderr="rm: cannot remove '/data/params': Directory not empty"), + subprocess.CompletedProcess([], 0), + ] + ) + calls = [] + sleeps = [] + + def fake_run(*args, **kwargs): + calls.append((args, kwargs)) + return next(results) + + monkeypatch.setattr(factory_reset.subprocess, "run", fake_run) + monkeypatch.setattr(factory_reset.time, "sleep", sleeps.append) + + factory_reset.remove_path("/data/params") + + assert len(calls) == 2 + assert calls[0][0][0] == ["sudo", "rm", "-rf", "--", "/data/params"] + assert sleeps == [factory_reset._DELETE_RETRY_DELAY_S] + + +def test_remove_path_does_not_retry_non_transient_error(monkeypatch): + calls = [] + + def fake_run(*args, **kwargs): + calls.append((args, kwargs)) + return subprocess.CompletedProcess([], 1, stderr="rm: cannot remove '/data/params': Permission denied") + + monkeypatch.setattr(factory_reset.subprocess, "run", fake_run) + + with pytest.raises(RuntimeError, match="Permission denied"): + factory_reset.remove_path("/data/params") + + assert len(calls) == 1 diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index 4905876ec..4416a7bb9 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -78,6 +78,7 @@ from openpilot.starpilot.common.testing_grounds import ( TESTING_GROUNDS_STATE_PATH as SHARED_TESTING_GROUNDS_STATE_PATH, ) from openpilot.starpilot.navigation.destination_store import normalize_destination_payload, update_recent_destinations +from openpilot.starpilot.system.the_pond.factory_reset import remove_path as _run_factory_reset_delete from openpilot.starpilot.system.the_pond import utilities DISCORD_WEBHOOK_URL = os.getenv("DISCORD_WEBHOOK_URL") @@ -633,7 +634,6 @@ _FAST_UPDATE_REBOOT_NOTICE_SECONDS = 6.0 _FAST_UPDATE_FETCH_TIMEOUT_S = 60 _FAST_BRANCH_SWITCH_FETCH_TIMEOUT_S = 60 _FAST_ROLLBACK_FETCH_TIMEOUT_S = 60 -_FACTORY_RESET_DELETE_TIMEOUT_S = 1800 _GIT_PROGRESS_PERCENT_RE = re.compile(r'([A-Za-z][A-Za-z /_-]+):\s*([0-9]{1,3})%') _GIT_SUBMODULE_SECTION_RE = re.compile(r'^\s*\[submodule\s+"[^"]+"\]\s*$', re.MULTILINE) _ROLLBACK_REF = "refs/starpilot/rollback" @@ -1572,18 +1572,6 @@ def _set_fast_update_error_state(message, exception): progressDetail="Update failed. See Last Error below.", ) -def _run_factory_reset_delete(path): - result = subprocess.run( - ["sudo", "rm", "-rf", path], - capture_output=True, - text=True, - timeout=_FACTORY_RESET_DELETE_TIMEOUT_S, - check=False, - ) - if result.returncode != 0: - error_text = (result.stderr or result.stdout or "sudo rm -rf failed").strip() - raise RuntimeError(f"Failed to remove {path}: {error_text}") - def _factory_reset_worker(): started_at = time.time()