mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-28 18:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

The fields speak to me

Browse Source
This commit is contained in:
firestar5683
2026-06-20 18:04:39 -05:00
parent fc90fcb879
commit 97881b5c66
3 changed files with 57 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+35 -20
View File
@@ -536,13 +536,13 @@ IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19
IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.13
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.82
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.15
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.16
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
@@ -551,21 +551,21 @@ IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.12
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.16
IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.38
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 4.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.55
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 3.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.5
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 8.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 3.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.4, 0.7, 1.0]
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.75, 0.0]
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.14
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.19
IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.0
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.9
IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.08
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.045
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.42
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 4.5
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.65
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 2.4
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.35
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 6.5
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.6
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0]
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.82, 0.0]
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
@@ -575,9 +575,9 @@ IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.06
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.62
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.84
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.18
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
@@ -614,6 +614,11 @@ KIA_EV6_CENTER_TAPER_LAT = 0.16
KIA_EV6_CENTER_TAPER_LAT_WIDTH = 0.04
KIA_EV6_CENTER_TAPER_SPEED = 17.0
KIA_EV6_CENTER_TAPER_SPEED_WIDTH = 2.8
KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.24
KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08
KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4
VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7
VOLT_PLEXY_FF_GAIN_LEFT = 0.12
@@ -1979,6 +1984,13 @@ def get_kia_ev6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -
return 1.0 - reduction
def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
center_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def volt_plexy_lateral_testing_ground_active() -> bool:
return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID)
@@ -2239,6 +2251,7 @@ class LatControlTorque(LatControl):
kia_niro_phev_2022_center_taper = get_kia_niro_phev_2022_center_taper_scale(setpoint, CS.vEgo) if kia_niro_phev_2022_active else 1.0
kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0
kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0
kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0
silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0
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()
@@ -2338,6 +2351,8 @@ class LatControlTorque(LatControl):
output_torque, CS.vEgo)
elif volt_standard_test_active:
output_torque *= volt_standard_center_taper
elif kia_ev6_test_active:
output_torque *= kia_ev6_low_speed_center_taper
elif self.is_silverado:
output_torque *= silverado_center_taper
elif kia_niro_phev_2022_active:
selfdrive/controls/lib/longitudinal_planner.py
+1 -1
View File
@@ -1446,7 +1446,7 @@ class LongitudinalPlanner:
if gap_error > gap_buffer:
return None
if current_source == "cruise" and tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
return None
if current_source == "cruise" and self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active):
return None
selfdrive/controls/tests/test_longitudinal_planner.py
+21
View File
@@ -2447,6 +2447,27 @@ def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelera
assert cap is None
def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_flips_to_lead0():
v_ego = 24.361867904663086
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=38.5, v_lead=24.46365737915039,
a_lead=0.4625629186630249, radar=True, model_prob=1.0, y_rel=0.0,
)
cap = planner.get_lead_catchup_accel_cap(
lead,
v_ego,
1.3549551963806152,
current_source="lead0",
tracking_lead_active=True,
)
assert planner.is_comfortable_accelerating_away_follow(lead, v_ego, 1.3549551963806152)
assert cap is None
def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave():
v_ego = 22.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)