diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d6e138abbc..b16fbbeaab 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -65,15 +65,16 @@ class LongControl(): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Interp control trajectory # TODO estimate car specific lag, use .15s for now - if len(long_plan.speeds) == CONTROL_N: - v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds) - a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0] + speeds = long_plan.speeds + if len(speeds) == CONTROL_N: + v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], speeds) + a_target_lower = 2 * (v_target_lower - speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0] - v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds) - a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] + v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds) + a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] a_target = min(a_target_lower, a_target_upper) - v_target_future = long_plan.speeds[-1] + v_target_future = speeds[-1] else: v_target_future = 0.0 a_target = 0.0 @@ -96,7 +97,7 @@ class LongControl(): # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: - self.v_pid = long_plan.speeds[0] + self.v_pid = speeds[0] # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration