mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-22 23:12:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added update_with_xva method to long_mpc so it can be used in sim (#22284)

Browse Source 
old-commit-hash: 485afda79d
This commit is contained in:
Mitchell Goff
2021-09-20 14:20:01 -07:00
committed by GitHub
parent b70c64d0d4
commit aa4b201b96
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+9 -2
View File
@@ -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])