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

PlotWranglerPro

Browse Source
This commit is contained in:
firestar5683
2026-06-28 22:46:07 -05:00
parent 2e53b419d1
commit e88854319c
6 changed files with 44 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/gm/carcontroller.py
+1 -1
View File
@@ -356,7 +356,7 @@ def shape_bolt_acc_pedal_low_speed_friction(apply_brake: int, v_ego: float, stop
return 0, False
if stopping:
stop_fade = float(np.interp(v_ego, [0.0, 0.5, 1.0, 2.0], [0.0, 0.0, 0.45, 0.85]))
stop_fade = float(np.interp(v_ego, [0.0, 0.6, 0.9, 1.2, 1.8, 2.8], [0.0, 0.0, 0.05, 0.12, 0.32, 0.78]))
apply_brake = int(round(apply_brake * stop_fade))
if apply_brake <= 0 or apply_brake < release_threshold:
return 0, False
opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py
+7
View File
@@ -208,6 +208,13 @@ def test_bolt_acc_pedal_low_speed_friction_preserves_rolling_stop_authority():
assert active
def test_bolt_acc_pedal_low_speed_friction_drops_out_before_two_clamp():
apply_brake, active = shape_bolt_acc_pedal_low_speed_friction(80, 0.9, True, True)
assert apply_brake == 0
assert not active
def test_bolt_pedal_long_accel_limit_matches_planner_regen_envelope():
assert get_bolt_pedal_long_accel_limit(6.66) == pytest.approx(-2.379, abs=1e-3)
assert get_bolt_pedal_long_accel_limit(3.0) == pytest.approx(-1.70, abs=1e-3)
selfdrive/controls/lib/latcontrol_torque.py
+3 -1
View File
@@ -4,6 +4,7 @@ from collections import deque
from cereal import log
from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.car.lateral import get_friction
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
@@ -86,6 +87,7 @@ class LatControlTorque(LatControl):
self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS
self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
self.is_silverado = CP.carFingerprint in SILVERADO_CARS
self.is_hkg_canfd_torque = CP.brand == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD)
if self.is_ioniq_6:
self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED)
self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017
@@ -239,7 +241,7 @@ class LatControlTorque(LatControl):
civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if (
self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active()
) else 1.0
friction_threshold = get_friction_threshold(CS.vEgo)
friction_threshold = get_hkg_canfd_base_friction_threshold(CS.vEgo) if self.is_hkg_canfd_torque else get_friction_threshold(CS.vEgo)
friction_scale = 1.0
if bolt_2022_2023_tuned_path_active:
ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+9 -4
View File
@@ -8,6 +8,7 @@ from openpilot.common.constants import CV
from openpilot.starpilot.common.testing_grounds import testing_ground
CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39
CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75
@@ -455,7 +456,7 @@ IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2
IONIQ_6_FF_GAIN_LEFT = 0.045
IONIQ_6_FF_GAIN_RIGHT = 0.015
IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
IONIQ_6_BASE_FRICTION_THRESHOLD = 0.36
IONIQ_6_BASE_FRICTION_THRESHOLD = HKG_CANFD_BASE_FRICTION_THRESHOLD
IONIQ_6_FF_ONSET = 0.10
IONIQ_6_FF_ONSET_WIDTH = 0.04
IONIQ_6_FF_CUTOFF = 0.48
@@ -659,6 +660,10 @@ 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 get_hkg_canfd_base_friction_threshold(v_ego: float) -> float:
return max(get_friction_threshold(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD)
def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float:
load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0)
speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0])
@@ -1631,7 +1636,7 @@ def get_ioniq_5_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
def get_ioniq_5_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)
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1771,7 +1776,7 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = max(get_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD)
base_threshold = max(get_hkg_canfd_base_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD)
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1964,7 +1969,7 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
def get_kia_ev6_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)
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
selfdrive/controls/lib/longcontrol.py
+17
View File
@@ -32,6 +32,19 @@ def apply_deadzone(error, deadzone):
return error
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
if output_accel >= -0.05 or a_target >= -0.80 or v_ego <= 5.0:
return 0.0
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if authority_gap <= 0.25:
return 0.0
speed_factor = interp(v_ego, [5.0, 10.0, 15.0, 25.0], [0.0, 0.55, 0.85, 1.0])
max_bias = interp(abs(a_target), [0.8, 1.4, 2.2, 3.5], [0.0, 0.14, 0.42, 0.70])
return float(min(authority_gap * 0.30, max_bias) * speed_factor)
def long_control_state_trans(CP, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill, starpilot_toggles,
allow_stopping_release=True):
@@ -301,6 +314,10 @@ class LongControl:
return output_accel
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if self.is_bolt_acc_pedal_friction_car:
bias = get_bolt_acc_pedal_friction_bias(output_accel, a_target, CS.vEgo)
return output_accel - float(bias)
if authority_gap <= 0.40:
return output_accel
selfdrive/controls/tests/test_latcontrol.py
+7 -4
View File
@@ -19,6 +19,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import (
get_civic_bosch_modified_pid_output_alpha,
get_civic_bosch_modified_pid_output_scale,
)
from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_hkg_canfd_base_friction_threshold
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_civic_bosch_modified_a_center_taper_scale,
LatControlTorque,
@@ -432,7 +433,7 @@ class TestLatControl:
assert unwind_right < unwind_left
def test_ioniq_5_friction_curves(self):
base = get_friction_threshold(12.0)
base = get_hkg_canfd_base_friction_threshold(12.0)
turn_in_left_threshold = get_ioniq_5_friction_threshold(12.0, 0.7, 0.8)
turn_in_right_threshold = get_ioniq_5_friction_threshold(12.0, -0.7, -0.8)
unwind_left_threshold = get_ioniq_5_friction_threshold(12.0, 0.7, -0.8)
@@ -449,6 +450,7 @@ class TestLatControl:
assert turn_in_left_scale > turn_in_right_scale > 1.0
assert unwind_left_scale < 1.0
assert unwind_right_scale <= unwind_left_scale
assert get_ioniq_5_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0)
def test_ioniq_5_center_taper_curve(self):
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
@@ -519,7 +521,7 @@ class TestLatControl:
assert get_ioniq_6_output_taper_scale(-1.2, 0.7, 25.0) <= get_ioniq_6_output_taper_scale(-1.2, 0.0, 25.0)
def test_ioniq_6_friction_threshold_curve(self):
base = max(get_friction_threshold(6.0), 0.36)
base = get_hkg_canfd_base_friction_threshold(6.0)
left_turn_in = get_ioniq_6_friction_threshold(6.0, 0.5, 0.8)
right_turn_in = get_ioniq_6_friction_threshold(6.0, -0.5, -0.8)
left_unwind = get_ioniq_6_friction_threshold(6.0, 0.5, -0.8)
@@ -527,7 +529,7 @@ class TestLatControl:
assert max(left_turn_in, right_turn_in) < base
assert left_unwind >= base
assert right_unwind >= base
assert get_ioniq_6_friction_threshold(25.0, 0.0, 0.0) >= 0.36
assert get_ioniq_6_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0)
def test_ioniq_6_friction_scale_curve(self):
base = get_ioniq_6_friction_scale(25.0, 0.5, 0.8)
@@ -555,12 +557,13 @@ class TestLatControl:
assert get_kia_ev6_ff_scale(1.2, 0.0, 20.0) < get_kia_ev6_ff_scale(0.4, 0.0, 20.0)
def test_kia_ev6_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_hkg_canfd_base_friction_threshold(6.0)
left_turn_in = get_kia_ev6_friction_threshold(6.0, 0.5, 0.8)
right_turn_in = get_kia_ev6_friction_threshold(6.0, -0.5, -0.8)
left_unwind = get_kia_ev6_friction_threshold(6.0, 0.5, -0.8)
right_unwind = get_kia_ev6_friction_threshold(6.0, -0.5, 0.8)
assert right_turn_in < left_turn_in < base < right_unwind <= left_unwind
assert get_kia_ev6_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0)
def test_kia_ev6_friction_scale_curve(self):
base = get_kia_ev6_friction_scale(25.0, 0.5, 0.8)