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

update

Browse Source
This commit is contained in:
firestar5683
2026-05-02 13:29:21 -05:00
parent 1a016da5fa
commit fc4a53c02e
7 changed files with 110 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+11 -11
View File
@@ -14,25 +14,25 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool:
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)
speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0)
center_weight = min(max((4.0 - abs_angle) / 4.0, 0.0), 1.0)
angle_weight = min(max((abs_angle - 8.0) / 24.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
center_taper = 0.06
base_scale = 0.08 if is_left else 0.10
turn_in_scale = 0.08 if is_left else 0.10
unwind_scale = 0.10 if is_left else 0.14
scale = 1.0 + (speed_weight * angle_weight * base_scale)
scale = 1.0 - (speed_weight * center_weight * center_taper)
scale += 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
scale -= speed_weight * angle_weight * unwind_scale
return scale
return max(scale, 0.84)
class LatControlPID(LatControl):
selfdrive/controls/lib/latcontrol_torque.py
+6 -6
View File
@@ -211,19 +211,19 @@ 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.96
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.00
IONIQ_6_TURN_IN_BOOST_LEFT = 1.02
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.12
IONIQ_6_UNWIND_TAPER_LEFT = 1.52
IONIQ_6_UNWIND_TAPER_RIGHT = 2.92
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.29
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.31
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.46
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.60
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.50
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.14
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.22
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.16
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.28
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.38
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 3.00
IONIQ_6_CENTER_TAPER_MAX = 0.046
selfdrive/controls/tests/test_conditional_experimental_mode.py
+54 -2
View File
@@ -57,8 +57,8 @@ def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = Fal
def make_update_sm(*, standstill: bool):
return {
"carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=False),
"carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=False, dashboardStopSign=0, accelPressed=False),
}
@@ -206,6 +206,21 @@ def test_standstill_update_can_activate_exp_from_red_light_detection(monkeypatch
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_green_light_clears_exp_immediately(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
@@ -259,6 +274,43 @@ def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monke
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_stop_sign_latches_until_pedal_even_after_force_stop_ends(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
sm["starpilotCarState"].dashboardStopSign = 0
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
def test_standstill_stop_sign_releases_on_pedal(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
sm["starpilotCarState"].dashboardStopSign = 0
sm["carState"].gasPressed = True
cem.update(0.0, sm, toggles)
assert not cem.experimental_mode
assert cem.standstill_stop_reason is None
def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead():
v_ego = 35 * CV.MPH_TO_MS
cem = make_cem(
selfdrive/controls/tests/test_latcontrol.py
+5 -3
View File
@@ -380,11 +380,13 @@ 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(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)
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)