mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-07-26 20:32:04 +08:00
mid day stuffs
This commit is contained in:
firestar5683
@@ -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))
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user