feat(long): smooth brake

common/params_keys.h

@@ -236,6 +236,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
     {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SmoothBraking", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // sunnypilot model params

selfdrive/controls/lib/longcontrol.py

@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.brake_smoother import BrakeOnsetSmoother
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -56,6 +57,7 @@ class LongControl:
                              (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
                              rate=1 / DT_CTRL)
     self.last_output_accel = 0.0
+    self.brake_smoother = BrakeOnsetSmoother()
 
   def reset(self):
     self.pid.reset()
@@ -87,6 +89,8 @@ class LongControl:
       error = a_target - CS.aEgo
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
+      # Ease the brake-onset command (comfort); bypassed for hard braking. See BrakeOnsetSmoother.
+      output_accel = self.brake_smoother.apply(output_accel, self.last_output_accel, a_target)
 
     self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -28,7 +28,7 @@ STOCK_RISE_RATE = 0.05  # m/s^2 per planner cycle (DT_MDL=0.05s -> 1.0 m/s^2/s);
 # Eco launch (v<=10) is kept ~stock so departing a stop/green light is not sluggish (no getting honked at);
 # the eco character is in the cruise/highway roll-on (>=25 m/s), not off-the-line.
 A_CRUISE_MAX_V = {
-  ECO:    [1.6, 1.10, 0.55, 0.40],
+  ECO:    [1.6, 1.10, 0.45, 0.30],
   NORMAL: STOCK_A_CRUISE_MAX_V,
   SPORT:  [1.8, 1.40, 1.00, 0.75],
 }

sunnypilot/selfdrive/controls/lib/brake_smoother.py

@@ -0,0 +1,40 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+
+_PARAM_REFRESH_FRAMES = max(1, int(1.0 / DT_CTRL))
+
+# Cap how fast the longitudinal command may DEEPEN a brake at onset, so braking eases in gradually
+# instead of biting sharply (the planner aTarget is already smooth; the PID proportional term reacting
+# to the aEgo lag at onset is what makes the command jerk). Comfort only:
+#  - applied solely while the brake command is getting more negative (onset / deepening),
+#  - bypassed entirely when aTarget demands a real hard brake, so emergencies are never delayed.
+BRAKE_ONSET_JERK = 2.5      # m/s^3, max rate the brake command may deepen at onset
+HARD_BRAKE_THRESH = -1.8    # m/s^2, aTarget at/below this = real hard brake -> pass through unmodified
+
+
+class BrakeOnsetSmoother:
+  def __init__(self, params=None):
+    self._params = params or Params()
+    self._enabled = self._params.get_bool("SmoothBraking")
+    self._frame = 0
+
+  def apply(self, output_accel: float, last_output_accel: float, a_target: float) -> float:
+    if self._frame % _PARAM_REFRESH_FRAMES == 0:
+      self._enabled = self._params.get_bool("SmoothBraking")
+    self._frame += 1
+
+    if not self._enabled:
+      return output_accel
+
+    # Only ease the onset of a comfort brake: command deepening AND the plan isn't asking for a hard brake.
+    if output_accel < last_output_accel and a_target > HARD_BRAKE_THRESH:
+      return max(output_accel, last_output_accel - BRAKE_ONSET_JERK * DT_CTRL)
+
+    return output_accel

sunnypilot/selfdrive/controls/lib/tests/test_brake_smoother.py

@@ -0,0 +1,43 @@
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.selfdrive.controls.lib.brake_smoother import BrakeOnsetSmoother, BRAKE_ONSET_JERK, HARD_BRAKE_THRESH
+
+MAX_STEP = BRAKE_ONSET_JERK * DT_CTRL
+
+
+class MockParams:
+  def __init__(self, enabled):
+    self._v = {"SmoothBraking": enabled}
+
+  def get_bool(self, k):
+    return bool(self._v.get(k, False))
+
+
+def test_disabled_passthrough():
+  s = BrakeOnsetSmoother(params=MockParams(False))
+  assert s.apply(-1.0, 0.0, -0.5) == -1.0
+
+
+def test_comfort_onset_rate_limited():
+  s = BrakeOnsetSmoother(params=MockParams(True))
+  # deepening comfort brake (a_target above the hard threshold) -> clamped to last - max_step
+  out = s.apply(-0.5, 0.0, -0.5)
+  assert out == -MAX_STEP
+
+
+def test_hard_brake_passthrough():
+  s = BrakeOnsetSmoother(params=MockParams(True))
+  # a_target at/below hard threshold -> no limiting, full brake passes through
+  assert s.apply(-1.0, 0.0, HARD_BRAKE_THRESH - 0.1) == -1.0
+
+
+def test_release_passthrough():
+  s = BrakeOnsetSmoother(params=MockParams(True))
+  # output less negative than last (releasing brake) -> not limited
+  assert s.apply(-0.2, -0.5, -0.3) == -0.2
+
+
+def test_already_within_rate():
+  s = BrakeOnsetSmoother(params=MockParams(True))
+  # small deepening within the cap is unchanged
+  out = s.apply(-MAX_STEP * 0.5, 0.0, -0.3)
+  assert out == -MAX_STEP * 0.5

sunnypilot/sunnylink/settings_ui.json

@@ -679,6 +679,19 @@
                 }
               ]
             },
+            {
+              "key": "SmoothBraking",
+              "widget": "toggle",
+              "title": "Smooth Brake Onset",
+              "description": "Eases the start of light braking so it builds in gradually instead of biting sharply. Hard and emergency braking are unaffected.",
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -68,6 +68,12 @@ sections:
     description: Holds radar leads through brief dropouts, ignores close-range radar ghosts, and eases braking when the radar lead target jumps, for smoother lead following. Emergency braking is unaffected.
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: SmoothBraking
+    widget: toggle
+    title: Smooth Brake Onset
+    description: Eases the start of light braking so it builds in gradually instead of biting sharply. Hard and emergency braking are unaffected.
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)