Merge branch 'master' into blend-exp

Use a k=2 sigmoid normalized at 0.5

longitudinal_planner: simple transition

Allow a toggleable param in the cruise panel

selfdrive/controls/lib/longitudinal_planner.py

@@ -102,6 +102,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       if not self.mlsim:
         self.mpc.mode = dec_mpc_mode
 
+    self.handle_mode_transition(mode)
+
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
     else:
@@ -177,7 +179,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
     else:
-      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      output_a_target = self.blend_accel_transition(output_a_target_mpc, output_a_target_e2e, v_ego)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
 
     for idx in range(2):

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -4,9 +4,11 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+import math
 
 from cereal import messaging, custom
 from opendbc.car import structs
+from opendbc.car.interfaces import ACCEL_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
@@ -16,6 +18,7 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.transition_init()
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
 
   @property
@@ -29,6 +32,32 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def transition_init(self) -> None:
+    self._transition_counter = 0
+    self._transition_steps = 20
+    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.dec.enabled():
+      if self._transition_counter < self._transition_steps:
+        self._transition_counter += 1
+        progress = self._transition_counter / self._transition_steps
+        if v_ego > 5.0 and e2e_accel < 0.0:
+          if mpc_accel < 0.0 and e2e_accel > mpc_accel:
+            return mpc_accel
+          # use k3.0 and normalize midpoint at 0.5
+          sigmoid = 1.0 / (1.0 + math.exp(-3.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
+          return blended
+    return min(mpc_accel, e2e_accel)
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)