Longitudinal: Smart Cruise Control prerequisites (#1249)

* Controls: Vision Turn Speed Control

* fix

* Data type temp fix

* format

* more

* even more

* self contain targets

* state cleanup

* fix

* param updates

* no need

* use similar state machine

* raise exception if not found

* new state

* entirely internal

* use long active

* more

* rename and expose aTarget

* rename to SCC-V

* init tests

* slight tests

* expose toggle

* lint

* todo

* remove lat planner sub and mock sm data

* introduce aTarget

* rename

* rename

* update fill_model_msg.py to calculate PLAN_T_IDXS for lanelines and road edges

* sync upstream

* no SCC-V yet

* Revert "no SCC-V yet"

This reverts commit b67281bcac.

* wrap it with SCC main

* no SCC-V yet

* noqa now

* fix

* OP long for now, enable for ICBM once merged

* type hints

* let's get it straight from carcontrol instead

* not needed

* unused

* add source to track

* we can do this

---------

Co-authored-by: discountchubbs <alexgrant990@gmail.com>

cereal/custom.capnp

@@ -122,6 +122,8 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
+  longitudinalPlanSource @1 :LongitudinalPlanSource;
+  smartCruiseControl @2 :SmartCruiseControl;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -133,6 +135,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
+
+  struct SmartCruiseControl {
+  }
+
+  enum LongitudinalPlanSource {
+    cruise @0;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

selfdrive/controls/lib/longitudinal_planner.py

@@ -146,6 +146,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 and a_desired from Smart Cruise Control
+    v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+
     if force_slow_decel:
       v_cruise = 0.0
 

sunnypilot/__init__.py

@@ -0,0 +1,7 @@
+"""
+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.
+"""
+PARAMS_UPDATE_PERIOD = 3  # seconds

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,15 +8,19 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.scc = SmartCruiseControl()
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = Source.cruise
 
   @property
   def mlsim(self) -> bool:
@@ -29,6 +33,18 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    self.scc.update(sm, v_ego, a_ego, v_cruise)
+
+    targets = {
+      Source.cruise : (v_cruise, a_ego),
+    }
+
+    self.source = min(targets, key=lambda k: targets[k][0])
+    v_target, a_target = targets[self.source]
+
+    return v_target, a_target
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
 
@@ -38,6 +54,7 @@ class LongitudinalPlannerSP:
     plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+    longitudinalPlanSP.longitudinalPlanSource = self.source
 
     # Dynamic Experimental Control
     dec = longitudinalPlanSP.dec
@@ -45,4 +62,7 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    # Smart Cruise Control
+    smartCruiseControl = longitudinalPlanSP.smartCruiseControl  # noqa: F841
+
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -0,0 +1,13 @@
+"""
+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.
+"""
+from cereal import messaging
+
+
+class SmartCruiseControl:
+
+  def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float):
+    pass