mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-30 18:43:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(stopping_controller): adjust end request parameters for improved stopping behavior

Browse Source
This commit is contained in:
rav4kumar
2026-10-13 13:27:53 -07:00
parent e2197e2089
commit 61e551bb42
2 changed files with 42 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/sunnypilot/selfdrive/controls/lib/stopping_controller.py
+9 -6
View File
@@ -67,10 +67,10 @@ class StoppingController:
BLEND_V = 10.0 / 3.6 # m/s
END_V_HI = 5.0 / 3.6 # m/s
END_HI = -0.65 # m/s^2
END_LO = -0.30 # m/s^2
END_LO = -0.15 # m/s^2
END_CURVE_V = [0.0, 1.0 / 3.6, 2.0 / 3.6, 3.0 / 3.6, 4.0 / 3.6, 5.0 / 3.6] # m/s
END_CURVE_A = [END_LO, (END_LO + -0.56) / 2, -0.56, -0.60, -0.65, END_HI] # m/s^2
END_CURVE_A = [END_LO, (END_LO + -0.45) / 2, -0.45, -0.58, -0.65, END_HI] # m/s^2
END_PLAN_MIN = -0.25 # m/s^2
END_RATE = 2.0 # m/s^3
END_CAP_RATE = 1.0 # m/s^3
@@ -78,7 +78,8 @@ class StoppingController:
END_CAP_V = [0.0, 2.0 / 3.6, 4.0 / 3.6, 6.0 / 3.6, 8.0 / 3.6, 10.0 / 3.6] # m/s
END_CAP_A = [END_LO, -0.45, -0.65, -0.95, -1.25, -1.55] # m/s^2
LEAD_END_FLOOR = END_LO # m/s^2
LEAD_END_FLOOR = -0.30 # m/s^2
LEAD_END_FLOOR_V = 2.0 / 3.6 # m/s
RELEASE_FAST = 4.0 # m/s^3
RELEASE_SLOW = 1.5 # m/s^3
@@ -137,12 +138,14 @@ class StoppingController:
# a lead sets the end of the stop (and the gap), so follow the plan's own ease down to the wheel stop. The line
# used to floor this and held a plan easing to -0.15 at the line's -0.65 (seg 13), firming the last few metres.
# LEAD_END_FLOOR keeps that from going the other way and dissolving the stop into a creep.
target = min(a_target, self.LEAD_END_FLOOR) if a_target < 0.0 else a_target
floor = float(np.interp(max(v_ego, 0.0), [0.0, self.LEAD_END_FLOOR_V], [self.END_LO, self.LEAD_END_FLOOR]))
target = min(a_target, floor) if a_target < 0.0 else a_target
else:
line = self._blend(v_ego)
target = min(self._end_cap(a_target, v_ego), line) # soften a firm free stop; the line still floors it
if v_ego < self.END_V_HI and target < line:
return float(min(line, prev_accel + self.END_CAP_RATE * DT_CTRL))
step = self.END_CAP_RATE * DT_CTRL
return float(np.clip(line, prev_accel - step, prev_accel + step))
step = self.END_RATE * DT_CTRL
return float(np.clip(target, prev_accel - step, prev_accel + step))
@@ -175,7 +178,7 @@ class StoppingController:
return False
self.cf_t += DT_CTRL
self.cf_vmax = max(self.cf_vmax, CS.vEgo)
if self.cf_t > self.CF_WINDOW or self.cf_vmax >= self.CF_V_MAX or a_target > 0.0 and self.cf_stopping:
if self.cf_t > self.CF_WINDOW or self.cf_vmax >= self.CF_V_MAX or (a_target > 0.0 and self.cf_stopping):
self.cf_t, self.cf_stopping = None, False
return False
if a_target <= self.CF_TRIGGER:
openpilot/sunnypilot/selfdrive/controls/lib/tests/test_stopping_controller.py
+33 -9
View File
@@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details.
import math
from types import SimpleNamespace
import numpy as np
from openpilot.common.realtime import DT_CTRL
from openpilot.common.test import OpenpilotTestCase
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
@@ -33,6 +35,12 @@ def blend(sc, v_kph):
return sc._blend(v_kph / 3.6)
def lead_floor(sc, v_kph):
"""the lead stop's terminal floor: LEAD_END_FLOOR above LEAD_END_FLOOR_V, tapering to END_LO at the wheel stop"""
return float(np.interp(v_kph / 3.6, [0.0, StoppingController.LEAD_END_FLOOR_V],
[StoppingController.END_LO, StoppingController.LEAD_END_FLOOR]))
class TestEndOfStop(OpenpilotTestCase):
"""the line is the free-stop (no lead) finish: it runs from the entry request through 5 km/h to the rest value,
and floors a plan that is already easing. A stop with a lead follows the plan instead (TestLeadStop)."""
@@ -48,11 +56,12 @@ class TestEndOfStop(OpenpilotTestCase):
# 5 km/h and below: the line eases from END_HI through its knots toward the rest value, then END_LO at the wheel stop
out = run(sc, PID, cs(5.0), -0.20, out, 0.4, has_lead=False)
assert abs(out - END_HI) < 1e-6
knots = StoppingController.END_CURVE_A
out = run(sc, PID, cs(2.5), -0.20, out, 0.4, has_lead=False)
assert abs(out - (-0.56 + -0.60) / 2) < 1e-6 # halfway between the 2 and 3 km/h knots of the line
assert abs(out - (knots[2] + knots[3]) / 2) < 1e-6 # halfway between the 2 and 3 km/h knots of the line
assert abs(blend(sc, 4.0) - END_HI) < 1e-6
assert abs(blend(sc, 3.0) - (-0.60)) < 1e-6 and abs(blend(sc, 1.0) - (-0.43)) < 1e-6
assert abs(blend(sc, 0.5) - ((END_LO + -0.43) / 2)) < 1e-6 # halfway between the 0 and 1 km/h knots
assert abs(blend(sc, 3.0) - knots[3]) < 1e-6 and abs(blend(sc, 1.0) - knots[1]) < 1e-6
assert abs(blend(sc, 0.5) - (knots[0] + knots[1]) / 2) < 1e-6 # halfway between the 0 and 1 km/h knots
out = run(sc, PID, cs(0.0), -0.20, out, 0.4, has_lead=False)
assert abs(out - END_LO) < 1e-6
@@ -150,14 +159,19 @@ class TestLeadStop(OpenpilotTestCase):
assert abs(out - StoppingController.LEAD_END_FLOOR) < 1e-6
assert out < -0.15 # firmer than the plan
assert out > sc._blend(4.0 / 3.6) # ...but never firmer than the line did
# a plan that eases right down to the wheel stop still holds the floor, not the plan
# a plan that eases right down to the wheel stop still holds the floor, not the plan. Below LEAD_END_FLOOR_V the
# floor itself tapers toward END_LO, so the request is the taper there, not the full LEAD_END_FLOOR
out = self._step(sc, 1.0, StoppingController.END_LO / 2, out)
assert abs(out - StoppingController.LEAD_END_FLOOR) < 1e-6
assert abs(out - lead_floor(sc, 1.0)) < 1e-6
assert lead_floor(sc, 1.0) > StoppingController.LEAD_END_FLOOR
def test_the_floor_is_never_firmer_than_the_line_it_replaced(self):
def test_the_floor_is_a_taper_that_never_firms_past_the_line(self):
sc = StoppingController(-2.0)
for v_kph in (0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 8.0):
assert StoppingController.LEAD_END_FLOOR >= sc._blend(v_kph / 3.6)
assert abs(lead_floor(sc, 0.0) - StoppingController.END_LO) < 1e-9 # rests on the line's rest value
assert abs(lead_floor(sc, 2.0) - StoppingController.LEAD_END_FLOOR) < 1e-9 # full floor by LEAD_END_FLOOR_V
for v_kph in (0.0, 0.5, 1.0, 1.5, 2.0, 3.0, 4.0, 5.0, 8.0):
assert lead_floor(sc, v_kph) >= sc._blend(v_kph / 3.6) # never firmer than the line the stop used to be held at
assert lead_floor(sc, 0.5) > lead_floor(sc, 2.0) # ...and it tapers as the wheels stop
def test_a_moving_lead_is_still_followed(self):
sc = StoppingController(-2.0)
@@ -259,7 +273,8 @@ class TestRerollHold(OpenpilotTestCase):
def test_steps_to_the_reroll_request_within_0_1s(self):
outs = self._reroll(StoppingController(-2.0))
assert outs[4] <= StoppingController.REROLL_ACCEL + 1e-9 # 0.05 s
# the step reaches REROLL_ACCEL within 0.1 s, then the creep rate keeps digging past it to arrest the roll
assert outs[9] <= StoppingController.REROLL_ACCEL + 1e-9
assert all(b - a >= -StoppingController.REROLL_JERK * DT_CTRL - 1e-9 for a, b in zip(outs, outs[1:], strict=False))
@@ -411,3 +426,12 @@ class TestFreeStopCap(OpenpilotTestCase):
out = self._step(sc, 3.0, -1.8, False, -0.9)
assert out >= StoppingController.END_CAP_A[3] - 1e-6 # never softer than the cap
assert out > -1.8
def test_coming_up_onto_the_line_is_rate_limited_both_ways(self):
# below END_V_HI a firm plan puts the cap firmer than the line, so the request comes up onto the line. It must ramp
# there, not jump: the cap rate bounds the request in both directions
sc = StoppingController(-2.0)
assert sc._end_cap(-1.5, 4.5 / 3.6) < sc._blend(4.5 / 3.6) # the branch below is actually taken at 4.5 km/h
out = self._step(sc, 4.5, -1.5, False, -0.40, secs=DT_CTRL)
assert abs(out - (-0.40 - StoppingController.END_CAP_RATE * DT_CTRL)) < 1e-6
assert out > sc._blend(4.5 / 3.6) # still short of the line after one tick, instead of jumping to it