From a93c936e4b2e662dfb4287832ecd2b5953053e1a Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 6 Feb 2023 19:07:43 -0500
Subject: [PATCH] Log e2e x factor

---
 .../controls/lib/longitudinal_mpc_lib/long_mpc.py     | 11 +++++++++++
 selfdrive/controls/lib/longitudinal_planner.py        |  2 ++
 2 files changed, 13 insertions(+)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index c017951232..5cc7568c9d 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -203,6 +203,8 @@ class LongitudinalMpc:
     self.reset()
     self.source = SOURCES[2]
 
+    self.e2e_x = np.zeros(13, dtype=np.float64)
+
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.solver.reset()
@@ -319,6 +321,13 @@ class LongitudinalMpc:
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
+    cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
+    e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
+    e2e_x = np.cumsum(np.insert(e2e_xforward, 0, x[0]))
+
+    x_and_cruise = np.column_stack([e2e_x, cruise_target])
+    e2e_x = np.min(x_and_cruise, axis=1)
+
     self.params[:,0] = MIN_ACCEL
     self.params[:,1] = self.max_a
 
@@ -357,6 +366,8 @@ class LongitudinalMpc:
     else:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update')
 
+    self.e2e_x = e2e_x[:]
+
     self.yref[:,1] = x
     self.yref[:,2] = v
     self.yref[:,3] = a
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 0febfbafd9..9ec9a35d50 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -154,4 +154,6 @@ class LongitudinalPlanner:
 
     longitudinalPlan.solverExecutionTime = self.mpc.solve_time
 
+    longitudinalPlan.e2eX = self.mpc.e2e_x.tolist()
+
     pm.send('longitudinalPlan', plan_send)