test10

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -13,6 +13,9 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import (
   CSC_FARFIELD_GAIN,
   CSC_LAT_ACCEL_MAX,
   CSC_MIN_SPEED,
+  MAX_CURVATURE,
+  PRIOR_CURVATURE_BP,
+  PRIOR_LAT_ACCEL_V,
   CSC_NUDGE,
   CSC_NUDGE_WEIGHT,
   CSC_OVERRIDE_WATCH_TIME,
@@ -244,7 +247,9 @@ def test_prior_gives_higher_lat_accel_for_sharper_curves():
   _, controller = make_controller()
 
   assert controller.learned_lat_accel(0.001) == pytest.approx(1.5, abs=0.05)
-  assert controller.learned_lat_accel(0.1) == pytest.approx(2.9, abs=0.05)
+  assert controller.learned_lat_accel(MAX_CURVATURE) > controller.learned_lat_accel(0.001)
+  assert controller.learned_lat_accel(MAX_CURVATURE) == pytest.approx(
+    float(np.interp(MAX_CURVATURE, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V)), abs=0.05)
   assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION)
 
 
@@ -334,13 +339,19 @@ def test_learned_curve_stays_monotonic_despite_low_outlier_bucket():
 
 def test_dense_bucket_is_not_overridden_by_sparse_neighbour():
   # real device data: a running maximum ratcheted the 80-sample bucket up to the 20-sample neighbour
-  _, controller = make_controller(curvature_data={
+  _, dense_low = make_controller(curvature_data={
     "0.003": {"average": 1.95, "count": 20},
     "0.005": {"average": 1.38, "count": 80},
   })
+  _, dense_high = make_controller(curvature_data={
+    "0.003": {"average": 1.95, "count": 80},
+    "0.005": {"average": 1.38, "count": 20},
+  })
 
-  assert controller.learned_lat_accel(0.005) < 1.82
-  assert controller.learned_lat_accel(0.005) >= controller.learned_lat_accel(0.003)
+  assert dense_low.learned_lat_accel(0.005) < 1.95      # not ratcheted to the sparse neighbour
+  assert dense_low.learned_lat_accel(0.005) >= dense_low.learned_lat_accel(0.003)
+  # whichever side is better sampled should pull the fit: swapping the counts must raise it
+  assert dense_high.learned_lat_accel(0.005) > dense_low.learned_lat_accel(0.005)
 
 
 def test_weighted_isotonic_pools_violators_by_weight():

starpilot/controls/lib/curve_speed_controller.py

@@ -55,10 +55,20 @@ CSC_FARFIELD_MIN_CURVATURE = 0.004   # ~R 250 m; at this strength range readings
 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
-STEP = 0.001
+# Buckets are spaced geometrically, because comfort is a speed and v = sqrt(a/k) -- a linear
+# curvature grid puts nearly all its resolution where CSC can never operate. On real drives
+# 100% of active frames sat in k 0.001-0.006, which a 0.001 linear step covered in five
+# buckets, the widest spanning 95->67 mph. Geometric spacing makes every bucket ~3 mph wide.
+# Changing this grid is safe: _normalize_curvature_data re-buckets stored keys on load.
+MIN_CURVATURE = 0.0005            # R 2000 m — gentler than this never constrains anything
+MAX_CURVATURE = 0.02              # R 50 m — already well below the CSC_MIN_SPEED floor
+# 24 keeps every bucket under ~7 mph wide while holding ~45% of the old per-bucket sample
+# density; finer grids resolve better but leave more buckets prior-dominated for longer.
+CURVATURE_BUCKETS = 24
+ROUNDING_PRECISION = 6
+CURVATURE_GRID = MIN_CURVATURE * np.power(MAX_CURVATURE / MIN_CURVATURE,
+                                          np.arange(CURVATURE_BUCKETS) / (CURVATURE_BUCKETS - 1))
+LOG_CURVATURE_GRID = np.log(CURVATURE_GRID)
 
 # Drivers accept more lateral acceleration in sharp slow corners than in highway sweepers.
 PRIOR_CURVATURE_BP = [0.001, 0.003, 0.01, 0.03, 0.1]
@@ -148,7 +158,8 @@ class CurveSpeedController:
     curvature_data = self.starpilot_planner.params.get("CurvatureData")
     self.curvature_data = self._normalize_curvature_data(curvature_data)
 
-    self.required_curvatures = [str(round(road_curvature, ROUNDING_PRECISION)) for road_curvature in np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)]
+    # built through the bucketer so the keys are byte-identical to what training writes
+    self.required_curvatures = [self._bucket_curvature(curvature) for curvature in CURVATURE_GRID]
 
     self.rebuild_lat_accel_curve()
     # publish on the first flush even if this drive never trains, or the readout
@@ -157,10 +168,10 @@ class CurveSpeedController:
 
   @staticmethod
   def _bucket_curvature(road_curvature):
-    clipped_curvature = float(np.clip(road_curvature, MIN_CURVATURE, MAX_CURVATURE))
-    bucket_index = round((clipped_curvature - MIN_CURVATURE) / STEP)
-    bucketed_curvature = MIN_CURVATURE + (bucket_index * STEP)
-    return str(round(bucketed_curvature, ROUNDING_PRECISION))
+    clipped_curvature = float(np.clip(abs(road_curvature), MIN_CURVATURE, MAX_CURVATURE))
+    # nearest in log space, so a bucket is a constant speed step rather than a constant radius one
+    bucket_index = int(np.argmin(np.abs(LOG_CURVATURE_GRID - np.log(clipped_curvature))))
+    return str(round(float(CURVATURE_GRID[bucket_index]), ROUNDING_PRECISION))
 
   @classmethod
   def _normalize_curvature_data(cls, curvature_data):