diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index 724cb5ef8..82c579ee6 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/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(): diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index 0fe36039f..6f31fa5fd 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/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):