LongControl: storing T_IDXS[:CONTROL_N] in a variable for reuse (#32599)

Improve Control Trajectory Interpolation Efficiency
commaai · Jun 5, 2024 · 23fcd68 · 23fcd68
1 parent e494d98
commit 23fcd68
Showing 1 changed file with 7 additions and 5 deletions.
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
@@ -5,6 +5,8 @@
 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 @@ def update(self, active, CS, long_plan, accel_limits, t_since_plan):
     # 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