sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Improve Control Trajectory Interpolation Efficiency
old-commit-hash: 23fcd68707
097
Dean Lee 11 months ago committed by GitHub
parent edd2e43239
commit 4acd5f4293
1 changed files with 7 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      selfdrive/controls/lib/longcontrol.py

12
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -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.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.constants import ModelConstants
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState LongCtrlState = car.CarControl.Actuators.LongControlState
@ -68,19 +70,19 @@ class LongControl:
# Interp control trajectory # Interp control trajectory
speeds = long_plan.speeds speeds = long_plan.speeds
if len(speeds) == CONTROL_N: if len(speeds) == CONTROL_N:
v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds)
a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) 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 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 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) v_target = min(v_target_lower, v_target_upper)
a_target = min(a_target_lower, a_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: else:
v_target = 0.0 v_target = 0.0
v_target_now = 0.0 v_target_now = 0.0

Write Preview
Loading…
Cancel
Save