diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index 3445c5741..724cb5ef8 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -10,6 +10,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_COMFORT_MARGIN, CSC_COUNT_CAP, CSC_EGO_HEADROOM, + CSC_FARFIELD_GAIN, CSC_LAT_ACCEL_MAX, CSC_MIN_SPEED, CSC_NUDGE, @@ -147,6 +148,42 @@ def test_upward_jitter_in_the_envelope_is_rate_limited(): peak = controller.target +def test_firm_distant_curvature_is_corrected_for_the_model_under_read(): + # the model reads ~0.81x actual at range, so a firm distant bend binds later than it should + distance = 90.0 + _, plain = make_controller(curve_profile=single_apex_profile(0.0045, distance)) + _, probe = make_controller() + corrected = probe._correct_far_field(*single_apex_profile(0.0045, distance)) + + assert corrected.max() == pytest.approx(0.0045 * CSC_FARFIELD_GAIN) + assert converge(plain, 30.0, 30.0) < envelope_speed(plain, 0.0045, distance) + 1e-6 + + +def test_weak_or_near_readings_are_left_alone(): + _, probe = make_controller() + + # too weak to carry usable magnitude at range + weak = probe._correct_far_field(*single_apex_profile(0.002, 90.0)) + assert weak.max() == pytest.approx(0.002) + + # firm, but close enough that the model is already accurate + near = probe._correct_far_field(*single_apex_profile(0.0045, 10.0)) + assert near.max() == pytest.approx(0.0045) + + +def test_far_field_correction_brings_the_slowdown_forward(): + profile = single_apex_profile(0.0045, 120.0) + _, controller = make_controller(curve_profile=profile) + + corrected = converge(controller, 30.0, 30.0) + raw_curvatures, distances = profile + uncorrected = float(np.sqrt( + max(np.sqrt(controller.lat_accel_for_curvature(0.0045) / 0.0045), CSC_MIN_SPEED) ** 2 + + 2.0 * CSC_APPROACH_DECEL * 120.0)) + + assert corrected < uncorrected # binds sooner than the model's own reading would + + def test_fresh_activation_seeds_at_envelope_not_cruise(): _, controller = make_controller(curve_profile=(np.full(33, 0.05), np.linspace(0.0, 60.0, 33))) diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index ed9a4ff1f..0fe36039f 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -48,6 +48,13 @@ CSC_TRAINING_SETTLE_TIME = 2.0 # driver-owned seconds before a sample counts, # below their habit. Speed scales as the square root, so 0.85 is ~8% slower. CSC_COMFORT_MARGIN = CSC_DEFAULT_MARGIN_PERCENT / 100.0 +# The model under-reads curvature at range: measured 0.81x actual beyond ~75 m. That holds +# only where the reading is already firm -- weak distant readings carry no usable magnitude +# (0.40x median with a 14:1 spread), so scaling those would amplify noise, not signal. +CSC_FARFIELD_MIN_CURVATURE = 0.004 # ~R 250 m; at this strength range readings were 85%+ reliable +CSC_FARFIELD_MIN_DISTANCE = 30.0 # inside this the model is already accurate +CSC_FARFIELD_GAIN = 1.23 # 1 / 0.81 + MAX_CURVATURE = 0.1 MIN_CURVATURE = 0.001 ROUNDING_PRECISION = 5 @@ -379,6 +386,12 @@ class CurveSpeedController: return lat_accel + @staticmethod + def _correct_far_field(curvatures, distances): + """Undo the model's known under-read of distant curvature, where the reading is firm.""" + firm = (curvatures >= CSC_FARFIELD_MIN_CURVATURE) & (distances >= CSC_FARFIELD_MIN_DISTANCE) + return np.minimum(np.where(firm, curvatures * CSC_FARFIELD_GAIN, curvatures), MAX_CURVATURE) + def reset(self, v_cruise): self.target = float(v_cruise) self.release_timer = 0.0 @@ -395,6 +408,7 @@ class CurveSpeedController: raw_target = float(v_cruise) self.binding_distance = 0.0 else: + curvatures = self._correct_far_field(curvatures, distances) lat_accel = self.lat_accel_for_curvature(curvatures) point_speeds = np.sqrt(lat_accel / np.maximum(curvatures, 1e-4)) point_speeds = np.maximum(point_speeds, CSC_MIN_SPEED)