Smarter longitudinal lag compensations (#22094)

* smarter lag compensations * need to use speed from plan * update ref Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com> old-commit-hash: edae2218d7
4 years ago · a06ffc48c1
parent 48b7754ab5
commit a06ffc48c1
2 changed files with 2 additions and 2 deletions
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@ -81,7 +81,7 @@ class LongControl():
    if len(long_plan.speeds) == CONTROL_N:
      v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
      v_target_future = long_plan.speeds[-1]
-      a_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.accels)
+      a_target = 2 * (v_target - long_plan.speeds[0])/DEFAULT_LONG_LAG - long_plan.accels[0]
    else:
      v_target = 0.0
      v_target_future = 0.0
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@ -1 +1 @@
-3e8037d39f40c59a2b9e86539e0952645b2cb3ea
+9f181202bbd6c64330bf4903ec265a179201057c