mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-06 08:46:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

honda

Browse Source
This commit is contained in:
firestar5683
2026-05-03 21:05:23 -05:00
parent da5a028057
commit 261b139bcc
4 changed files with 119 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+8 -6
View File
@@ -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):
selfdrive/controls/tests/test_latcontrol.py
+1 -1
View File
@@ -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)
system/manager/launch_param_migrations.py
+68 -36
View File
@@ -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()
system/manager/test/test_launch_param_migrations.py
+42 -15
View File
@@ -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