From 2d47bdf297da32d7daad9a37b37cbf57a5cfeb31 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 29 May 2025 21:17:41 -0400
Subject: [PATCH] implement in long plan

---
 cereal/custom.capnp                           | 12 ++++++++++
 .../controls/lib/longitudinal_planner.py      |  3 +++
 .../controls/lib/longitudinal_planner.py      | 24 +++++++++++++++++++
 3 files changed, 39 insertions(+)

diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index 243702a140..e52961da5e 100644
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -102,6 +102,9 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
 
+  events @1 :List(OnroadEventSP.Event);
+  slc @2 :SpeedLimitControl;
+
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
     enabled @1 :Bool;
@@ -113,6 +116,15 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     }
   }
 
+  struct SpeedLimitControl {
+    state @0 :SpeedLimitControlState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+    speedLimit @3 :Float32;
+    speedLimitOffset @4 :Float32;
+    distToSpeedLimit @5 :Float32;
+  }
+
   enum SpeedLimitControlState {
     inactive @0; # No speed limit set or not enabled by parameter.
     tempInactive @1; # User wants to ignore speed limit until it changes.
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 4e842f8d1b..4bb3aecf18 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -144,6 +144,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
 
+    # Get new v_cruise from Speed Limit Control
+    v_cruise = LongitudinalPlannerSP.update_v_cruise(self, sm, not reset_state, self.v_desired_filter.x, self.a_desired, v_cruise)
+
     if force_slow_decel:
       v_cruise = 0.0
 
diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
index 56f32373d5..9b880df6d7 100644
--- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
@@ -7,7 +7,10 @@ See the LICENSE.md file in the root directory for more details.
 
 from cereal import messaging, custom
 from opendbc.car import structs
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 
@@ -15,6 +18,7 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.slc = SpeedLimitController(CP)
 
   def get_mpc_mode(self) -> str | None:
     if not self.dec.active():
@@ -22,6 +26,17 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def update_v_cruise(self, sm: messaging.SubMaster, long_enabled: bool, v_ego: float, a_ego: float, v_cruise: float) -> float:
+    events_sp = EventsSP()
+
+    self.slc.update(long_enabled, v_ego, a_ego, sm, v_cruise, events_sp)
+
+    v_cruise_slc = self.slc.speed_limit_offseted if self.slc.is_active else V_CRUISE_UNSET
+
+    v_cruise_final = min(v_cruise, v_cruise_slc)
+
+    return v_cruise_final
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
 
@@ -38,4 +53,13 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    # Speed Limit Control
+    slc = longitudinalPlanSP.slc
+    slc.state = self.slc.state
+    slc.enabled = self.slc._is_enabled
+    slc.active = self.slc.is_active
+    slc.speedLimit = float(self.slc.speed_limit)
+    slc.speedLimitOffset = float(self.slc.speed_limit_offset)
+    slc.distToSpeedLimit = float(self.slc.distance)
+
     pm.send('longitudinalPlanSP', plan_sp_send)