From 0ce258612bb753bdd9067ac6871ac420df5f2f4e Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 20 Feb 2023 22:48:51 -0500
Subject: [PATCH 1/2] move-fast: v-tsc fix

---
 selfdrive/controls/lib/lateral_planner.py        |  4 ++++
 selfdrive/controls/lib/vision_turn_controller.py | 11 ++++++-----
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index ac76730702..717f06de52 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -40,6 +40,7 @@ class LateralPlanner:
     self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
+    self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
 
     self.lat_mpc = LateralMpc()
     self.reset_mpc(np.zeros(4))
@@ -134,4 +135,7 @@ class LateralPlanner:
     lateralPlan.laneChangeDirection = self.DH.lane_change_direction
     lateralPlan.laneChangePrev = self.DH.prev_lane_change
 
+    lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
+    lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
+
     pm.send('lateralPlan', plan_send)
diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py
index e267f98d5b..76dc7cb52c 100644
--- a/selfdrive/controls/lib/vision_turn_controller.py
+++ b/selfdrive/controls/lib/vision_turn_controller.py
@@ -6,7 +6,7 @@ from common.params import Params
 from common.realtime import sec_since_boot
 from common.conversions import Conversions as CV
 from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
+from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 
 
 _MIN_V = 5.6  # Do not operate under 20km/h
@@ -186,10 +186,11 @@ class VisionTurnController():
         c_y = width_pts / 2 + lll_y
         path_poly = np.polyfit(ll_x, c_y, 3)
 
-    # 2. If not polynomial derived from lanes, then derive it from driving path as provided by `lateralPlanner`.
-    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.psis) == CONTROL_N \
-       and lat_planner_data.dPathPoints[0] > 0:
-      path_poly = np.polyfit(lat_planner_data.psis, lat_planner_data.dPathPoints, 3)
+    # 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as
+    # provided by `lateralPlanner`.
+    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \
+       and lat_planner_data.dPathWLinesX[0] > 0:
+      path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3)
 
     # 3. If no polynomial derived from lanes or driving path, then provide a straight line poly.
     if path_poly is None:

From f7b0250652aa1dbf1c74f78382d1099e28fbed9d Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 21 Feb 2023 01:22:45 -0500
Subject: [PATCH 2/2] GAC: params init

---
 common/params.cc             | 2 ++
 selfdrive/manager/manager.py | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/common/params.cc b/common/params.cc
index a1b64c1832..7e26d5cce3 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -238,6 +238,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"EnforceTorqueLateral", PERSISTENT},
     {"EnhancedScc", PERSISTENT},
     {"GapAdjustCruise", PERSISTENT},
+    {"GapAdjustCruiseMax", PERSISTENT},
+    {"GapAdjustCruiseMin", PERSISTENT},
     {"GapAdjustCruiseMode", PERSISTENT},
     {"GapAdjustCruiseTr", PERSISTENT},
     {"GmapKey", PERSISTENT},
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index eda9eb748d..735d03287f 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -69,6 +69,8 @@ def manager_init() -> None:
     ("EndToEndLongToggle", "1"),
     ("EnhancedScc", "0"),
     ("GapAdjustCruise", "1"),
+    ("GapAdjustCruiseMax", "0"),
+    ("GapAdjustCruiseMin", "0"),
     ("GapAdjustCruiseMode", "0"),
     ("GapAdjustCruiseTr", "4"),
     ("GpxDeleteAfterUpload", "1"),