From 2c9268a019ecfd91e39082c4126086b026fb0d39 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Fri, 25 Sep 2026 10:33:41 -0700 Subject: [PATCH] deacel --- .../controls/lib/longitudinal_planner.py | 21 ++++++++++------- .../lib/accel_controller/accel_controller.py | 23 +++++++++++++++---- .../controls/lib/longitudinal_planner.py | 17 ++++++++++++-- 3 files changed, 47 insertions(+), 14 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 3b313fc1c8..39a3f5944e 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -35,8 +35,12 @@ def get_max_accel(v_ego): def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py -def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle): - max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) +def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle, + max_accel_override=None): + if max_accel_override is not None: + max_accel = max_accel_override + else: + max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) if not e2e: a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) @@ -141,11 +145,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP): is_e2e = self.is_e2e(sm) + max_accel_override = self.get_max_accel_override(v_ego) + v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel) a_cruise_prev = self.a_cruise gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset, - self.CP, self.dt, accel_coast, self.allow_throttle) + self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override) ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset, - self.CP, self.dt, accel_coast, True) + self.CP, self.dt, accel_coast, True, max_accel_override) self.a_cruise = self.arbitrate_cruise_candidate( sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source, allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel, @@ -157,11 +163,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): if is_e2e: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) - output_a_target, self.mpc.source, self.output_should_stop = min(candidates, key=lambda candidate: candidate[0]) - output_a_target = self.accel_controller.limit_accel(output_a_target, v_ego) - + output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) + self.output_should_stop = any(should_stop for _, _, should_stop in candidates) self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX) - self.accel_controller_active = self.is_accel_controller_active(force_decel, self.output_a_target) + self.accel_controller_active = self.is_accel_controller_active(force_decel) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 6d5b7bd350..2f591177c5 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -19,6 +19,16 @@ MAX_ACCEL_PROFILES = { AccelProfile.normal: [1.90, 1.70, 1.42, 0.99, 0.80, 0.66, 0.52], AccelProfile.sport: [2.00, 2.00, 1.86, 1.30, 1.02, 0.86, 0.72], } +CRUISE_DECEL_RESPONSE_TIME = { # seconds + AccelProfile.eco: 4.0, + AccelProfile.normal: 3.5, + AccelProfile.sport: 3.0, +} +CRUISE_DECEL_ACCEL = { # m/s^2; comfort-first cruise deceleration target + AccelProfile.eco: -0.35, + AccelProfile.normal: -0.50, + AccelProfile.sport: -0.65, +} class AccelController: @@ -40,7 +50,12 @@ class AccelController: def get_max_accel(self, v_ego: float) -> float: return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile])) - def limit_accel(self, accel: float, v_ego: float) -> float: - if not self.is_enabled() or accel <= 0.0: - return accel - return min(accel, self.get_max_accel(v_ego)) + def get_cruise_target(self, v_ego: float, v_target: float) -> float: + if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego: + return v_target + + response_time = CRUISE_DECEL_RESPONSE_TIME[self._profile] + target_delta = v_target - v_ego + if target_delta < CRUISE_DECEL_ACCEL[self._profile] * response_time * 2.0: + return v_target + return float(v_ego + target_delta / response_time) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index b652b0e86c..99ecf4069a 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -56,8 +56,21 @@ class LongitudinalPlannerSP: return False - def is_accel_controller_active(self, force_decel: bool, accel_target: float) -> bool: - return bool(self.accel_controller.is_enabled() and not force_decel and accel_target >= 0.0) + def get_max_accel_override(self, v_ego: float) -> float | None: + if not self.accel_controller.is_enabled(): + return None + + return self.accel_controller.get_max_accel(v_ego) + + def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool) -> float: + if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise: + return v_target + + return self.accel_controller.get_cruise_target(v_ego, v_target) + + def is_accel_controller_active(self, force_decel: bool) -> bool: + return bool(self.accel_controller.is_enabled() and not force_decel and + self.mpc.source == MpcPlanSource.cruise) def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool: radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)