From 0ef378b6bcca278b08daebf236301d5bba36cb37 Mon Sep 17 00:00:00 2001
From: Hikari1023 <93507091+Hikari1023@users.noreply.github.com>
Date: Mon, 27 Jun 2022 22:03:08 +0800
Subject: [PATCH] fix follow distance (#156)

follow distance sequence not properly matched
---
 .../controls/lib/longitudinal_mpc_lib/long_mpc.py    | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 6340fcb9b..8d0bc4bae 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -337,17 +337,17 @@ class LongitudinalMpc:
     if carstate.distanceLines == 1: # Traffic
       # At slow speeds more time, decrease time up to 60mph
       # in kph ~= 0    10    20     30     40     50     60     90    150
+      x_vel = [0, 2.25, 4.5, 6.75, 9, 11.25, 13.5, 15.75, 18, 20.25, 22.5, 24.75, 27, 29.25, 31.5, 33.75, 36, 38.25, 40.5]
+      y_dist = [1.25, 1.24, 1.23, 1.22, 1.21, 1.20, 1.18, 1.16, 1.13, 1.11, 1.09, 1.07, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05]
+      self.desired_TF = np.interp(carstate.vEgo, x_vel, y_dist)
+    elif carstate.distanceLines == 2: # Relaxed
       x_vel = [0,   2.788,  5.56,  8.333,  11.11, 13.89, 16.67, 25.0, 41.67]
       y_dist = [1.24, 1.24, 1.27,  1.29,   1.35,  1.35,   1.35,  1.1,  1.3]
       self.desired_TF = np.interp(carstate.vEgo, x_vel, y_dist)
-    elif carstate.distanceLines == 2: # Relaxed
-      x_vel = [0.0, 2.788,  5.56,  8.333,  11.11, 13.89, 19.44, 27.78, 41.67]  # velocities
-      y_dist = [1.27, 1.27, 1.29,   1.32,   1.38,  1.38,  1.38,  1.619, 1.8]
-      self.desired_TF = np.interp(carstate.vEgo, x_vel, y_dist)
       #self.desired_TF = 1.7
     else:
-      x_vel = [0, 2.25, 4.5, 6.75, 9, 11.25, 13.5, 15.75, 18, 20.25, 22.5, 24.75, 27, 29.25, 31.5, 33.75, 36, 38.25, 40.5]
-      y_dist = [1.25, 1.24, 1.23, 1.22, 1.21, 1.20, 1.18, 1.16, 1.13, 1.11, 1.09, 1.07, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05]
+      x_vel = [0.0, 2.788,  5.56,  8.333,  11.11, 13.89, 19.44, 27.78, 41.67]  # velocities
+      y_dist = [1.27, 1.27, 1.29,   1.32,   1.38,  1.38,  1.38,  1.619, 1.8]
       self.desired_TF = np.interp(carstate.vEgo, x_vel, y_dist)
       #self.desired_TF = T_FOLLOW