From 21793721cc6667250f4e234ed3ceb60d7dc028a1 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 21:29:42 -0500 Subject: [PATCH] rename --- .../controls/lib/longitudinal_planner.py | 11 ++++--- sunnypilot/selfdrive/controls/lib/dec/dec.py | 32 +++++++++++-------- .../helpers.py => longitudinal_planner.py} | 11 +++---- 3 files changed, 28 insertions(+), 26 deletions(-) rename sunnypilot/selfdrive/controls/lib/{dec/helpers.py => longitudinal_planner.py} (83%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 237cf0263f..c15a65bf49 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from openpilot.sunnypilot.selfdrive.controls.lib.dec.helpers import DecPlanner +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -69,11 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner(DecPlanner): +class LongitudinalPlanner(LongitudinalPlannerSP): def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - DecPlanner.__init__(self, self.CP, self.mpc) + LongitudinalPlannerSP.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -108,9 +108,9 @@ class LongitudinalPlanner(DecPlanner): return x, v, a, j, throttle_prob def update(self, sm): - DecPlanner.update(self, sm) + LongitudinalPlannerSP.update(self, sm) self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if dec_mpc_mode := self.get_dec_mpc_mode(): + if dec_mpc_mode := self.get_mpc_mode(): self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: @@ -212,4 +212,5 @@ class LongitudinalPlanner(DecPlanner): longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 1801f9946b..931d86f09d 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -25,6 +25,7 @@ import numpy as np from cereal import messaging +from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL @@ -109,7 +110,9 @@ class WeightedMovingAverageCalculator: class DynamicExperimentalController: - def __init__(self, params=None): + def __init__(self, CP: structs.CarParams, mpc, params=None): + self._CP = CP + self._mpc = mpc self._params = params or Params() self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") self._active: bool = False @@ -151,6 +154,10 @@ class DynamicExperimentalController: self._set_mode_timeout = 0 + def _read_params(self) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self._enabled = self._params.get_bool("DynamicExperimentalControl") + @staticmethod def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: """ @@ -362,8 +369,8 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active - def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: - self._mpc_fcw_crash_cnt = crash_cnt + def set_mpc_fcw_crash_cnt(self) -> None: + self._mpc_fcw_crash_cnt = self._mpc.crash_cnt def _set_mode(self, mode: str) -> None: if self._set_mode_timeout == 0: @@ -374,20 +381,17 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 - def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: - self._enabled = self._params.get_bool("DynamicExperimentalControl") - - def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: + def update(self, sm: messaging.SubMaster) -> None: self._read_params() - if self._enabled: - self._update_calculations(sm) + self.set_mpc_fcw_crash_cnt() - if radar_unavailable: - self._radarless_mode() - else: - self._radar_mode() + self._update_calculations(sm) + + if self._CP.radarUnavailable: + self._radarless_mode() + else: + self._radar_mode() self._active = sm['selfdriveState'].experimentalMode and self._enabled diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py similarity index 83% rename from sunnypilot/selfdrive/controls/lib/dec/helpers.py rename to sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 4eb7be0afa..56f32373d5 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -12,21 +12,18 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState -class DecPlanner: +class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): - self.CP = CP - self.mpc = mpc - self.dec = DynamicExperimentalController() + self.dec = DynamicExperimentalController(CP, mpc) - def get_dec_mpc_mode(self) -> str | None: + def get_mpc_mode(self) -> str | None: if not self.dec.active(): return None return self.dec.mode() def update(self, sm: messaging.SubMaster) -> None: - self.dec.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dec.update(self.CP.radarUnavailable, sm) + self.dec.update(sm) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP')