mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-20 05:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

fast

Browse Source
This commit is contained in:
rav4kumar
2026-08-16 22:25:13 -07:00
parent bdff333046
commit 960eb70950
4 changed files with 51 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py
+11 -9
View File
@@ -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:
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py
+4 -4
View File
@@ -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
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py
+10 -3
View File
@@ -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()
openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py
+26 -31
View File
@@ -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