diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8d4bc3cb82..e09b32a0a6 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 96248b7132..139cdc06e7 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py index e69de29bb2..ab5441aa71 100644 --- a/sunnypilot/__init__.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 6952a97f1f..ec020000a3 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py new file mode 100644 index 0000000000..0a04c243c1 --- /dev/null +++ b/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