Bugfix longAPI error calculation?

* change error calc from feedforward value to desired value
* from "CarrotMaster" on discord

selfdrive/controls/controlsd.py

@@ -641,7 +641,10 @@ def state_control(self, CS):
     if not self.joystick_mode:
       # accel PID loop
       pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
-      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
+      t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
+      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, t_since_plan, long_plan)
+      if len(long_plan.speeds):
+        actuators.speed = long_plan.speeds[-1]
 
       # Steering PID loop and lateral MPC
       if self.model_use_lateral_planner:

selfdrive/controls/lib/longcontrol.py

@@ -1,5 +1,5 @@
 from cereal import car
-from openpilot.common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.controls.lib.pid import PIDController
@@ -58,7 +58,7 @@ def __init__(self, CP):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, CS, a_target, should_stop, accel_limits):
+  def update(self, active, CS, a_target, should_stop, accel_limits, t_since_plan, long_plan):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
@@ -82,7 +82,11 @@ def update(self, active, CS, a_target, should_stop, accel_limits):
       self.reset()
 
     else:  # LongCtrlState.pid
-      error = a_target - CS.aEgo
+      # v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
+
+      # error = v_target_now - CS.vEgo  # better for PID control?
+      error = a_target_now - CS.aEgo   # error = a_target - CS.aEgo (problematic code)
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)