From 23fcd687071c7825a7d518aec898fcdd536b2855 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 5 Jun 2024 08:04:50 +0800 Subject: [PATCH] LongControl: storing T_IDXS[:CONTROL_N] in a variable for reuse (#32599) Improve Control Trajectory Interpolation Efficiency --- selfdrive/controls/lib/longcontrol.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 2dd3390bb0..d08ee05035 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,6 +5,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_dead from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] + LongCtrlState = car.CarControl.Actuators.LongControlState @@ -68,19 +70,19 @@ class LongControl: # Interp control trajectory speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) - a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) + v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds) + a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels) - v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, CONTROL_N_T_IDX, speeds) a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now - v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, CONTROL_N_T_IDX, speeds) a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) - v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds) else: v_target = 0.0 v_target_now = 0.0