From 10bc36ae5803000f942a96b01ee92cecc4e61c67 Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Wed, 28 Sep 2022 13:27:07 -0700
Subject: [PATCH] Minor ACC fixes (#25911)

* Change cruise accel limits

* Long tuning script looks good

* Cap cruise slowdown aggression in e2e mode

* Revert atau change

* Cleanup

* Update ref

* fix ref
old-commit-hash: 1007df874f284f2c01a48c245f48c209ed806957
---
 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 5 +++--
 selfdrive/controls/lib/longitudinal_planner.py          | 4 ++--
 selfdrive/controls/lib/radar_helpers.py                 | 3 +--
 selfdrive/test/longitudinal_maneuvers/plant.py          | 5 +++--
 selfdrive/test/process_replay/ref_commit                | 2 +-
 5 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index ea9b0683a4..057cf5623f 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -320,7 +320,7 @@ class LongitudinalMpc:
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':
-      self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a
+      self.params[:,0] = MIN_ACCEL
       self.params[:,1] = self.cruise_max_a
       self.params[:,5] = LEAD_DANGER_FACTOR
 
@@ -341,11 +341,12 @@ class LongitudinalMpc:
     elif self.mode == 'blended':
       self.params[:,0] = MIN_ACCEL
       self.params[:,1] = MAX_ACCEL
+
       self.params[:,5] = 1.0
 
       x_obstacles = np.column_stack([lead_0_obstacle,
                                      lead_1_obstacle])
-      cruise_target = T_IDXS * v_cruise + x[0]
+      cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
       xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
       x = np.cumsum(np.insert(xforward, 0, x[0]))
 
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index b9b16e34fc..45a6db2e90 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -18,8 +18,8 @@ from system.swaglog import cloudlog
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
-A_CRUISE_MAX_BP = [0., 15., 25., 40.]
+A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
+A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py
index 0e2b96668f..bf788190cd 100644
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@@ -2,8 +2,7 @@ from common.numpy_fast import mean
 from common.kalman.simple_kalman import KF1D
 
 
-# the longer lead decels, the more likely it will keep decelerating
-# TODO is this a good default?
+# Default lead acceleration decay set to 50% at 1s
 _LEAD_ACCEL_TAU = 1.5
 
 # radar tracks
diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py
index 21af1cd3b1..e81510e9ba 100755
--- a/selfdrive/test/longitudinal_maneuvers/plant.py
+++ b/selfdrive/test/longitudinal_maneuvers/plant.py
@@ -8,7 +8,7 @@ from common.realtime import Ratekeeper, DT_MDL
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.modeld.constants import T_IDXS
 from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 
 class Plant():
   messaging_initialized = False
@@ -83,7 +83,8 @@ class Plant():
     lead.vLead = float(v_lead)
     lead.vLeadK = float(v_lead)
     lead.aLeadK = float(a_lead)
-    lead.aLeadTau = float(1.5)
+    # TODO use real radard logic for this
+    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
     lead.status = status
     lead.modelProb = float(prob)
     if not self.only_lead2:
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index c7daecd5a3..98fbb74ae4 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-a82580edc3a842dd58814bf2fe1a0f0f85d438f5
\ No newline at end of file
+9098c1cf6993598071c3e27448356eef86660d02