mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-27 17:42:04 +08:00
Errybody wants tuning
This commit is contained in:
firestar5683
@@ -6,9 +6,21 @@ from opendbc.car.honda import hondacan
|
||||
from opendbc.car.honda.values import CAR, CruiseButtons, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
|
||||
HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX = 4864
|
||||
|
||||
|
||||
def civic_bosch_modified_lateral_testing_ground_active() -> bool:
|
||||
return testing_ground.use("8", "B")
|
||||
|
||||
|
||||
def get_civic_bosch_modified_steer_can_max(base_steer_can_max: int, CP) -> int:
|
||||
if CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH and CP.dashcamOnly and civic_bosch_modified_lateral_testing_ground_active():
|
||||
return CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX
|
||||
return base_steer_can_max
|
||||
|
||||
|
||||
def compute_gb_honda_bosch(accel, speed):
|
||||
@@ -140,8 +152,12 @@ class CarController(CarControllerBase):
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
steer_can_max = get_civic_bosch_modified_steer_can_max(self.params.STEER_MAX, self.CP)
|
||||
if steer_can_max != self.params.STEER_MAX:
|
||||
apply_torque = int(np.clip(-limited_torque * steer_can_max, -steer_can_max, steer_can_max))
|
||||
else:
|
||||
apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
# Send CAN commands
|
||||
can_sends = []
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
import re
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.honda.carcontroller import CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX, get_civic_bosch_modified_steer_can_max
|
||||
from opendbc.car.honda.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL
|
||||
from opendbc.car.honda.values import CAR, HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL, HondaFlags
|
||||
|
||||
HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$"
|
||||
|
||||
@@ -16,3 +18,15 @@ class TestHondaFingerprint:
|
||||
|
||||
def test_tja_bosch_only(self):
|
||||
assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list"
|
||||
|
||||
def test_modified_civic_steer_can_max_is_scoped(self, monkeypatch):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HONDA_CIVIC_BOSCH
|
||||
CP.flags = int(HondaFlags.BOSCH)
|
||||
CP.dashcamOnly = True
|
||||
|
||||
monkeypatch.setattr("opendbc.car.honda.carcontroller.civic_bosch_modified_lateral_testing_ground_active", lambda: True)
|
||||
assert get_civic_bosch_modified_steer_can_max(4096, CP) == CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX
|
||||
|
||||
CP.dashcamOnly = False
|
||||
assert get_civic_bosch_modified_steer_can_max(4096, CP) == 4096
|
||||
|
||||
@@ -27,7 +27,9 @@ HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0
|
||||
IONIQ_6_CANFD_SCC_ACCEL_STEP = 6.0 / 50.0
|
||||
IONIQ_6_CANFD_SCC_DECEL_STEP = 15.0 / 50.0
|
||||
GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5]
|
||||
GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.30, -0.55]
|
||||
GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.36, -0.72]
|
||||
GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5]
|
||||
GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.10, 0.09, 0.08, 0.07, 0.05]
|
||||
GENESIS_G90_RELEASE_SPEED_BP = [0.0, 0.3, 0.6]
|
||||
GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.24, 0.20, 0.14]
|
||||
GENESIS_G90_RELEASE_DECEL_STEP_V = [0.30, 0.24, 0.18]
|
||||
@@ -156,7 +158,12 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt
|
||||
stopping = long_control_state == LongCtrlState.stopping
|
||||
if stopping and v_ego <= GENESIS_G90_STOP_HOLD_SPEED_BP[-1]:
|
||||
state.release_active = False
|
||||
state.actual_accel = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V))
|
||||
target_hold = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V))
|
||||
if state.actual_accel < target_hold:
|
||||
relax_step = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP, GENESIS_G90_STOP_HOLD_RELAX_STEP_V))
|
||||
state.actual_accel = min(state.actual_accel + relax_step, target_hold)
|
||||
else:
|
||||
state.actual_accel = target_hold
|
||||
else:
|
||||
if state.long_control_state_last == LongCtrlState.stopping and long_control_state == LongCtrlState.pid and \
|
||||
accel_cmd > 0.0 and v_ego < GENESIS_G90_RELEASE_MAX_SPEED:
|
||||
|
||||
@@ -313,6 +313,17 @@ class TestHyundaiFingerprint:
|
||||
assert state.actual_accel == pytest.approx(-0.12)
|
||||
assert not state.release_active
|
||||
|
||||
def test_genesis_g90_longitudinal_tuning_gradually_unwinds_into_stop_hold(self):
|
||||
state = GenesisG90LongitudinalTuningState(actual_accel=-2.0)
|
||||
|
||||
state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.5,
|
||||
long_control_state=LongCtrlState.stopping, long_active=True)
|
||||
assert state.actual_accel == pytest.approx(-1.95)
|
||||
|
||||
state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.3,
|
||||
long_control_state=LongCtrlState.stopping, long_active=True)
|
||||
assert state.actual_accel == pytest.approx(-1.88)
|
||||
|
||||
def test_genesis_g90_longitudinal_tuning_ramps_out_of_stop_hold(self):
|
||||
state = GenesisG90LongitudinalTuningState(actual_accel=-0.12, long_control_state_last=LongCtrlState.stopping)
|
||||
|
||||
@@ -563,10 +574,15 @@ class TestHyundaiFingerprint:
|
||||
ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
|
||||
|
||||
sportage_params = CarControllerParams(sportage)
|
||||
sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0)
|
||||
sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0)
|
||||
comparison_params = CarControllerParams(comparison_angle)
|
||||
ioniq6_params = CarControllerParams(ioniq6)
|
||||
|
||||
assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
|
||||
def test_ioniq_5_canfd_aux_messages_are_optional(self):
|
||||
|
||||
@@ -53,12 +53,15 @@ class CarControllerParams:
|
||||
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
|
||||
self.STEER_THRESHOLD = 175
|
||||
|
||||
# The Sportage angle port is rough at low speed on the higher global jerk limit.
|
||||
# Keep the branch-wide higher limit for other cars, but restore the older calmer
|
||||
# jerk ceiling on this port only.
|
||||
# The Sportage angle port gets rough if it keeps the branch-wide higher jerk
|
||||
# limit everywhere, but the fully calmed 3.0 m/s^3 cap lags too much in tight
|
||||
# low-speed turns. Give it extra low-speed authority, then fade back to the
|
||||
# calmer ceiling by normal road speeds.
|
||||
if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
|
||||
sportage_low_speed_weight = min(max((15.0 - vEgoRaw) / 7.0, 0.0), 1.0)
|
||||
sportage_lateral_jerk = 3.0 + (1.2 * sportage_low_speed_weight)
|
||||
self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS,
|
||||
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL))
|
||||
MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL))
|
||||
|
||||
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
|
||||
# If the max stock LKAS request is <384, add your car to this list.
|
||||
|
||||
@@ -31,6 +31,7 @@ MAX_PITCH_COMPENSATION = 1.5 # m/s^2
|
||||
TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s
|
||||
TOYOTA_COAST_BRAKE_ENABLE_ACCEL = -0.10 # m/s^2
|
||||
TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06 # m/s^2
|
||||
TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30 # m/s^2
|
||||
|
||||
# LKA limits
|
||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||
@@ -64,10 +65,13 @@ def get_long_tune(CP, params):
|
||||
|
||||
|
||||
def update_permit_braking(current: bool, net_acceleration_request_min: float, stopping: bool,
|
||||
long_active: bool, v_ego: float) -> bool:
|
||||
long_active: bool, v_ego: float, lead_visible: bool) -> bool:
|
||||
if stopping or not long_active:
|
||||
return True
|
||||
|
||||
if not lead_visible and v_ego >= TOYOTA_COAST_BRAKE_MIN_SPEED:
|
||||
return net_acceleration_request_min <= TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL
|
||||
|
||||
# At cruising speeds, some Toyota platforms turn tiny negative accel corrections
|
||||
# into noticeable brake taps. Keep a small coast-only band so mild follow/cruise
|
||||
# trims stay off the brakes while still allowing real negative requests through.
|
||||
@@ -316,7 +320,8 @@ class CarController(CarControllerBase):
|
||||
net_acceleration_request_min,
|
||||
stopping,
|
||||
CC.longActive,
|
||||
CS.out.vEgo)
|
||||
CS.out.vEgo,
|
||||
lead)
|
||||
|
||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
||||
|
||||
|
||||
@@ -222,18 +222,25 @@ class TestToyotaCarController:
|
||||
assert controller.standstill_req is False
|
||||
|
||||
def test_permit_braking_high_speed_coasts_for_tiny_decel(self):
|
||||
assert update_permit_braking(True, -0.05, False, True, 25.0) is False
|
||||
assert update_permit_braking(False, -0.05, False, True, 25.0) is False
|
||||
assert update_permit_braking(True, -0.05, False, True, 25.0, True) is False
|
||||
assert update_permit_braking(False, -0.05, False, True, 25.0, True) is False
|
||||
|
||||
def test_permit_braking_high_speed_brakes_for_meaningful_decel(self):
|
||||
assert update_permit_braking(False, -0.15, False, True, 25.0) is True
|
||||
def test_permit_braking_high_speed_brakes_for_meaningful_decel_with_lead(self):
|
||||
assert update_permit_braking(False, -0.15, False, True, 25.0, True) is True
|
||||
|
||||
def test_permit_braking_high_speed_no_lead_coasts_for_mild_decel(self):
|
||||
assert update_permit_braking(True, -0.25, False, True, 25.0, False) is False
|
||||
assert update_permit_braking(False, -0.25, False, True, 25.0, False) is False
|
||||
|
||||
def test_permit_braking_high_speed_no_lead_brakes_for_stronger_decel(self):
|
||||
assert update_permit_braking(False, -0.35, False, True, 25.0, False) is True
|
||||
|
||||
def test_permit_braking_low_speed_keeps_legacy_behavior(self):
|
||||
assert update_permit_braking(False, -0.05, False, True, 10.0) is True
|
||||
assert update_permit_braking(False, -0.05, False, True, 10.0, False) is True
|
||||
|
||||
def test_permit_braking_forces_on_when_stopping_or_inactive(self):
|
||||
assert update_permit_braking(False, 0.10, True, True, 25.0) is True
|
||||
assert update_permit_braking(False, 0.10, False, False, 25.0) is True
|
||||
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_sng_hack_clears_existing_standstill_latch(self):
|
||||
controller = self._make_controller(standstill_req=True, last_standstill=True)
|
||||
|
||||
@@ -1,10 +1,39 @@
|
||||
import math
|
||||
|
||||
from cereal import log
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
|
||||
def civic_bosch_modified_lateral_testing_ground_active() -> bool:
|
||||
return testing_ground.use("8", "B")
|
||||
|
||||
|
||||
def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float:
|
||||
abs_angle = abs(desired_angle_deg)
|
||||
if abs_angle < 8.0:
|
||||
return 1.0
|
||||
|
||||
speed_weight = min(max((v_ego - 2.0) / 8.0, 0.0), 1.0)
|
||||
angle_weight = min(max((abs_angle - 8.0) / 32.0, 0.0), 1.0)
|
||||
phase = desired_angle_deg * desired_angle_delta_deg
|
||||
|
||||
is_left = desired_angle_deg > 0.0
|
||||
base_scale = 0.02 if is_left else 0.05
|
||||
turn_in_scale = 0.02 if is_left else 0.03
|
||||
unwind_scale = 0.00 if is_left else 0.02
|
||||
|
||||
scale = 1.0 + (speed_weight * angle_weight * base_scale)
|
||||
if phase > 0.2:
|
||||
scale += speed_weight * angle_weight * turn_in_scale
|
||||
elif phase < -0.2:
|
||||
scale += speed_weight * angle_weight * unwind_scale
|
||||
|
||||
return scale
|
||||
|
||||
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CI, dt):
|
||||
super().__init__(CP, CI, dt)
|
||||
@@ -13,6 +42,8 @@ class LatControlPID(LatControl):
|
||||
pos_limit=self.steer_max, neg_limit=-self.steer_max)
|
||||
self.ff_factor = CP.lateralTuning.pid.kf
|
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function()
|
||||
self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and CP.dashcamOnly
|
||||
self.prev_angle_steers_des_no_offset = 0.0
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
@@ -28,6 +59,7 @@ class LatControlPID(LatControl):
|
||||
if not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset
|
||||
|
||||
else:
|
||||
# offset does not contribute to resistive torque
|
||||
@@ -39,11 +71,17 @@ class LatControlPID(LatControl):
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=freeze_integrator)
|
||||
|
||||
if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active():
|
||||
desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset
|
||||
output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo)
|
||||
output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = float(self.pid.p)
|
||||
pid_log.i = float(self.pid.i)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(output_torque)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset
|
||||
|
||||
return output_torque, angle_steers_des, pid_log
|
||||
|
||||
@@ -211,21 +211,21 @@ IONIQ_6_FF_CUTOFF = 0.48
|
||||
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
|
||||
IONIQ_6_TRANSITION_SPEED = 10.0
|
||||
IONIQ_6_PHASE_SCALE = 0.10
|
||||
IONIQ_6_TURN_IN_BOOST_LEFT = 0.88
|
||||
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.92
|
||||
IONIQ_6_UNWIND_TAPER_LEFT = 1.52
|
||||
IONIQ_6_UNWIND_TAPER_RIGHT = 2.92
|
||||
IONIQ_6_TURN_IN_BOOST_LEFT = 0.94
|
||||
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.00
|
||||
IONIQ_6_UNWIND_TAPER_LEFT = 1.60
|
||||
IONIQ_6_UNWIND_TAPER_RIGHT = 3.08
|
||||
IONIQ_6_FRICTION_MULT = 0.995
|
||||
IONIQ_6_FRICTION_LAT_RISE = 0.20
|
||||
IONIQ_6_FRICTION_JERK_RISE = 0.24
|
||||
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24
|
||||
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.34
|
||||
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.40
|
||||
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.10
|
||||
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.11
|
||||
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.18
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.22
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.58
|
||||
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.26
|
||||
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.38
|
||||
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.48
|
||||
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.30
|
||||
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.12
|
||||
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.20
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.32
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.82
|
||||
IONIQ_6_CENTER_TAPER_MAX = 0.042
|
||||
IONIQ_6_CENTER_TAPER_LAT = 0.18
|
||||
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
|
||||
@@ -242,8 +242,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.44
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.72
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.58
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.70
|
||||
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
|
||||
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
|
||||
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
|
||||
|
||||
@@ -44,6 +44,18 @@ VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
|
||||
VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30
|
||||
VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0
|
||||
VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9
|
||||
VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO = 0.16
|
||||
VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO = 0.24
|
||||
VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC = 16.0
|
||||
VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0
|
||||
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.25
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0
|
||||
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0
|
||||
VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.7
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1
|
||||
VISION_SLOW_LEAD_MAX_SPEED = 5.0
|
||||
VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
|
||||
VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
|
||||
@@ -342,6 +354,51 @@ class LongitudinalPlanner:
|
||||
|
||||
return max(accel_min, -approach_decel)
|
||||
|
||||
def get_vision_untracked_slow_lead_cap(self, lead, v_ego, accel_min):
|
||||
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
|
||||
return None
|
||||
|
||||
lead_prob = float(getattr(lead, "modelProb", 0.0))
|
||||
if lead_prob < VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB:
|
||||
return None
|
||||
|
||||
lead_brake = max(0.0, -float(lead.aLeadK))
|
||||
reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
|
||||
closing_speed = max(0.0, v_ego - lead.vLead)
|
||||
projected_closing_speed = closing_speed + lead_brake * reaction_t
|
||||
if projected_closing_speed < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED:
|
||||
return None
|
||||
|
||||
closing_ratio = projected_closing_speed / max(float(v_ego), 0.1)
|
||||
if closing_ratio < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO:
|
||||
return None
|
||||
|
||||
max_distance = float(np.clip(VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME * v_ego,
|
||||
VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE,
|
||||
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE))
|
||||
if float(lead.dRel) > max_distance:
|
||||
return None
|
||||
|
||||
projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1)
|
||||
if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC:
|
||||
return None
|
||||
|
||||
time_factor = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - projected_ttc) /
|
||||
(VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC),
|
||||
0.0, 1.0))
|
||||
prob_factor = float(np.clip((lead_prob - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB) /
|
||||
(VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB),
|
||||
0.0, 1.0))
|
||||
closing_factor = float(np.clip((closing_ratio - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO) /
|
||||
(VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO),
|
||||
0.0, 1.0))
|
||||
approach_decel = VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL * np.clip(
|
||||
0.5 * time_factor + 0.3 * prob_factor + 0.2 * closing_factor, 0.0, 1.0)
|
||||
if approach_decel < VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL:
|
||||
return None
|
||||
|
||||
return max(accel_min, -approach_decel)
|
||||
|
||||
def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow):
|
||||
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
|
||||
return None
|
||||
@@ -742,6 +799,19 @@ class LongitudinalPlanner:
|
||||
vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))
|
||||
output_accel_min = comfort_output_accel_min
|
||||
|
||||
if not tracking_lead:
|
||||
pretracking_vision_caps = []
|
||||
for lead in (self.lead_one, self.lead_two):
|
||||
if lead.status and not bool(getattr(lead, "radar", False)):
|
||||
pretracking_cap = self.get_vision_untracked_slow_lead_cap(lead, v_ego, vision_cap_accel_min)
|
||||
if pretracking_cap is not None:
|
||||
pretracking_vision_caps.append(pretracking_cap)
|
||||
|
||||
if pretracking_vision_caps:
|
||||
pretracking_vision_cap = min(pretracking_vision_caps)
|
||||
self.a_desired = min(self.a_desired, pretracking_vision_cap)
|
||||
output_a_target = min(output_a_target, pretracking_vision_cap)
|
||||
|
||||
close_lead_caps = []
|
||||
vision_low_speed_stop_active = False
|
||||
vision_brake_cap_active = False
|
||||
|
||||
@@ -3,6 +3,7 @@ from types import SimpleNamespace
|
||||
|
||||
from cereal import car, custom, log
|
||||
import openpilot.selfdrive.controls.lib.latcontrol_torque as latcontrol_torque
|
||||
import openpilot.selfdrive.controls.lib.latcontrol_pid as latcontrol_pid
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
@@ -12,7 +13,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID, get_civic_bosch_modified_pid_output_scale
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
LatControlTorque,
|
||||
get_bolt_2017_center_taper_scale,
|
||||
@@ -73,6 +74,30 @@ class TestLatControl:
|
||||
starpilot_toggles = SimpleNamespace()
|
||||
return controller, VM, CS, params, starpilot_toggles
|
||||
|
||||
@staticmethod
|
||||
def _build_pid_controller(car_name):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP.dashcamOnly = True
|
||||
CI = CarInterface(CP, custom.StarPilotCarParams.new_message())
|
||||
controller = LatControlPID(CP.as_reader(), CI, DT_CTRL)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 12
|
||||
CS.steeringPressed = False
|
||||
CS.steeringAngleDeg = -6.0
|
||||
CS.steeringRateDeg = 0.0
|
||||
|
||||
params = log.LiveParametersData.new_message()
|
||||
params.steerRatio = CP.steerRatio
|
||||
params.stiffnessFactor = 1.0
|
||||
params.roll = 0.0
|
||||
params.angleOffsetDeg = 0.0
|
||||
|
||||
starpilot_toggles = SimpleNamespace()
|
||||
return controller, VM, CS, params, starpilot_toggles
|
||||
|
||||
def test_bolt_2017_testing_ground_scale_curve(self):
|
||||
assert get_bolt_2017_base_torque_scale(0.1) == 1.0
|
||||
assert get_bolt_2017_base_torque_scale(-0.1) == 1.0
|
||||
@@ -354,6 +379,25 @@ class TestLatControl:
|
||||
|
||||
assert lac_log.active
|
||||
|
||||
def test_civic_bosch_modified_pid_scale_curve(self):
|
||||
assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) == 1.0
|
||||
assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0)
|
||||
assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0)
|
||||
assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0)
|
||||
assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0)
|
||||
|
||||
def test_civic_bosch_modified_pid_testing_ground_update_path(self, monkeypatch):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(HONDA.HONDA_CIVIC_BOSCH)
|
||||
monkeypatch.setattr(latcontrol_pid, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)
|
||||
|
||||
base_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.004, False, 0.2, None, None, starpilot_toggles)
|
||||
assert lac_log.active
|
||||
|
||||
# Use a second update to create a turn-in phase with a larger desired angle delta.
|
||||
tuned_output, _, _ = controller.update(True, CS, VM, params, False, 0.006, False, 0.2, None, None, starpilot_toggles)
|
||||
|
||||
assert abs(tuned_output) >= abs(base_output)
|
||||
|
||||
@parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
|
||||
(NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)])
|
||||
def test_saturation(self, car_name, controller):
|
||||
|
||||
@@ -237,6 +237,23 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
|
||||
assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None
|
||||
|
||||
|
||||
def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_case():
|
||||
route_v_ego = 23.23
|
||||
far_v_ego = 29.0
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=route_v_ego)
|
||||
route_like_lead = make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92)
|
||||
far_mild_lead = make_lead(status=True, d_rel=82.0, v_lead=25.0, a_lead=0.0, radar=False, model_prob=0.9)
|
||||
|
||||
route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, route_v_ego, -1.0)
|
||||
far_cap = planner.get_vision_untracked_slow_lead_cap(far_mild_lead, far_v_ego, -1.0)
|
||||
|
||||
assert route_cap is not None
|
||||
assert route_cap < -0.1
|
||||
assert far_cap is None
|
||||
|
||||
|
||||
def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop():
|
||||
v_ego = 13.207
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
@@ -370,6 +387,41 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers
|
||||
assert planner_close.output_a_target < planner_approach.output_a_target - 0.25
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
|
||||
def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_version):
|
||||
v_ego = 23.23
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
sm_no_lead = make_sm(
|
||||
v_ego,
|
||||
desired_accel=0.2,
|
||||
min_accel=-0.5,
|
||||
experimental_mode=False,
|
||||
tracking_lead=False,
|
||||
)
|
||||
sm_with_lead = make_sm(
|
||||
v_ego,
|
||||
desired_accel=0.2,
|
||||
min_accel=-0.5,
|
||||
experimental_mode=False,
|
||||
tracking_lead=False,
|
||||
lead_one=make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92),
|
||||
)
|
||||
sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0
|
||||
sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0
|
||||
|
||||
for _ in range(6):
|
||||
planner_no_lead.update(sm_no_lead, make_toggles(model_version))
|
||||
planner_with_lead.update(sm_with_lead, make_toggles(model_version))
|
||||
|
||||
assert planner_with_lead.mode == "acc"
|
||||
assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego)
|
||||
assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.04
|
||||
assert planner_with_lead.output_a_target < -0.2
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
|
||||
def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version):
|
||||
v_ego = 3.8
|
||||
|
||||
@@ -14,6 +14,7 @@ TESTING_GROUND_4 = "4"
|
||||
TESTING_GROUND_5 = "5"
|
||||
TESTING_GROUND_6 = "6"
|
||||
TESTING_GROUND_7 = "7"
|
||||
TESTING_GROUND_8 = "8"
|
||||
|
||||
TESTING_GROUND_IDS = (
|
||||
TESTING_GROUND_1,
|
||||
@@ -23,6 +24,7 @@ TESTING_GROUND_IDS = (
|
||||
TESTING_GROUND_5,
|
||||
TESTING_GROUND_6,
|
||||
TESTING_GROUND_7,
|
||||
TESTING_GROUND_8,
|
||||
)
|
||||
|
||||
TESTING_GROUNDS_STATE_PATH = Path("/tmp/the_pond_testing_grounds_slots.json") if PC else Path("/data/testing_grounds/slots.json")
|
||||
@@ -80,6 +82,13 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = (
|
||||
"aLabel": "A - Installed tune",
|
||||
"bLabel": "B - Firestar Tune",
|
||||
},
|
||||
{
|
||||
"id": TESTING_GROUND_8,
|
||||
"name": "Modified Civic Lateral",
|
||||
"description": "Modified Honda Civic Bosch lateral sandbox.",
|
||||
"aLabel": "A - Installed tune",
|
||||
"bLabel": "B - Firestar Tune",
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
@@ -248,6 +257,7 @@ use_testing_ground_4 = _TestingGroundFlag(TESTING_GROUND_4)
|
||||
use_testing_ground_5 = _TestingGroundFlag(TESTING_GROUND_5)
|
||||
use_testing_ground_6 = _TestingGroundFlag(TESTING_GROUND_6)
|
||||
use_testing_ground_7 = _TestingGroundFlag(TESTING_GROUND_7)
|
||||
use_testing_ground_8 = _TestingGroundFlag(TESTING_GROUND_8)
|
||||
|
||||
|
||||
class _TestingGroundNamespace:
|
||||
@@ -260,6 +270,7 @@ class _TestingGroundNamespace:
|
||||
id_5 = TESTING_GROUND_5
|
||||
id_6 = TESTING_GROUND_6
|
||||
id_7 = TESTING_GROUND_7
|
||||
id_8 = TESTING_GROUND_8
|
||||
|
||||
use_1 = use_testing_ground_1
|
||||
use_2 = use_testing_ground_2
|
||||
@@ -268,6 +279,7 @@ class _TestingGroundNamespace:
|
||||
use_5 = use_testing_ground_5
|
||||
use_6 = use_testing_ground_6
|
||||
use_7 = use_testing_ground_7
|
||||
use_8 = use_testing_ground_8
|
||||
|
||||
def use(self, slot_id, variant=TESTING_GROUND_TEST_VARIANT):
|
||||
return is_testing_ground_active(slot_id, variant)
|
||||
|
||||
Reference in New Issue
Block a user