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