test9 -the cure is worse than the disease

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)))
 

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)