From 960eb70950c8ec2734c193be0b47bad047a5c9fd Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 16 Aug 2026 22:25:13 -0700 Subject: [PATCH] fast --- .../lib/accel_controller/accel_controller.py | 20 ++++--- .../lib/accel_controller/constants.py | 8 +-- .../tests/test_accel_controller.py | 13 ++++- .../test_accel_controller_closed_loop.py | 57 +++++++++---------- 4 files changed, 51 insertions(+), 47 deletions(-) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 7f6375aae2..f6eed5c0b0 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -7,7 +7,7 @@ from openpilot.cereal import custom, log from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( ACCEL_V, BRAKE_BUILD_JERK, BRAKE_ONSET_JERK, DECEL_V, DESIRED_STOP_DISTANCE, FOLLOW_HEADWAY, GAP_DEADBAND_METERS, - GAP_DEADBAND_SECONDS, LAUNCH_JERK, NEUTRAL_ACCEL, PACE_BUFFER_TIME, PACE_GAIN, PACE_MAX_CLOSING_SPEED, PACE_MAX_OPENING_SPEED, + GAP_DEADBAND_SECONDS, LAUNCH_ACCEL, NEUTRAL_ACCEL, PACE_BUFFER_TIME, PACE_GAIN, PACE_MAX_CLOSING_SPEED, PACE_MAX_OPENING_SPEED, PACE_STABILITY_MARGIN, RELEASE_JERK, ROUTINE_DECEL, SPEED_BP, SPEED_RESPONSE_TIME, STOP_MARGIN_BP, STOP_MARGIN_V, TERMINAL_MAX_DECEL, TERMINAL_PREVIEW_TIME, TERMINAL_TIME_CONSTANT, URGENT_BRAKE_JERK, AccelProfile, ) @@ -211,9 +211,13 @@ class AccelController: return AccelDecision() def _govern_accel(self, raw_target: float, max_accel: float, previous_plan_accel: float, *, launch: bool, terminal: bool) -> float: + if launch: + self._a_command = raw_target + return self._a_command + if self._a_command is None: initial = previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0 - self._a_command = 0.0 if launch else min(initial, max_accel) + self._a_command = min(initial, max_accel) previous = self._a_command if raw_target < previous: @@ -225,9 +229,7 @@ class AccelController: jerk = BRAKE_BUILD_JERK updated = max(raw_target, previous - jerk * self.dt) else: - if launch: - jerk = LAUNCH_JERK - elif terminal: + if terminal: jerk = min(RELEASE_JERK, 0.6) else: jerk = RELEASE_JERK @@ -303,10 +305,10 @@ class AccelController: all_leads_clear = all(self._lead_clear_for_launch(lead) for lead in observed_leads) departure_confirmed = self._departure_confirmed(primary, bool(standstill)) and (primary is not None or secondary is None) - launch = bool(standstill and departure_confirmed and all_leads_clear and v_cruise > 0.3) + launch = bool(standstill and departure_confirmed and all_leads_clear and v_cruise > 0.3 and stock_cruise_accel > 0.0) if launch: self._braking_latched = False - raw_target = min(max_accel, max(0.8, free_accel)) + raw_target = min(LAUNCH_ACCEL, v_cruise - projected_v_ego) selected_projected = primary_projected elif standstill: raw_target = 0.0 @@ -327,11 +329,11 @@ class AccelController: if abs(raw_target) < NEUTRAL_ACCEL and not launch and not terminal: raw_target = 0.0 raw_target = float(np.clip(raw_target, -(TERMINAL_MAX_DECEL if terminal else max_decel), max_accel)) - if not standstill or launch: + if not standstill: raw_target = min(raw_target, stock_cruise_accel) a_target = self._govern_accel(raw_target, max_accel, previous_plan_accel, launch=launch, terminal=terminal) - if not standstill or launch: + if not standstill: a_target = min(a_target, stock_cruise_accel) self._a_command = a_target if a_target <= -0.15 and primary_projected is not None: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py index 32a14e2718..0c421417b4 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -5,9 +5,9 @@ AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile SPEED_BP = (0.0, 3.0, 10.0, 20.0, 30.0, 40.0) ACCEL_V = { - AccelProfile.eco: (0.90, 0.85, 0.78, 0.65, 0.52, 0.40), - AccelProfile.normal: (1.10, 1.05, 0.95, 0.80, 0.65, 0.50), - AccelProfile.sport: (1.20, 1.15, 1.05, 0.90, 0.72, 0.56), + AccelProfile.eco: (2.00, 1.25, 0.90, 0.70, 0.55, 0.44), + AccelProfile.normal: (2.00, 1.40, 1.08, 0.84, 0.67, 0.52), + AccelProfile.sport: (2.00, 1.48, 1.20, 0.93, 0.73, 0.60), } DECEL_V = (1.0, 1.2, 2.3, 2.5, 2.5, 2.5) @@ -29,7 +29,7 @@ BRAKE_ONSET_JERK = 1.0 BRAKE_BUILD_JERK = 0.45 URGENT_BRAKE_JERK = 2.2 RELEASE_JERK = 0.8 -LAUNCH_JERK = 1.8 +LAUNCH_ACCEL = 2.0 DESIRED_STOP_DISTANCE = 6.0 TERMINAL_TIME_CONSTANT = 3.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 2396ee4043..0ba36d221f 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -9,7 +9,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelDecision from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( - ACCEL_V, BRAKE_ONSET_JERK, DECEL_V, RELEASE_JERK, SPEED_BP, AccelProfile, + ACCEL_V, BRAKE_ONSET_JERK, DECEL_V, LAUNCH_ACCEL, RELEASE_JERK, SPEED_BP, AccelProfile, ) @@ -124,6 +124,7 @@ class TestAccelControllerContract(OpenpilotTestCase): self.assertEqual(set(ACCEL_V), {AccelProfile.eco, AccelProfile.normal, AccelProfile.sport}) for values in ACCEL_V.values(): self.assertEqual(len(values), len(SPEED_BP)) + self.assertEqual(values[0], LAUNCH_ACCEL) self.assertTrue(all(after <= before for before, after in zip(values[:-1], values[1:], strict=True))) for speed, expected in zip(SPEED_BP, values, strict=True): self.assertEqual(np.interp(speed, SPEED_BP, values), expected) @@ -131,7 +132,7 @@ class TestAccelControllerContract(OpenpilotTestCase): midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0 expected = (values[index] + values[index + 1]) / 2.0 self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, values), expected) - for speed in SPEED_BP: + for speed in SPEED_BP[1:]: self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.eco]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal])) self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.sport])) @@ -300,7 +301,13 @@ class TestStopAndLaunch(OpenpilotTestCase): standstill=True, ) self.assertFalse(released.should_stop) - self.assertGreater(released.a_target, 0.0) + self.assertEqual(released.a_target, LAUNCH_ACCEL) + + limited = controller() + settle(limited, stopped, frames=8, v_ego=0.0, v_cruise=0.5, stock_cruise_accel=0.1, stock_mpc_accel=-0.5, + stock_should_stop=True, stock_mpc_lead=0, standstill=True) + released = update(limited, departing, v_ego=0.0, v_cruise=0.5, stock_cruise_accel=0.1, stock_mpc_accel=0.1, standstill=True) + self.assertEqual(released.a_target, 0.5) def test_radar_fault_requires_clear_road_confirmation_before_launch(self): instance = controller() diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 5bab90e94d..507a91176c 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -9,13 +9,14 @@ from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc, LongitudinalPlanSource -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import NEUTRAL_ACCEL +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_V, LAUNCH_ACCEL, NEUTRAL_ACCEL, AccelProfile from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP as Plant -def configure(plant, *, enabled=True): +def configure(plant, *, enabled=True, profile=AccelProfile.normal): planner: Any = plant.planner planner.accel_controller_enabled = enabled + planner.accel_controller_profile = profile planner.read_accel_controller_params = lambda: None dec: Any = plant.planner.dec dec._enabled = False @@ -239,38 +240,32 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): class TestAccelControllerClosedLoopAcceptance(OpenpilotTestCase): - def test_confirmed_lead_departure_releases_brake_in_the_same_planner_frame(self): - plant = Plant( - enabled=True, - lead_relevancy=True, - speed=0.0, - distance_lead=6.0, - actuator_model=PRIUS_TSS2_ROUTE_MODEL, - run_long_control=True, - ) - configure(plant) + def test_every_profile_launches_promptly_after_confirmed_departure(self): + for profile in ACCEL_V: + plant = Plant(enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + configure(plant, profile=profile) + with scripted_stock_candidates(plant, mpc_accel=-0.5, cruise_accel=1.6, source=LongitudinalPlanSource.lead0) as stock: + for _ in range(12): + held = plant.step(v_lead=0.0, v_cruise=8.0) + self.assertTrue(held["should_stop"]) + self.assertLess(held["actuator_command"], 0.0) - with scripted_stock_candidates(plant, mpc_accel=-0.5, cruise_accel=1.1, source=LongitudinalPlanSource.lead0) as stock: - for _ in range(12): - held = plant.step(v_lead=0.0, v_cruise=8.0) - self.assertTrue(held["should_stop"]) - self.assertLess(held["actuator_command"], 0.0) + stock["mpc"] = 1.6 + unconfirmed = plant.step(v_lead=1.0, v_cruise=8.0) + confirmed = plant.step(v_lead=1.0, v_cruise=8.0) - stock["mpc"] = 1.1 - unconfirmed = plant.step(v_lead=1.0, v_cruise=8.0) - self.assertTrue(unconfirmed["should_stop"]) + self.assertTrue(unconfirmed["should_stop"]) + self.assertFalse(confirmed["should_stop"]) + self.assertEqual(confirmed["a_target"], LAUNCH_ACCEL) + self.assertGreater(confirmed["actuator_command"], 0.0) + self.assertEqual(confirmed["long_control_state"], LongCtrlState.pid) - confirmed = plant.step(v_lead=1.0, v_cruise=8.0) - self.assertFalse(confirmed["should_stop"]) - self.assertGreater(confirmed["a_target"], 0.0) - self.assertGreater(confirmed["actuator_command"], 0.0) - self.assertEqual(confirmed["long_control_state"], LongCtrlState.pid) - - for _ in range(12): - moving = plant.step(v_lead=1.0, v_cruise=8.0) - if moving["speed"] > 0.0: - break - self.assertGreater(moving["speed"], 0.0) + moving = confirmed + for _ in range(12): + moving = plant.step(v_lead=1.0, v_cruise=8.0) + if moving["speed"] >= 0.01: + break + self.assertGreaterEqual(moving["speed"], 0.01, profile) def test_slower_lead_causes_routine_decel_while_ttc_is_still_long(self): initial_gap = 55.0