From 1a9f5d29d40036407a0c1c45943485d01e35d4e1 Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Mon, 25 Apr 2022 20:15:04 -0700 Subject: [PATCH] longitudinal_mpc: Compute forward distance from velocity (#24328) * add vel integ as x in e2e long mpc * add comment --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 74608dc1fe..b99ee9e9ac 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -345,6 +345,11 @@ class LongitudinalMpc: self.crash_cnt = 0 def update_with_xva(self, x, v, a): + # v, and a are in local frame, but x is wrt the x[0] position + # In >90degree turns, x goes to 0 (and may even be -ve) + # So, we use integral(v) + x[0] to obtain the forward-distance + xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) + x = np.cumsum(np.insert(xforward, 0, x[0])) self.yref[:,1] = x self.yref[:,2] = v self.yref[:,3] = a