diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index b1230f241..2a6b40484 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -23,9 +23,9 @@ def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_ is_left = desired_angle_deg > 0.0 center_taper = 0.32 - mid_turn_scale = 0.08 if is_left else -0.06 - mid_turn_turn_in_scale = 0.05 if is_left else -0.04 - mid_turn_unwind_scale = -0.03 if is_left else 0.06 + mid_turn_scale = 0.12 if is_left else -0.10 + mid_turn_turn_in_scale = 0.08 if is_left else -0.08 + mid_turn_unwind_scale = -0.05 if is_left else -0.08 base_scale = 0.12 if is_left else 0.10 turn_in_scale = 0.12 if is_left else 0.12 unwind_scale = 0.14 if is_left else 0.18 @@ -46,20 +46,22 @@ def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_ def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float, output_torque: float, prev_output_torque: float) -> float: abs_angle = abs(desired_angle_deg) - if abs_angle < 3.0 or abs_angle > 22.0: + if abs_angle < 0.75 or abs_angle > 22.0: return 1.0 speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) - onset = min(max((abs_angle - 3.0) / 5.0, 0.0), 1.0) + onset = min(max((abs_angle - 0.75) / 5.25, 0.0), 1.0) cutoff = min(max((22.0 - abs_angle) / 8.0, 0.0), 1.0) band_weight = onset * cutoff + small_curve_weight = min(max((10.0 - abs_angle) / 4.0, 0.0), 1.0) large_turn_weight = min(max((abs_angle - 16.0) / 6.0, 0.0), 1.0) transition_weight = min(abs(desired_angle_delta_deg) / 0.35, 1.0) sign_change_weight = 1.0 if (output_torque * prev_output_torque) < 0.0 else 0.0 smoothing = band_weight * (0.36 + (0.22 * speed_weight) + (0.12 * transition_weight) + (0.12 * sign_change_weight)) + smoothing *= 1.0 + (0.25 * small_curve_weight) smoothing *= 1.0 - (0.50 * large_turn_weight) - return min(max(1.0 - smoothing, 0.18), 1.0) + return min(max(1.0 - smoothing, 0.16), 1.0) class LatControlPID(LatControl): diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 8d7ca78ca..81b047a44 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -395,7 +395,7 @@ class TestLatControl: 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.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.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) diff --git a/system/manager/launch_param_migrations.py b/system/manager/launch_param_migrations.py index 97613b543..33f80f06c 100644 --- a/system/manager/launch_param_migrations.py +++ b/system/manager/launch_param_migrations.py @@ -17,38 +17,41 @@ QT_STEER_KP_PLACEHOLDER = 1.0 LAUNCH_PARAM_MIGRATION_MARKER = ".starpilot_launch_param_migrations_v2" BRANCH_DEFAULTS_MIGRATION_MARKER = ".starpilot_branch_defaults_migrations_v1" ACCELERATION_PROFILE_MIGRATION_MARKER = ".starpilot_acceleration_profile_default_v1" +MARKER_DIRNAME = ".starpilot_param_migrations" + +LEGACY_CE_STOPPED_LEAD_DEFAULT = True +LEGACY_FORCE_STOPS_DEFAULT = False +LEGACY_AGGRESSIVE_FOLLOW_HIGH_DEFAULT = 1.25 +LEGACY_STANDARD_FOLLOW_HIGH_DEFAULT = 1.45 +LEGACY_RELAXED_FOLLOW_DEFAULT = 1.75 +LEGACY_RELAXED_FOLLOW_HIGH_DEFAULT = 1.75 +LEGACY_JERK_DEFAULT = 50.0 +LEGACY_ACCELERATION_PROFILE_DEFAULT = 2 + STANDARD_ACCELERATION_PROFILE = 0 -BRANCH_BOOL_DEFAULTS = { - "ConditionalExperimental": True, - "CELead": True, - "CESlowerLead": True, - "CEStoppedLead": False, - "ForceStops": True, +BRANCH_BOOL_MIGRATIONS = { + "CEStoppedLead": (LEGACY_CE_STOPPED_LEAD_DEFAULT, False), + "ForceStops": (LEGACY_FORCE_STOPS_DEFAULT, True), } -BRANCH_FLOAT_DEFAULTS = { - "AggressiveFollow": 1.25, - "AggressiveFollowHigh": 1.0, - "AggressiveJerkAcceleration": 50.0, - "AggressiveJerkDanger": 100.0, - "AggressiveJerkDeceleration": 50.0, - "AggressiveJerkSpeed": 50.0, - "AggressiveJerkSpeedDecrease": 50.0, - "StandardFollow": 1.45, - "StandardFollowHigh": 1.2, - "StandardJerkAcceleration": 100.0, - "StandardJerkDanger": 100.0, - "StandardJerkDeceleration": 100.0, - "StandardJerkSpeed": 100.0, - "StandardJerkSpeedDecrease": 100.0, - "RelaxedFollow": 1.6, - "RelaxedFollowHigh": 1.4, - "RelaxedJerkAcceleration": 100.0, - "RelaxedJerkDanger": 100.0, - "RelaxedJerkDeceleration": 100.0, - "RelaxedJerkSpeed": 100.0, - "RelaxedJerkSpeedDecrease": 100.0, +BRANCH_FLOAT_MIGRATIONS = { + "AggressiveFollowHigh": (LEGACY_AGGRESSIVE_FOLLOW_HIGH_DEFAULT, 1.0), + "StandardFollowHigh": (LEGACY_STANDARD_FOLLOW_HIGH_DEFAULT, 1.2), + "StandardJerkAcceleration": (LEGACY_JERK_DEFAULT, 100.0), + "StandardJerkDeceleration": (LEGACY_JERK_DEFAULT, 100.0), + "StandardJerkSpeed": (LEGACY_JERK_DEFAULT, 100.0), + "StandardJerkSpeedDecrease": (LEGACY_JERK_DEFAULT, 100.0), + "RelaxedFollow": (LEGACY_RELAXED_FOLLOW_DEFAULT, 1.6), + "RelaxedFollowHigh": (LEGACY_RELAXED_FOLLOW_HIGH_DEFAULT, 1.4), + "RelaxedJerkAcceleration": (LEGACY_JERK_DEFAULT, 100.0), + "RelaxedJerkDeceleration": (LEGACY_JERK_DEFAULT, 100.0), + "RelaxedJerkSpeed": (LEGACY_JERK_DEFAULT, 100.0), + "RelaxedJerkSpeedDecrease": (LEGACY_JERK_DEFAULT, 100.0), +} + +ACCELERATION_PROFILE_MIGRATION = { + "AccelerationProfile": (LEGACY_ACCELERATION_PROFILE_DEFAULT, STANDARD_ACCELERATION_PROFILE), } @@ -67,15 +70,38 @@ def _approx_equal(lhs: float, rhs: float, tolerance: float = 1e-6) -> bool: def _default_marker_path(params: ParamsLike) -> Path: - return Path(params.get_param_path()) / LAUNCH_PARAM_MIGRATION_MARKER + return _marker_dir_path(params) / LAUNCH_PARAM_MIGRATION_MARKER def _branch_defaults_marker_path(params: ParamsLike) -> Path: - return Path(params.get_param_path()) / BRANCH_DEFAULTS_MIGRATION_MARKER + return _marker_dir_path(params) / BRANCH_DEFAULTS_MIGRATION_MARKER def _acceleration_profile_marker_path(params: ParamsLike) -> Path: - return Path(params.get_param_path()) / ACCELERATION_PROFILE_MIGRATION_MARKER + return _marker_dir_path(params) / ACCELERATION_PROFILE_MIGRATION_MARKER + + +def _marker_dir_path(params: ParamsLike) -> Path: + params_path = Path(params.get_param_path()) + # Params.clear_all() removes unknown files inside the params directory, so + # one-time migration markers must live alongside it, not inside it. + return params_path.parent / MARKER_DIRNAME / params_path.name + + +def _param_file_exists(params: ParamsLike, key: str) -> bool: + return Path(params.get_param_path(key)).exists() + + +def _should_migrate_bool_param(params: ParamsLike, key: str, legacy_default: bool) -> bool: + return not _param_file_exists(params, key) or params.get_bool(key) == legacy_default + + +def _should_migrate_int_param(params: ParamsLike, key: str, legacy_default: int) -> bool: + return not _param_file_exists(params, key) or params.get_int(key) == legacy_default + + +def _should_migrate_float_param(params: ParamsLike, key: str, legacy_default: float) -> bool: + return not _param_file_exists(params, key) or _approx_equal(params.get_float(key), legacy_default) def _apply_legacy_launch_param_migrations(params: ParamsLike, marker: Path) -> None: @@ -111,11 +137,13 @@ def _apply_branch_default_migration(params: ParamsLike, marker: Path) -> None: marker.parent.mkdir(parents=True, exist_ok=True) - for key, value in BRANCH_BOOL_DEFAULTS.items(): - params.put_bool(key, value) + for key, (legacy_default, new_default) in BRANCH_BOOL_MIGRATIONS.items(): + if _should_migrate_bool_param(params, key, legacy_default): + params.put_bool(key, new_default) - for key, value in BRANCH_FLOAT_DEFAULTS.items(): - params.put_float(key, value) + for key, (legacy_default, new_default) in BRANCH_FLOAT_MIGRATIONS.items(): + if _should_migrate_float_param(params, key, legacy_default): + params.put_float(key, new_default) marker.touch() @@ -125,7 +153,11 @@ def _apply_acceleration_profile_default_migration(params: ParamsLike, marker: Pa return marker.parent.mkdir(parents=True, exist_ok=True) - params.put_int("AccelerationProfile", STANDARD_ACCELERATION_PROFILE) + + for key, (legacy_default, new_default) in ACCELERATION_PROFILE_MIGRATION.items(): + if _should_migrate_int_param(params, key, legacy_default): + params.put_int(key, new_default) + marker.touch() diff --git a/system/manager/test/test_launch_param_migrations.py b/system/manager/test/test_launch_param_migrations.py index 61e10eb3c..5f1701948 100644 --- a/system/manager/test/test_launch_param_migrations.py +++ b/system/manager/test/test_launch_param_migrations.py @@ -5,6 +5,7 @@ from openpilot.system.manager.launch_param_migrations import ( BRANCH_DEFAULTS_MIGRATION_MARKER, DEFAULT_STEER_KP, LAUNCH_PARAM_MIGRATION_MARKER, + MARKER_DIRNAME, STANDARD_ACCELERATION_PROFILE, apply_launch_param_migrations, ) @@ -48,6 +49,10 @@ class FileBackedFakeParams: Path(self.get_param_path(key)).write_text(str(float(value)), encoding="utf-8") +def marker_path(tmp_path: Path, marker_name: str) -> Path: + return tmp_path / MARKER_DIRNAME / "params" / marker_name + + def test_apply_launch_param_migrations_sets_branch_defaults_once(tmp_path): params = FileBackedFakeParams(tmp_path / "params") @@ -60,7 +65,7 @@ def test_apply_launch_param_migrations_sets_branch_defaults_once(tmp_path): assert params.get_bool("LongPitch") assert params.get_float("SteerKP") == DEFAULT_STEER_KP assert params.get_float("SteerKPStock") == DEFAULT_STEER_KP - assert (tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER).is_file() + assert marker_path(tmp_path, LAUNCH_PARAM_MIGRATION_MARKER).is_file() def test_apply_launch_param_migrations_initializes_use_prebuilt(tmp_path): @@ -82,7 +87,7 @@ def test_apply_launch_param_migrations_does_not_overwrite_use_prebuilt(tmp_path) def test_apply_launch_param_migrations_does_not_reapply_after_marker(tmp_path): params = FileBackedFakeParams(tmp_path / "params") - marker = tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER + marker = marker_path(tmp_path, LAUNCH_PARAM_MIGRATION_MARKER) params.put_bool("LongPitch", False) params.put_float("SteerKP", 0.65) @@ -100,9 +105,6 @@ def test_apply_launch_param_migrations_applies_branch_defaults_for_existing_inst params = FileBackedFakeParams(tmp_path / "params") params.put_bool("LongPitch", False) - params.put_bool("ConditionalExperimental", False) - params.put_bool("CELead", False) - params.put_bool("CESlowerLead", False) params.put_bool("CEStoppedLead", True) params.put_bool("ForceStops", False) params.put_float("AggressiveFollowHigh", 1.25) @@ -111,14 +113,11 @@ def test_apply_launch_param_migrations_applies_branch_defaults_for_existing_inst params.put_float("RelaxedFollow", 1.75) params.put_float("RelaxedFollowHigh", 1.75) params.put_float("RelaxedJerkSpeed", 50.0) - (tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER).touch() + marker_path(tmp_path, LAUNCH_PARAM_MIGRATION_MARKER).touch() apply_launch_param_migrations(params) assert not params.get_bool("LongPitch") - assert params.get_bool("ConditionalExperimental") - assert params.get_bool("CELead") - assert params.get_bool("CESlowerLead") assert not params.get_bool("CEStoppedLead") assert params.get_bool("ForceStops") assert params.get_float("AggressiveFollowHigh") == 1.0 @@ -127,12 +126,12 @@ def test_apply_launch_param_migrations_applies_branch_defaults_for_existing_inst assert params.get_float("RelaxedFollow") == 1.6 assert params.get_float("RelaxedFollowHigh") == 1.4 assert params.get_float("RelaxedJerkSpeed") == 100.0 - assert (tmp_path / "params" / BRANCH_DEFAULTS_MIGRATION_MARKER).is_file() + assert marker_path(tmp_path, BRANCH_DEFAULTS_MIGRATION_MARKER).is_file() def test_apply_launch_param_migrations_does_not_reapply_branch_defaults_after_marker(tmp_path): params = FileBackedFakeParams(tmp_path / "params") - branch_defaults_marker = tmp_path / "params" / BRANCH_DEFAULTS_MIGRATION_MARKER + branch_defaults_marker = marker_path(tmp_path, BRANCH_DEFAULTS_MIGRATION_MARKER) params.put_bool("ConditionalExperimental", False) params.put_bool("CEStoppedLead", True) @@ -152,22 +151,40 @@ def test_apply_launch_param_migrations_does_not_reapply_branch_defaults_after_ma assert params.get_float("RelaxedFollow") == 2.0 +def test_apply_launch_param_migrations_preserves_custom_branch_defaults_without_marker(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + + params.put_bool("CEStoppedLead", True) + params.put_bool("ForceStops", True) + params.put_float("AggressiveFollowHigh", 2.0) + params.put_float("StandardJerkAcceleration", 25.0) + params.put_float("RelaxedFollow", 2.0) + + apply_launch_param_migrations(params) + + assert not params.get_bool("CEStoppedLead") + assert params.get_bool("ForceStops") + assert params.get_float("AggressiveFollowHigh") == 2.0 + assert params.get_float("StandardJerkAcceleration") == 25.0 + assert params.get_float("RelaxedFollow") == 2.0 + + def test_apply_launch_param_migrations_updates_acceleration_profile_for_existing_installs(tmp_path): params = FileBackedFakeParams(tmp_path / "params") params.put_int("AccelerationProfile", 2) - (tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER).touch() - (tmp_path / "params" / BRANCH_DEFAULTS_MIGRATION_MARKER).touch() + marker_path(tmp_path, LAUNCH_PARAM_MIGRATION_MARKER).touch() + marker_path(tmp_path, BRANCH_DEFAULTS_MIGRATION_MARKER).touch() apply_launch_param_migrations(params) assert params.get_int("AccelerationProfile") == STANDARD_ACCELERATION_PROFILE - assert (tmp_path / "params" / ACCELERATION_PROFILE_MIGRATION_MARKER).is_file() + assert marker_path(tmp_path, ACCELERATION_PROFILE_MIGRATION_MARKER).is_file() def test_apply_launch_param_migrations_does_not_reapply_acceleration_profile_after_marker(tmp_path): params = FileBackedFakeParams(tmp_path / "params") - acceleration_profile_marker = tmp_path / "params" / ACCELERATION_PROFILE_MIGRATION_MARKER + acceleration_profile_marker = marker_path(tmp_path, ACCELERATION_PROFILE_MIGRATION_MARKER) params.put_int("AccelerationProfile", 3) acceleration_profile_marker.touch() @@ -175,3 +192,13 @@ def test_apply_launch_param_migrations_does_not_reapply_acceleration_profile_aft apply_launch_param_migrations(params) assert params.get_int("AccelerationProfile") == 3 + + +def test_apply_launch_param_migrations_preserves_custom_acceleration_profile_without_marker(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + + params.put_int("AccelerationProfile", 1) + + apply_launch_param_migrations(params) + + assert params.get_int("AccelerationProfile") == 1