From 3f2b42b4e3527b9575f0a385079bfa8869b12581 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 2 Feb 2021 17:09:01 -0800 Subject: [PATCH] Deal with long lag compensation (#20004) * add hack for big lags * wrong bracket * new pathplanner values --- selfdrive/controls/lib/pathplanner.py | 8 ++++++++ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 1cf819147f..a1a119045c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -191,6 +191,14 @@ class PathPlanner(): next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) next_tire_angle_rate = self.mpc_solution.tire_angle_rate[0] + # TODO This gets around the fact that MPC can plan to turn and turn back in the + # time between now and delay, need better abstraction between planner and controls + plan_ahead_idx = sum(self.t_idxs < delay) + if next_tire_angle_rate > 0: + next_tire_angle = max(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle]) + else: + next_tire_angle = min(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle]) + # reset to current steer angle if not active or overriding if active: tire_angle_desired = next_tire_angle diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5ebe739bef..e71b8a5eb2 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f69a7c09518ad0ee7a7ca88cd4a8974145f24a6d \ No newline at end of file +537244985e387f5d290d0deaaf900dc8231d13f8 \ No newline at end of file