From 50f3e3173da01ea50686ebb41f9c3e5ef6b3f99a Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 4 Jan 2026 13:03:03 -0700 Subject: [PATCH] misc. tune no lead dempaning ref Update acceleration and braking profiles slight adjustments to decel exponential and mv avg tune improve accel limits and lead dampening bpvs Refactor follow distance profiles and smoothing logic Updated follow distance profiles and added smoothing factor for dynamic follow behavior. Refactor acceleration profiles and smoothing logic Updated acceleration and braking profiles for different personalities, added smoothing and rate limiting for acceleration adjustments. reft tun adjust accel and braking profiles Update accel_controller.py slight adjustment try lower and no expno Revise follow distance profiles and smoothing parameters Updated follow distance profiles and breakpoints for different personalities. Adjusted smoothing parameters for better performance. Refactor acceleration and braking profiles lower ki and no smoothing to last min call try Update dynamic_follow.py new new fff Update accel_controller.py try what if just try df exponential smoothing exponential smoothing tt Update custom.capnp conflic ff ref rebase fix --- selfdrive/selfdrived/selfdrived.py | 8 +- .../lib/accel_personality/accel_controller.py | 117 +++++++++++------- .../lib/dynamic_personality/dynamic_follow.py | 96 ++++++++++++-- .../controls/lib/longitudinal_planner.py | 4 +- 4 files changed, 168 insertions(+), 57 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3a03553ff3..ae42cb7f5b 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -233,8 +233,8 @@ class SelfdriveD(CruiseHelper): # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ - (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ + (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): self.events.add(EventName.pedalPressed) # Create events for temperature, disk space, and memory @@ -310,7 +310,7 @@ class SelfdriveD(CruiseHelper): else: self.events.add(EventName.preLaneChangeRight) elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, - LaneChangeState.laneChangeFinishing): + LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) # Handle lane turn @@ -505,7 +505,7 @@ class SelfdriveD(CruiseHelper): # All pandas not in silent mode must have controlsAllowed when openpilot is enabled if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] - if ps.safetyModel not in IGNORED_SAFETY_MODES): + if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 return CS diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index f35f687ef2..357a17c1be 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -9,81 +9,112 @@ from cereal import custom import numpy as np from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality # Acceleration Profiles MAX_ACCEL_PROFILES = { - AccelPersonality.eco: [2.0, 1.99, 1.88, 1.10, 0.500, 0.292, 0.15, 0.10], - AccelPersonality.normal: [1.0, 2.00, 1.94, 1.22, 0.635, 0.33, 0.22, 0.16], - AccelPersonality.sport: [.5, 2.00, 2.00, 1.85, 0.800, 0.54, 0.32, 0.22], + AccelPersonality.eco: [1.30, 1.25, 1.15, 0.83, 0.65, 0.51, 0.30, 0.12, 0.08, 0.06], + AccelPersonality.normal: [1.80, 1.76, 1.48, 0.88, 0.73, 0.58, 0.40, 0.15, 0.09, 0.07], + AccelPersonality.sport: [2.00, 1.95, 1.80, 0.93, 0.81, 0.69, 0.50, 0.21, 0.10, 0.08], } -MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.] +MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 12., 18., 24., 32., 42., 55.] + # Braking Profiles MIN_ACCEL_PROFILES = { - AccelPersonality.eco: [-0.14, -0.0006, -0.010, -0.30, -1.20], - AccelPersonality.normal: [-0.1, -0.0007, -0.012, -0.35, -1.20], - AccelPersonality.sport: [-0.6, -0.0008, -0.014, -0.40, -1.20], + AccelPersonality.eco: [-.68, -1.20], + AccelPersonality.normal: [-.74, -1.30], + AccelPersonality.sport: [-.80, -1.40], } -MIN_ACCEL_BREAKPOINTS = [0., 3., 11., 14., 50.] +MIN_ACCEL_BREAKPOINTS = [7.5, 18.] +DECEL_SMOOTH_ALPHA = 0.55 # Very aggressive smoothing for decel (lower = smoother) +ACCEL_SMOOTH_ALPHA = 0.65 # Less aggressive for accel (higher = more responsive) + +# Asymmetric rate limiting +MAX_DECEL_INCREASE_RATE = 1.5 # When braking harder (m/s² per second) +MAX_DECEL_DECREASE_RATE = 0.5 # When releasing brake (m/s² per second) + class AccelPersonalityController: - def __init__(self): self.params = Params() self.frame = 0 self.accel_personality = AccelPersonality.normal - self.param_keys = { - 'personality': 'AccelPersonality', - 'enabled': 'AccelPersonalityEnabled' - } + self.last_max_accel = 2.0 + self.last_min_accel = -0.01 + self.first_run = True + self.param_keys = {'personality': 'AccelPersonality', 'enabled': 'AccelPersonalityEnabled'} self._load_personality_from_params() def _load_personality_from_params(self): try: saved = self.params.get(self.param_keys['personality']) if saved is not None: - personality_value = int(saved) - if personality_value in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: - self.accel_personality = personality_value - else: - cloudlog.warning(f"Invalid personality value {personality_value}, using normal") - self.accel_personality = AccelPersonality.normal - except (ValueError, TypeError) as e: - cloudlog.warning(f"Failed to load personality from params: {e}") - self.accel_personality = AccelPersonality.normal + val = int(saved) + if val in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: + self.accel_personality = val + except (ValueError, TypeError): + pass def _update_from_params(self): - if self.frame % int(1. / DT_MDL) != 0: - return - self._load_personality_from_params() + if self.frame % int(1. / DT_MDL) == 0: + self._load_personality_from_params() + + def update(self, sm=None): + self.frame += 1 + self._update_from_params() def get_accel_personality(self) -> int: self._update_from_params() return int(self.accel_personality) def set_accel_personality(self, personality: int): - if personality not in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: - cloudlog.error(f"Invalid personality {personality}, ignoring") - return - - self.accel_personality = personality - self.params.put(self.param_keys['personality'], str(personality)) - cloudlog.info(f"Accel personality set to {personality}") + if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: + self.accel_personality = personality + self.params.put(self.param_keys['personality'], str(personality)) def cycle_accel_personality(self) -> int: - personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport] - current_idx = personalities.index(self.accel_personality) - next_personality = personalities[(current_idx + 1) % len(personalities)] + personality = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport] + next_personality = personality[(personality.index(self.accel_personality) + 1) % len(personality)] self.set_accel_personality(next_personality) return int(next_personality) def get_accel_limits(self, v_ego: float) -> tuple[float, float]: - max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality]) - min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality]) - return float(min_a), float(max_a) + v_ego = max(0.0, v_ego) + target_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality]) + target_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality]) + + if self.first_run: + self.last_max_accel, self.last_min_accel = target_max, target_min + self.first_run = False + return float(target_min), float(target_max) + + # Smoothing + self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel) + smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel) + + # Rate Limiting (Asymmetric) + raw_change = smoothed_decel - self.last_min_accel + + if raw_change < 0: + limit = MAX_DECEL_INCREASE_RATE * DT_MDL + decel_change = np.clip(raw_change, -limit, limit) + else: + limit = MAX_DECEL_DECREASE_RATE * DT_MDL + decel_change = np.clip(raw_change, -limit, limit) + + self.last_min_accel += decel_change + + # Dynamic Safety Corridor: Ensure min is always strictly less than max. + # We maintain a gap of at least 0.1, or 5% of the current max acceleration. + # This scaling gap prevents solver crashes at high acceleration values. + gap = max(0.1, abs(self.last_max_accel) * 0.05) + + if self.last_min_accel > self.last_max_accel - gap: + self.last_min_accel = self.last_max_accel - gap + + return float(self.last_min_accel), float(self.last_max_accel) def get_min_accel(self, v_ego: float) -> float: return self.get_accel_limits(v_ego)[0] @@ -96,7 +127,6 @@ class AccelPersonalityController: def set_enabled(self, enabled: bool): self.params.put_bool(self.param_keys['enabled'], enabled) - cloudlog.info(f"Accel personality controller {'enabled' if enabled else 'disabled'}") def toggle_enabled(self) -> bool: current = self.is_enabled() @@ -106,7 +136,6 @@ class AccelPersonalityController: def reset(self): self.accel_personality = AccelPersonality.normal self.frame = 0 - - def update(self): - self.frame += 1 - self._update_from_params() \ No newline at end of file + self.last_max_accel = 2.0 + self.last_min_accel = -0.01 + self.first_run = True diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py index 2b697cc8af..873f5aa21d 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py @@ -14,11 +14,17 @@ LongPersonality = log.LongitudinalPersonality # Follow distance profiles mapped to LongPersonality FOLLOW_PROFILES = { - LongPersonality.relaxed: [1.55, 1.65, 1.65, 1.80], - LongPersonality.standard: [1.45, 1.45, 1.45, 1.55], - LongPersonality.aggressive: [1.20, 1.25, 1.28, 1.35], + LongPersonality.relaxed: [1.75, 1.75, 1.75, 1.75, 1.80, 1.80, 1.80], + LongPersonality.standard: [1.45, 1.45, 1.45, 1.45, 1.50, 1.50, 1.50], + LongPersonality.aggressive: [1.10, 1.10, 1.15, 1.15, 1.20, 1.20, 1.20], } -FOLLOW_BREAKPOINTS = [0., 6., 18., 36.] + +FOLLOW_BREAKPOINTS = [0., 10., 20., 30., 40., 50., 60.] + +SMOOTHING_BASE = 0.55 # Base smoothing factor (higher = smoother) +SMOOTHING_RANGE = 0.20 # Additional smoothing at high speeds +SMOOTHING_SPEED_THRESHOLD = 36.0 # m/s (~80 mph) for max smoothing +PERSONALITY_CHANGE_COOLDOWN_S = 2.0 class FollowDistanceController: @@ -26,23 +32,97 @@ class FollowDistanceController: self.params = Params() self.frame = 0 self.personality = LongPersonality.standard + self.current_multiplier = None + self.first_run = True + self.personality_change_cooldown = 0 + self.personality_cooldown_frames = int(PERSONALITY_CHANGE_COOLDOWN_S / DT_MDL) + self._load_personality() + + def _load_personality(self): + try: + saved = self.params.get('LongitudinalPersonality') + if saved is not None: + val = int(saved) + if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + self.personality = val + except (ValueError, TypeError): + pass def _update_from_params(self): if self.frame % int(1. / DT_MDL) != 0: return - self.personality = int(self.params.get('LongitudinalPersonality')) + + if self.personality_change_cooldown > 0: + self.personality_change_cooldown -= 1 + return + + try: + param = self.params.get('LongitudinalPersonality') + if param is not None: + val = int(param) + if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + if val != self.personality: + self.personality = val + self.personality_change_cooldown = self.personality_cooldown_frames + except (ValueError, TypeError): + pass + + def _get_smoothing_factor(self, v_ego: float) -> float: + speed_factor = np.clip(v_ego / SMOOTHING_SPEED_THRESHOLD, 0.3, 1.0) + return SMOOTHING_BASE + (SMOOTHING_RANGE * speed_factor) def is_enabled(self) -> bool: return self.params.get_bool('DynamicFollow') + def set_enabled(self, enabled: bool): + self.params.put_bool('DynamicFollow', enabled) + def toggle(self) -> bool: enabled = self.is_enabled() - self.params.put_bool('DynamicFollow', not enabled) + self.set_enabled(not enabled) return not enabled + def get_personality(self) -> int: + self._update_from_params() + return int(self.personality) + + def set_personality(self, personality: int): + if personality not in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + return + + self.personality = personality + self.params.put('LongitudinalPersonality', str(personality)) + self.personality_change_cooldown = self.personality_cooldown_frames + + def cycle_personality(self) -> int: + personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive] + current_idx = personalities.index(self.personality) + next_personality = personalities[(current_idx + 1) % len(personalities)] + self.set_personality(next_personality) + return int(next_personality) + def get_follow_distance_multiplier(self, v_ego: float) -> float: self._update_from_params() - return float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality])) + v_ego = max(0.0, v_ego) + target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality])) + + if self.first_run: + self.current_multiplier = target + self.first_run = False + return self.current_multiplier + + #exponential smoothing with speedadaptive factor + alpha = self._get_smoothing_factor(v_ego) + self.current_multiplier = alpha * self.current_multiplier + (1.0 - alpha) * target + return self.current_multiplier + + def reset(self): + self.personality = LongPersonality.standard + self.frame = 0 + self.current_multiplier = None + self.first_run = True + self.personality_change_cooldown = 0 def update(self): - self.frame += 1 \ No newline at end of file + self.frame += 1 + self._update_from_params() diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 08f7274186..cd71e437fa 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -83,7 +83,7 @@ class LongitudinalPlannerSP: self.events_sp.clear() self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) - self.accel_controller.update() + 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') @@ -102,6 +102,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