diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 8b6db639a2..60408d9a12 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -15,20 +15,23 @@ AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality # Acceleration Profiles MAX_ACCEL_PROFILES = { - AccelPersonality.eco: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], # eco - AccelPersonality.normal: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], # normal - AccelPersonality.sport: [2.0, 2.00, 1.97, 1.00, .635, .48, .31, .165], # sport - #AccelPersonality.sport: [2.0, 2.00, 2.00, 1.90, 1.40, .70, .42, .251], # sport + AccelPersonality.eco: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], + AccelPersonality.normal: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], + AccelPersonality.sport: [2.0, 2.00, 1.97, 1.00, .635, .48, .31, .165], } MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.] # Braking Profiles MIN_ACCEL_PROFILES = { - AccelPersonality.eco: [-.0100, -.0100, -.060, -.60, -1.20], # gentler braking - AccelPersonality.normal: [-.0000015, -.0000015, -.060, -.60, -1.20], # normal braking - AccelPersonality.sport: [-.0000020, -.0000020, -.200, -.80, -1.20], # more aggressive braking + AccelPersonality.eco: [-0.0000002, -0.0000002, -0.2, -0.2, -1.20], + AccelPersonality.normal: [-0.0000002, -0.0000002, -0.3, -0.3, -1.20], + AccelPersonality.sport: [-0.0000003, -0.0000003, -0.4, -0.4, -1.20], } -MIN_ACCEL_BREAKPOINTS = [0., 5.0, 11., 14, 25.] +MIN_ACCEL_BREAKPOINTS = [0., 5., 8., 14., 25.] + +DECEL_SMOOTH_ALPHA = 0.08 # Very aggressive smoothing for decel (lower = smoother) +ACCEL_SMOOTH_ALPHA = 0.20 # Less aggressive for accel (higher = more responsive) +MAX_DECEL_RATE = 0.15 # Maximum change in decel per timestep (m/s²) class AccelPersonalityController: @@ -37,6 +40,9 @@ class AccelPersonalityController: self.params = Params() self.frame = 0 self.accel_personality = AccelPersonality.normal + self.last_max_accel = 2.0 + self.last_min_accel = -0.01 + self.first_run = True self.param_keys = { 'personality': 'AccelPersonality', 'enabled': 'AccelPersonalityEnabled' @@ -83,9 +89,32 @@ class AccelPersonalityController: 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_accel = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality]) + target_min_accel = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality]) + + if self.first_run: + self.last_max_accel = target_max_accel + self.last_min_accel = target_min_accel + self.first_run = False + return float(target_min_accel), float(target_max_accel) + + # exponential smoothing to max accel + self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max_accel + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel) + # VERY aggressive smoothing to min accel for ultra-smooth braking + # Also add rate limiting as a safety net + smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min_accel + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel) + # Rate limit decel changes and/or prevent sudden jerks + max_change_per_step = MAX_DECEL_RATE * DT_MDL + decel_change = smoothed_decel - self.last_min_accel + decel_change = np.clip(decel_change, -max_change_per_step, max_change_per_step) + + self.last_min_accel = self.last_min_accel + decel_change + + if self.last_min_accel > self.last_max_accel: + self.last_min_accel = self.last_max_accel - 0.1 + + 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] @@ -108,7 +137,10 @@ class AccelPersonalityController: def reset(self): self.accel_personality = AccelPersonality.normal self.frame = 0 + self.last_max_accel = 2.0 + self.last_min_accel = -0.01 + self.first_run = True def update(self): self.frame += 1 - self._update_from_params() + self._update_from_params() \ No newline at end of file