From 29c1f1d06bf045546f82ef2aa1d215b2c60174b3 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Thu, 31 Jul 2025 11:45:36 -0700 Subject: [PATCH] move to sunny long planner --- selfdrive/controls/lib/longitudinal_planner.py | 7 ++----- .../controls/lib/longitudinal_planner.py | 18 +++++++++++------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 7422387856..75773dfeb1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -55,7 +55,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) # TODO remove mpc modes when TR released - self._last_mode = None self.mpc.mode = 'acc' LongitudinalPlannerSP.__init__(self, self.CP, self.mpc) self.fcw = False @@ -102,10 +101,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.mode = dec_mpc_mode if not self.mlsim: self.mpc.mode = dec_mpc_mode - if self._last_mode != self.mode: - if self.mode == 'blended': - self.reset_blend_transition() - self._last_mode = self.mode + + self.handle_mode_transition(self.mode) if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index d3c68079b4..83fdd4d1bd 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -22,11 +22,6 @@ class LongitudinalPlannerSP: self.transition_init() self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None - def transition_init(self) -> None: - self._params = Params() - self._transition_counter = 0 - self._transition_steps = 40 - @property def mlsim(self) -> bool: # If we don't have a generation set, we assume it's default model. Which as of today are mlsim. @@ -38,8 +33,17 @@ class LongitudinalPlannerSP: return self.dec.mode() - def reset_blend_transition(self): + def transition_init(self) -> None: + self._params = Params() self._transition_counter = 0 + self._transition_steps = 40 + self._last_mode = 'acc' + + def handle_mode_transition(self, mode: str) -> None: + if self._last_mode != mode: + if mode == 'blended': + self._transition_counter = 0 + self._last_mode = mode def blend_accel_transition(self, mpc_accel: float, e2e_accel: float, v_ego: float) -> float: if self._params.get_bool("BlendAccToE2ETransition"): @@ -47,7 +51,7 @@ class LongitudinalPlannerSP: self._transition_counter += 1 progress = self._transition_counter / self._transition_steps if v_ego > 5.0 and e2e_accel < 0.0: - # use k2.0 and normalize at 0.5 + # use k2.0 and normalize midpoint at 0.5 sigmoid = 1.0 / (1.0 + math.exp(-2.0 * (abs(e2e_accel / ACCEL_MIN) - 0.5))) blend_factor = 1.0 - (1.0 - progress) * (1.0 - sigmoid) blended = mpc_accel + (e2e_accel - mpc_accel) * blend_factor