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