feat: AccelPersonalityController

cereal/custom.capnp

@@ -194,6 +194,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelPersonality @8 :AccelerationPersonality;
+
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+  }
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;

common/params_keys.h

@@ -135,6 +135,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -313,11 +313,14 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, a_cruise_min=None):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
+    if a_cruise_min is None:
+      a_cruise_min = CRUISE_MIN_ACCEL
+
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
@@ -329,7 +332,7 @@ class LongitudinalMpc:
 
     # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
     # when the leads are no factor.
-    v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
+    v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
     # TODO does this make sense when max_a is negative?
     v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)

selfdrive/controls/lib/longitudinal_planner.py

@@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+    accel_clip = self.get_accel_clip(v_ego) or [ACCEL_MIN, get_max_accel(v_ego)]
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
     accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
 
@@ -138,7 +138,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
+                    a_cruise_min=self.get_cruise_min_accel(v_ego))
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

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

@@ -0,0 +1,156 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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 cereal import custom
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+
+A_MAX_BP = [0.0, 4.0, 8.0, 16.0, 40.0]
+A_MAX_V = {
+  AccelPersonality.eco:    [1.20, 1.40, 1.20, 0.40, 0.08],
+  AccelPersonality.normal: [1.80, 1.80, 1.35, 0.50, 0.15],
+  AccelPersonality.sport:  [2.20, 2.20, 1.60, 0.70, 0.25],
+}
+
+COAST_DRAG_BP = [0.0, 10.0, 25.0, 40.0]
+COAST_DRAG_V = {
+  AccelPersonality.eco:    [-0.03, -0.05, -0.08, -0.12],
+  AccelPersonality.normal: [-0.04, -0.07, -0.12, -0.18],
+  AccelPersonality.sport:  [-0.06, -0.10, -0.18, -0.28],
+}
+
+A_MIN_FLOOR_BP = [0.0, 5.0, 15.0, 40.0]
+A_MIN_FLOOR_V = {
+  AccelPersonality.eco:    [-0.20, -0.35, -0.55, -0.50],
+  AccelPersonality.normal: [-0.25, -0.45, -0.75, -0.65],
+  AccelPersonality.sport:  [-0.35, -0.65, -1.00, -0.95],
+}
+
+DEFICIT_TO_FLOOR = 8.5
+COAST_DEADBAND = 1.0
+RAMP_OFF_RANGE = 5.0
+
+A_MIN_TIGHTEN_RATE = 0.6
+A_MIN_RELAX_RATE = 0.9
+A_MAX_RATE_UP = 1.5
+A_MAX_RATE_DOWN = 0.6
+
+MIN_MAX_GAP = 0.05
+
+PARAM_REFRESH_FRAMES = max(1, int(1.0 / DT_MDL))
+
+
+class AccelPersonalityController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self._first = True
+
+    val = self.params.get('AccelPersonality')
+    self._personality = val if val is not None else AccelPersonality.normal
+    self._enabled = self.params.get_bool('AccelPersonalityEnabled')
+
+    self._v_cruise = 0.0
+    self._a_min = -0.05
+    self._a_max = 1.50
+
+    self._cache_v: float | None = None
+    self._cache_v_cruise: float | None = None
+    self._cache_a_min = self._a_min
+    self._cache_a_max = self._a_max
+
+  def update(self, sm=None):
+    self.frame += 1
+    self._cache_v = None
+    self._cache_v_cruise = None
+
+    if sm is not None:
+      vc = sm['carState'].vCruise
+      self._v_cruise = float(vc) * (1000.0 / 3600.0) if vc != V_CRUISE_UNSET else 0.0
+
+    if self.frame % PARAM_REFRESH_FRAMES == 0:
+      val = self.params.get('AccelPersonality')
+      self._personality = val if val is not None else AccelPersonality.normal
+      new_enabled = self.params.get_bool('AccelPersonalityEnabled')
+      if new_enabled and not self._enabled:
+        self._first = True
+      self._enabled = new_enabled
+
+  def get_accel_personality(self) -> int:
+    return int(self._personality)
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
+    v_ego = max(0.0, v_ego)
+    if (self._cache_v is not None
+        and abs(self._cache_v - v_ego) < 0.01
+        and self._cache_v_cruise == self._v_cruise):
+      return self._cache_a_min, self._cache_a_max
+    self._cache_a_min, self._cache_a_max = self._step(v_ego)
+    self._cache_v = v_ego
+    self._cache_v_cruise = self._v_cruise
+    return self._cache_a_min, self._cache_a_max
+
+  def get_min_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[0]
+
+  def get_max_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[1]
+
+  def _ramp_off(self, v_ego: float) -> float:
+    if self._v_cruise <= 0.0:
+      return 1.0
+    return float(np.clip((self._v_cruise - v_ego) / RAMP_OFF_RANGE, 0.0, 1.0))
+
+  def _target_max(self, v_ego: float) -> float:
+    base = float(np.interp(v_ego, A_MAX_BP, A_MAX_V[self._personality]))
+    return base * self._ramp_off(v_ego)
+
+  def _target_min(self, v_ego: float) -> float:
+    coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
+    if self._v_cruise <= 0.0 or v_ego >= self._v_cruise:
+      return coast
+    floor = float(np.interp(v_ego, A_MIN_FLOOR_BP, A_MIN_FLOOR_V[self._personality]))
+    deficit = self._v_cruise - v_ego
+    t = float(np.clip(deficit / DEFICIT_TO_FLOOR, 0.0, 1.0)) ** 1.5
+    return coast + t * (floor - coast)
+
+  def _apply_coast_deadband(self, v_ego: float, t_min: float, t_max: float) -> tuple[float, float]:
+    if self._v_cruise <= 0.0 or abs(v_ego - self._v_cruise) >= COAST_DEADBAND:
+      return t_min, t_max
+    coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
+    return coast, max(0.05, t_max * 0.25)
+
+  def _rate_limit(self, last: float, target: float, rate_down: float, rate_up: float) -> float:
+    rate = rate_up if target > last else rate_down
+    step = rate * DT_MDL
+    return float(np.clip(target, last - step, last + step))
+
+  def _step(self, v_ego: float) -> tuple[float, float]:
+    t_max = self._target_max(v_ego)
+    t_min = self._target_min(v_ego)
+    t_min, t_max = self._apply_coast_deadband(v_ego, t_min, t_max)
+
+    if self._first:
+      self._a_min, self._a_max = t_min, t_max
+      self._first = False
+      return self._a_min, self._a_max
+
+    new_min = self._rate_limit(self._a_min, t_min, rate_down=A_MIN_TIGHTEN_RATE, rate_up=A_MIN_RELAX_RATE)
+    new_max = self._rate_limit(self._a_max, t_max, rate_down=A_MAX_RATE_DOWN, rate_up=A_MAX_RATE_UP)
+
+    new_min = min(new_min, new_max - MIN_MAX_GAP)
+
+    self._a_min, self._a_max = new_min, new_max
+    return self._a_min, self._a_max

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -17,6 +17,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
+from opendbc.car.interfaces import ACCEL_MIN
+
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
@@ -26,6 +29,7 @@ class LongitudinalPlannerSP:
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel_controller = AccelPersonalityController()
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -43,6 +47,17 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def get_accel_clip(self, v_ego: float) -> list[float] | None:
+    if not self.accel_controller.is_enabled():
+      return None
+    a_max = self.accel_controller.get_max_accel(v_ego)
+    return [ACCEL_MIN, max(ACCEL_MIN, a_max)]
+
+  def get_cruise_min_accel(self, v_ego: float) -> float | None:
+    if self.accel_controller.is_enabled():
+      return self.accel_controller.get_min_accel(v_ego)
+    return None
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -77,6 +92,7 @@ class LongitudinalPlannerSP:
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
+    self.accel_controller.update(sm)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
@@ -95,6 +111,8 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    longitudinalPlanSP.accelPersonality = int(self.accel_controller.get_accel_personality())
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

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

@@ -0,0 +1,147 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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.
+
+Coverage for AccelPersonalityController:
+- live param flip via auto-refresh (no Python set_enabled() call needed)
+- V_CRUISE_UNSET guard
+- enable-transition snap to fresh target
+- per-personality accel limit deltas vs stock get_max_accel
+"""
+import numpy as np
+
+from cereal import custom
+
+from openpilot.common.params import Params
+from opendbc.car.interfaces import ACCEL_MIN
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel as stock_get_max_accel
+
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  AccelPersonalityController,
+  PARAM_REFRESH_FRAMES,
+)
+
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+
+class FakeCarState:
+  def __init__(self, v_cruise=30.0):
+    self.vCruise = v_cruise
+
+
+class FakeSM:
+  def __init__(self, v_cruise=30.0):
+    self._data = {'carState': FakeCarState(v_cruise)}
+
+  def __getitem__(self, k):
+    return self._data[k]
+
+
+def _print_table(title, header, rows):
+  print(f"\n--- {title} ---")
+  print(" | ".join(f"{h:>12}" for h in header))
+  print("-" * (15 * len(header)))
+  for row in rows:
+    print(" | ".join(f"{v:>12.3f}" if isinstance(v, float) else f"{v:>12}" for v in row))
+
+
+class TestAccelLiveFlip:
+  def test_enable_via_param(self):
+    Params().put_bool('AccelPersonalityEnabled', False)
+    c = AccelPersonalityController()
+    assert not c.is_enabled()
+    Params().put_bool('AccelPersonalityEnabled', True)
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM())
+    assert c.is_enabled()
+
+  def test_disable_via_param(self):
+    Params().put_bool('AccelPersonalityEnabled', True)
+    c = AccelPersonalityController()
+    assert c.is_enabled()
+    Params().put_bool('AccelPersonalityEnabled', False)
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM())
+    assert not c.is_enabled()
+
+  def test_personality_change_via_param(self):
+    Params().put('AccelPersonality', AccelPersonality.normal)
+    c = AccelPersonalityController()
+    assert c.get_accel_personality() == AccelPersonality.normal
+    Params().put('AccelPersonality', AccelPersonality.sport)
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM())
+    assert c.get_accel_personality() == AccelPersonality.sport
+
+  def test_refresh_boundary_below_threshold(self):
+    Params().put_bool('AccelPersonalityEnabled', False)
+    c = AccelPersonalityController()
+    Params().put_bool('AccelPersonalityEnabled', True)
+    for _ in range(PARAM_REFRESH_FRAMES - 1):
+      c.update(FakeSM())
+    assert not c.is_enabled()
+
+  def test_enable_transition_snaps_to_target(self):
+    Params().put_bool('AccelPersonalityEnabled', True)
+    Params().put('AccelPersonality', AccelPersonality.sport)
+    c = AccelPersonalityController()
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM(v_cruise=35.0))
+    c.get_accel_limits(25.0)
+
+    Params().put_bool('AccelPersonalityEnabled', False)
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM(v_cruise=35.0))
+    assert not c.is_enabled()
+
+    Params().put('AccelPersonality', AccelPersonality.eco)
+    Params().put_bool('AccelPersonalityEnabled', True)
+    for _ in range(PARAM_REFRESH_FRAMES + 1):
+      c.update(FakeSM(v_cruise=35.0))
+    assert c._first
+
+  def test_vcruise_unset_treated_as_zero(self):
+    Params().put_bool('AccelPersonalityEnabled', True)
+    c = AccelPersonalityController()
+    c.update(FakeSM(v_cruise=V_CRUISE_UNSET))
+    assert c._v_cruise == 0.0
+
+
+class TestAccelUsageDiff:
+  def test_accel_clip_per_personality(self, capsys):
+    rows = []
+    speeds = [3.0, 10.0, 20.0, 30.0]
+    personalities = [
+      ('eco', AccelPersonality.eco),
+      ('normal', AccelPersonality.normal),
+      ('sport', AccelPersonality.sport),
+    ]
+
+    Params().put_bool('AccelPersonalityEnabled', True)
+    sm = FakeSM(v_cruise=35.0)
+
+    any_delta = False
+    for label, p in personalities:
+      Params().put('AccelPersonality', p)
+      c = AccelPersonalityController()
+      c.update(sm)
+      for v_ego in speeds:
+        stock_hi = float(stock_get_max_accel(v_ego))
+        c_lo, c_hi = c.get_accel_limits(v_ego)
+        delta_hi = c_hi - stock_hi
+        delta_lo = c_lo - ACCEL_MIN
+        if abs(delta_hi) > 0.01 or abs(delta_lo) > 0.01:
+          any_delta = True
+        rows.append((label, v_ego, stock_hi, c_hi, delta_hi, c_lo, delta_lo))
+
+    with capsys.disabled():
+      _print_table(
+        "AccelPersonalityController: a_max stock vs controller",
+        ["personality", "v_ego", "stock_hi", "ctrl_hi", "delta_hi", "ctrl_lo", "delta_lo"],
+        rows,
+      )
+    assert any_delta

sunnypilot/sunnylink/params_metadata.json

@@ -1,4 +1,26 @@
 {
+  "AccelPersonality": {
+    "title": "Acceleration Personality",
+    "description": "Select the acceleration personality profile. Sport provides more aggressive acceleration, Eco provides gentler acceleration.",
+    "options": [
+      {
+        "value": 0,
+        "label": "Sport"
+      },
+      {
+        "value": 1,
+        "label": "Normal"
+      },
+      {
+        "value": 2,
+        "label": "Eco"
+      }
+    ]
+  },
+  "AccelPersonalityEnabled": {
+    "title": "Custom Acceleration Personality",
+    "description": "Enable custom acceleration and braking profiles that adjust max acceleration and min deceleration based on speed and selected personality."
+  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -620,6 +620,65 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Acceleration Personality",
+              "description": "Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "key": "AccelPersonality",
+              "widget": "multiple_button",
+              "title": "Acceleration Profile",
+              "description": "Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Sport"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Eco"
+                }
+              ],
+              "visibility": [
+                {
+                  "type": "param",
+                  "key": "AccelPersonalityEnabled",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                },
+                {
+                  "type": "param",
+                  "key": "AccelPersonalityEnabled",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,6 +43,34 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Acceleration Personality
+    description: Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.
+    options:
+    - value: 0
+      label: Sport
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Eco
+    visibility:
+    - type: param
+      key: AccelPersonalityEnabled
+      equals: true
+    enablement:
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: AccelPersonalityEnabled
+      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)