diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index cf1b0ab35..3445c5741 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -12,7 +12,9 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_EGO_HEADROOM, CSC_LAT_ACCEL_MAX, CSC_MIN_SPEED, + CSC_NUDGE, CSC_NUDGE_WEIGHT, + CSC_OVERRIDE_WATCH_TIME, CSC_TARGET_UP_RATE, CSC_TRAINING_SETTLE_TIME, CurveSpeedController, @@ -407,30 +409,69 @@ def test_sustained_ineligibility_still_drains_the_settle_timer(): assert not controller.enable_training +def settle_override(controller, sm=None, frames=None): + """Run the post-override watch out so the pseudo-sample is committed.""" + sm = sm if sm is not None else make_sm() + for _ in range(frames if frames is not None else int(CSC_OVERRIDE_WATCH_TIME / DT_MDL) + 1): + controller.handle_override(20.0, False, sm) + + def test_gas_override_nudges_bucket_up_once_per_episode(): _, controller = make_controller() prior = controller.learned_lat_accel(0.02) controller.target = 10.0 controller.handle_override(20.0, True, make_sm(gas=True)) + controller.handle_override(20.0, True, make_sm(gas=True)) + assert "0.02" not in controller.curvature_data # still watching what the driver holds + + settle_override(controller) assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT assert controller.curvature_data["0.02"]["average"] > prior - controller.handle_override(20.0, True, make_sm(gas=True)) - assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT - controller.handle_override(20.0, False, make_sm()) controller.target = 10.0 controller.handle_override(20.0, True, make_sm(gas=True)) + settle_override(controller) assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT +def test_override_learns_the_cornering_the_driver_actually_held(): + # the whole point: a fixed step needs several rejections to close a real disagreement, + # so record what they demonstrated instead + planner, observed = make_controller(driving_in_curve=True) + observed.target = 10.0 + observed.handle_override(20.0, True, make_sm(gas=True)) + planner.lateral_acceleration = 2.9 # they hold the curve much harder than CSC wanted + settle_override(observed, make_sm(gas=True)) + + _, stepped = make_controller(driving_in_curve=True) + stepped._apply_nudge(CSC_NUDGE) # what the old fixed-step path would have recorded + + assert observed.curvature_data["0.02"]["average"] == pytest.approx(2.9) + assert observed.curvature_data["0.02"]["average"] > stepped.curvature_data["0.02"]["average"] + assert observed.learned_lat_accel(0.02) > stepped.learned_lat_accel(0.02) + + +def test_override_on_a_straight_still_registers_the_fixed_step(): + planner, controller = make_controller() + prior = controller.learned_lat_accel(0.02) + controller.target = 10.0 + + controller.handle_override(20.0, True, make_sm(gas=True)) + planner.lateral_acceleration = 0.0 # never reached a corner + settle_override(controller) + + assert controller.curvature_data["0.02"]["average"] == pytest.approx(prior + CSC_NUDGE) + + def test_res_button_nudges_bucket_up_even_at_target_speed(): _, controller = make_controller() prior = controller.learned_lat_accel(0.02) controller.target = 20.0 # car tracking the target, so the gas-press condition would not fire controller.handle_override(20.0, True, make_sm(), accel_button=True) + settle_override(controller) assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT assert controller.curvature_data["0.02"]["average"] > prior diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 5e0fcc87b..ecd7452bc 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -46,6 +46,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False starpilot_following=SimpleNamespace(following_lead=False), tracking_lead=False, driving_in_curve=False, + lateral_acceleration=0.0, model_length=60.0, raw_model_stopped=raw_model_stopped, road_curvature=road_curvature, diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index e3867e40c..ed9a4ff1f 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -41,6 +41,7 @@ CSC_LAT_ACCEL_MIN = 1.2 CSC_LAT_ACCEL_MAX = 3.2 CSC_NUDGE = 0.15 CSC_NUDGE_WEIGHT = 20 # counts a single override pseudo-sample is worth +CSC_OVERRIDE_WATCH_TIME = 6.0 # s to keep watching what the driver holds after they reject a cut CSC_TRAINING_QUIET_TIME = 5.0 # blocks passive samples after CSC limited speed, so it can't learn its own cap CSC_TRAINING_SETTLE_TIME = 2.0 # driver-owned seconds before a sample counts, so it isn't openpilot's leftover speed # Learned values match the driver's own cornering, which alone would never slow them @@ -120,6 +121,10 @@ class CurveSpeedController: self.enable_training = False self.nudge_applied = False + self.override_watch_key = None + self.override_watch_peak = 0.0 + self.override_watch_timer = 0.0 + self.training_timer = 0.0 self.persistence_timer = 0.0 self.training_quiet_timer = 0.0 @@ -264,6 +269,8 @@ class CurveSpeedController: long_dropped = self._long_active_prev and not long_active self._long_active_prev = long_active + self._update_override_watch(sm) + if not was_controlling: self.nudge_applied = False return @@ -272,14 +279,46 @@ class CurveSpeedController: return if accel_button or (sm["carState"].gasPressed and self.target < v_ego - 0.5): - self._apply_nudge(CSC_NUDGE) + # Watch what the driver actually holds instead of stepping by a fixed amount. CSC is + # suspended while they override, so their cornering measures their comfort rather than + # this controller's own cap; a fixed step needs several rejections to close a real gap. + self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + self.override_watch_peak = abs(self.starpilot_planner.lateral_acceleration) + self.override_watch_timer = CSC_OVERRIDE_WATCH_TIME + self.nudge_applied = True elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve: self._apply_nudge(-CSC_NUDGE) + def _update_override_watch(self, sm): + if self.override_watch_key is None: + return + + lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) + if lateral_acceleration > self.override_watch_peak: + # credit the bucket the peak actually happened in, not the one at the button press + self.override_watch_peak = lateral_acceleration + self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + + self.override_watch_timer -= DT_MDL + if self.override_watch_timer > 0.0 and (is_user_overriding_longitudinal(sm) or + self.starpilot_planner.driving_in_curve): + return + + key = self.override_watch_key + self.override_watch_key = None + # floored at the old fixed step, so a rejection that never reaches a corner still counts + # and this path can only ever raise the bucket + self._record_pseudo_sample(key, max(self.override_watch_peak, + self.learned_lat_accel(float(key)) + CSC_NUDGE)) + def _apply_nudge(self, offset): key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) # relative to the learned value, not the margined one, or repeated overrides walk the bucket down - sample = float(np.clip(self.learned_lat_accel(float(key)) + offset, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX)) + self._record_pseudo_sample(key, self.learned_lat_accel(float(key)) + offset) + self.nudge_applied = True + + def _record_pseudo_sample(self, key, sample): + sample = float(np.clip(sample, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX)) data = self.curvature_data.get(key, {"average": sample, "count": 0}) effective_count = min(data["count"], CSC_COUNT_CAP) @@ -289,7 +328,6 @@ class CurveSpeedController: "count": data["count"] + CSC_NUDGE_WEIGHT, } - self.nudge_applied = True self.rebuild_lat_accel_curve() self.data_dirty = True self.flush_data()