This commit is contained in:
Jason Wen
2025-01-12 16:56:31 -05:00
parent de305a81d5
commit 426d41a0a7
2 changed files with 11 additions and 9 deletions
@@ -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.dec_longitudinal_planner import DecLongitudinalPlanner
from openpilot.sunnypilot.selfdrive.controls.lib.dec.helpers import DecPlanner
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(DecLongitudinalPlanner):
class LongitudinalPlanner(DecPlanner):
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
self.mpc = LongitudinalMpc(dt=dt)
DecLongitudinalPlanner.__init__(self, self.CP, self.mpc)
DecPlanner.__init__(self, self.CP, self.mpc)
self.fcw = False
self.dt = dt
self.allow_throttle = True
@@ -108,7 +108,7 @@ class LongitudinalPlanner(DecLongitudinalPlanner):
return x, v, a, j, throttle_prob
def update(self, sm):
DecLongitudinalPlanner.update(self, sm)
DecPlanner.update(self, sm)
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
if dec_mpc_mode := self.get_mpc_mode(sm):
self.mpc.mode = dec_mpc_mode
@@ -11,16 +11,14 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen
MpcSource = custom.LongitudinalPlanSP.MpcSource
class DecLongitudinalPlanner:
class DecPlanner:
def __init__(self, CP, mpc):
self.CP = CP
self.mpc = mpc
self.dynamic_experimental_controller = DynamicExperimentalController()
self.is_enabled = False
def update(self, sm):
self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm)
self.dynamic_experimental_controller = DynamicExperimentalController()
def get_mpc_mode(self, sm):
if not self.is_enabled or not sm['selfdriveState'].experimentalMode:
@@ -28,6 +26,10 @@ class DecLongitudinalPlanner:
return self.dynamic_experimental_controller.get_mpc_mode()
def update(self, sm):
self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm)
def publish_longitudinal_plan_sp(self, sm, pm):
plan_sp_send = messaging.new_message('longitudinalPlanSP')