From aa4b201b9600badc63b5fa44cfe9d6b786bb5145 Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Mon, 20 Sep 2021 14:20:01 -0700 Subject: [PATCH] Added update_with_xva method to long_mpc so it can be used in sim (#22284) old-commit-hash: 485afda79db7b65d00eb03553511c14f4b2f5301 --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 03f71bb32f..b4a551f06d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -156,8 +156,15 @@ class LongitudinalMpc(): def update(self, carstate, model, v_cruise): v_cruise_clipped = clip(v_cruise, self.x0[1] - 10., self.x0[1] + 10.0) - self.yref[:,0] = v_cruise_clipped * self.T_IDXS # position - self.yref[:,1] = v_cruise_clipped * np.ones(N+1) # speed + position = v_cruise_clipped * self.T_IDXS + speed = v_cruise_clipped * np.ones(N+1) + accel = np.zeros(N+1) + self.update_with_xva(position, speed, accel) + + def update_with_xva(self, position, speed, accel): + self.yref[:,0] = position + self.yref[:,1] = speed + self.yref[:,2] = accel self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) self.solver.cost_set(N, "yref", self.yref[N][:3])