From 058fab6da2bc44e4d0ee06bb13b162137cf3fb95 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Sun, 23 Jun 2024 21:51:02 +0800 Subject: [PATCH] reset state (mpc weight) when mode changed --- selfdrive/controls/lib/longitudinal_planner.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index dc20b20aa..00d526139 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -116,12 +116,13 @@ class LongitudinalPlanner: if self._dynamic_endtoend_controller.is_enabled(): self._dynamic_endtoend_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.mpc.mode = self._dynamic_endtoend_controller.get_mpc_mode(self.CP.radarUnavailable, + self._dynamic_endtoend_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'] ) #, sm['navInstruction'].maneuverDistance) + self.mpc.mode = self._dynamic_endtoend_controller.get_mpc_mode() else: self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' @@ -135,6 +136,10 @@ class LongitudinalPlanner: # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled + # dp + if not reset_state: + reset_state = self._dynamic_endtoend_controller.has_changed() + # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill)